ConstrainedPlanningSphere.py
1 #!/usr/bin/env python
2 
3 
36 
37 # Author: Mark Moll
38 
39 from __future__ import print_function
40 import argparse
41 import math
42 import numpy as np
43 from ConstrainedPlanningCommon import *
44 
45 
47 
48  def __init__(self):
49  super(SphereConstraint, self).__init__(3, 1)
50 
51  def function(self, x, out):
52  out[0] = np.linalg.norm(x) - 1
53 
54  def jacobian(self, x, out):
55  nrm = np.linalg.norm(x)
56  if np.isfinite(nrm) and nrm > 0:
57  out[0, :] = x / nrm
58  else:
59  out[0, :] = [1, 0, 0]
60 
61 
63 
64  def __init__(self, space):
65  super(SphereProjection, self).__init__(space)
66 
67  def getDimension(self):
68  return 2
69 
70  def defaultCellSizes(self):
71  self.cellSizes_ = list2vec([.1, .1])
72 
73  def project(self, state, projection):
74  projection[0] = math.atan2(state[1], state[0])
75  projection[1] = math.acos(state[2])
76 
77 
78 def obstacles(x):
79  if x[2] > -0.8 and x[2] < -0.6:
80  if x[1] > -0.05 and x[1] < 0.05:
81  return x[0] > 0
82  return False
83  elif x[2] > -0.1 and x[2] < 0.1:
84  if x[0] > -0.05 and x[0] < 0.05:
85  return x[1] < 0
86  return False
87  elif x[2] > 0.6 and x[2] < 0.8:
88  if x[1] > -0.05 and x[1] < 0.05:
89  return x[0] < 0
90  return False
91  return True
92 
93 
94 def spherePlanningOnce(cp, plannername, output):
95  cp.setPlanner(plannername, "sphere")
96 
97  # Solve the problem
98  stat = cp.solveOnce(output, "sphere")
99 
100  if output:
101  ou.OMPL_INFORM("Dumping problem information to `sphere_info.txt`.")
102  with open("sphere_info.txt", "w") as infofile:
103  print(cp.spaceType, file=infofile)
104 
105  cp.atlasStats()
106  if output:
107  cp.dumpGraph("sphere")
108  return stat
109 
110 
111 def spherePlanningBench(cp, planners):
112  cp.setupBenchmark(planners, "sphere")
113  cp.runBenchmark()
114 
115 
116 def spherePlanning(options):
117  # Create the ambient space state space for the problem.
118  rvss = ob.RealVectorStateSpace(3)
119  bounds = ob.RealVectorBounds(3)
120  bounds.setLow(-2)
121  bounds.setHigh(2)
122  rvss.setBounds(bounds)
123 
124  # Create our constraint.
125  constraint = SphereConstraint()
126 
127  cp = ConstrainedProblem(options.space, rvss, constraint, options)
128  cp.css.registerProjection("sphere", SphereProjection(cp.css))
129 
130  start = ob.State(cp.css)
131  goal = ob.State(cp.css)
132  start[0] = 0
133  start[1] = 0
134  start[2] = -1
135  goal[0] = 0
136  goal[1] = 0
137  goal[2] = 1
138  cp.setStartAndGoalStates(start, goal)
139  cp.ss.setStateValidityChecker(ob.StateValidityCheckerFn(obstacles))
140 
141  planners = options.planner.split(",")
142  if not options.bench:
143  spherePlanningOnce(cp, planners[0], options.output)
144  else:
145  spherePlanningBench(cp, planners)
146 
147 if __name__ == "__main__":
148  parser = argparse.ArgumentParser()
149  parser.add_argument("-o", "--output", action="store_true",
150  help="Dump found solution path (if one exists) in plain text and planning "
151  "graph in GraphML to `sphere_path.txt` and `sphere_graph.graphml` "
152  "respectively.")
153  parser.add_argument("--bench", action="store_true",
154  help="Do benchmarking on provided planner list.")
155  addSpaceOption(parser)
156  addPlannerOption(parser)
157  addConstrainedOptions(parser)
158  addAtlasOptions(parser)
159 
160  spherePlanning(parser.parse_args())
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition: Constraint.h:107
Definition of an abstract state.
Definition: State.h:113
virtual unsigned int getDimension() const =0
Return the dimension of the projection defined by this evaluator.
A state space representing Rn. The distance function is the L2 norm.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...
virtual void defaultCellSizes()
Set the default cell dimensions for this projection. The default implementation of this function is e...
The lower and upper bounds for an Rn space.