OptimalPlanning.py
1 #!/usr/bin/env python
2 
3 
36 
37 # Author: Luis G. Torres, Mark Moll
38 
39 try:
40  from ompl import util as ou
41  from ompl import base as ob
42  from ompl import geometric as og
43 except ImportError:
44  # if the ompl module is not in the PYTHONPATH assume it is installed in a
45  # subdirectory of the parent directory called "py-bindings."
46  from os.path import abspath, dirname, join
47  import sys
48  sys.path.insert(0, join(dirname(dirname(abspath(__file__))), 'py-bindings'))
49  from ompl import util as ou
50  from ompl import base as ob
51  from ompl import geometric as og
52 from math import sqrt
53 import argparse
54 
55 
60 class ValidityChecker(ob.StateValidityChecker):
61  # Returns whether the given state's position overlaps the
62  # circular obstacle
63  def isValid(self, state):
64  return self.clearance(state) > 0.0
65 
66  # Returns the distance from the given state's position to the
67  # boundary of the circular obstacle.
68  def clearance(self, state):
69  # Extract the robot's (x,y) position from its state
70  x = state[0]
71  y = state[1]
72 
73  # Distance formula between two points, offset by the circle's
74  # radius
75  return sqrt((x-0.5)*(x-0.5) + (y-0.5)*(y-0.5)) - 0.25
76 
77 
78 
82 def getPathLengthObjective(si):
84 
85 
88 def getThresholdPathLengthObj(si):
90  obj.setCostThreshold(ob.Cost(1.51))
91  return obj
92 
93 
105 class ClearanceObjective(ob.StateCostIntegralObjective):
106  def __init__(self, si):
107  super(ClearanceObjective, self).__init__(si, True)
108  self.si_ = si
109 
110  # Our requirement is to maximize path clearance from obstacles,
111  # but we want to represent the objective as a path cost
112  # minimization. Therefore, we set each state's cost to be the
113  # reciprocal of its clearance, so that as state clearance
114  # increases, the state cost decreases.
115  def stateCost(self, s):
116  return ob.Cost(1 / self.si_.getStateValidityChecker().clearance(s))
117 
118 
120 def getClearanceObjective(si):
121  return ClearanceObjective(si)
122 
123 
134 def getBalancedObjective1(si):
135  lengthObj = ob.PathLengthOptimizationObjective(si)
136  clearObj = ClearanceObjective(si)
137 
139  opt.addObjective(lengthObj, 5.0)
140  opt.addObjective(clearObj, 1.0)
141 
142  return opt
143 
144 
152 
153 
154 
157 def getPathLengthObjWithCostToGo(si):
159  obj.setCostToGoHeuristic(ob.CostToGoHeuristic(ob.goalRegionCostToGo))
160  return obj
161 
162 
163 # Keep these in alphabetical order and all lower case
164 def allocatePlanner(si, plannerType):
165  if plannerType.lower() == "bfmtstar":
166  return og.BFMT(si)
167  elif plannerType.lower() == "bitstar":
168  return og.BITstar(si)
169  elif plannerType.lower() == "fmtstar":
170  return og.FMT(si)
171  elif plannerType.lower() == "informedrrtstar":
172  return og.InformedRRTstar(si)
173  elif plannerType.lower() == "prmstar":
174  return og.PRMstar(si)
175  elif plannerType.lower() == "rrtstar":
176  return og.RRTstar(si)
177  elif plannerType.lower() == "sorrtstar":
178  return og.SORRTstar(si)
179  else:
180  ou.OMPL_ERROR("Planner-type is not implemented in allocation function.")
181 
182 
183 # Keep these in alphabetical order and all lower case
184 def allocateObjective(si, objectiveType):
185  if objectiveType.lower() == "pathclearance":
186  return getClearanceObjective(si)
187  elif objectiveType.lower() == "pathlength":
188  return getPathLengthObjective(si)
189  elif objectiveType.lower() == "thresholdpathlength":
190  return getThresholdPathLengthObj(si)
191  elif objectiveType.lower() == "weightedlengthandclearancecombo":
192  return getBalancedObjective1(si)
193  else:
194  ou.OMPL_ERROR("Optimization-objective is not implemented in allocation function.")
195 
196 
197 
198 def plan(runTime, plannerType, objectiveType, fname):
199  # Construct the robot state space in which we're planning. We're
200  # planning in [0,1]x[0,1], a subset of R^2.
201  space = ob.RealVectorStateSpace(2)
202 
203  # Set the bounds of space to be in [0,1].
204  space.setBounds(0.0, 1.0)
205 
206  # Construct a space information instance for this state space
207  si = ob.SpaceInformation(space)
208 
209  # Set the object used to check which states in the space are valid
210  validityChecker = ValidityChecker(si)
211  si.setStateValidityChecker(validityChecker)
212 
213  si.setup()
214 
215  # Set our robot's starting state to be the bottom-left corner of
216  # the environment, or (0,0).
217  start = ob.State(space)
218  start[0] = 0.0
219  start[1] = 0.0
220 
221  # Set our robot's goal state to be the top-right corner of the
222  # environment, or (1,1).
223  goal = ob.State(space)
224  goal[0] = 1.0
225  goal[1] = 1.0
226 
227  # Create a problem instance
228  pdef = ob.ProblemDefinition(si)
229 
230  # Set the start and goal states
231  pdef.setStartAndGoalStates(start, goal)
232 
233  # Create the optimization objective specified by our command-line argument.
234  # This helper function is simply a switch statement.
235  pdef.setOptimizationObjective(allocateObjective(si, objectiveType))
236 
237  # Construct the optimal planner specified by our command line argument.
238  # This helper function is simply a switch statement.
239  optimizingPlanner = allocatePlanner(si, plannerType)
240 
241  # Set the problem instance for our planner to solve
242  optimizingPlanner.setProblemDefinition(pdef)
243  optimizingPlanner.setup()
244 
245  # attempt to solve the planning problem in the given runtime
246  solved = optimizingPlanner.solve(runTime)
247 
248  if solved:
249  # Output the length of the path found
250  print('{0} found solution of path length {1:.4f} with an optimization ' \
251  'objective value of {2:.4f}'.format( \
252  optimizingPlanner.getName(), \
253  pdef.getSolutionPath().length(), \
254  pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value()))
255 
256  # If a filename was specified, output the path as a matrix to
257  # that file for visualization
258  if fname:
259  with open(fname, 'w') as outFile:
260  outFile.write(pdef.getSolutionPath().printAsMatrix())
261  else:
262  print("No solution found.")
263 
264 if __name__ == "__main__":
265  # Create an argument parser
266  parser = argparse.ArgumentParser(description='Optimal motion planning demo program.')
267 
268  # Add a filename argument
269  parser.add_argument('-t', '--runtime', type=float, default=1.0, help=\
270  '(Optional) Specify the runtime in seconds. Defaults to 1 and must be greater than 0.')
271  parser.add_argument('-p', '--planner', default='RRTstar', \
272  choices=['BFMTstar', 'BITstar', 'FMTstar', 'InformedRRTstar', 'PRMstar', 'RRTstar', \
273  'SORRTstar'], \
274  help='(Optional) Specify the optimal planner to use, defaults to RRTstar if not given.')
275  parser.add_argument('-o', '--objective', default='PathLength', \
276  choices=['PathClearance', 'PathLength', 'ThresholdPathLength', \
277  'WeightedLengthAndClearanceCombo'], \
278  help='(Optional) Specify the optimization objective, defaults to PathLength if not given.')
279  parser.add_argument('-f', '--file', default=None, \
280  help='(Optional) Specify an output path for the found solution path.')
281  parser.add_argument('-i', '--info', type=int, default=0, choices=[0, 1, 2], \
282  help='(Optional) Set the OMPL log level. 0 for WARN, 1 for INFO, 2 for DEBUG.' \
283  ' Defaults to WARN.')
284 
285  # Parse the arguments
286  args = parser.parse_args()
287 
288  # Check that time is positive
289  if args.runtime <= 0:
290  raise argparse.ArgumentTypeError(
291  "argument -t/--runtime: invalid choice: %r (choose a positive number greater than 0)" \
292  % (args.runtime,))
293 
294  # Set the log level
295  if args.info == 0:
296  ou.setLogLevel(ou.LOG_WARN)
297  elif args.info == 1:
298  ou.setLogLevel(ou.LOG_INFO)
299  elif args.info == 2:
300  ou.setLogLevel(ou.LOG_DEBUG)
301  else:
302  ou.OMPL_ERROR("Invalid log-level integer.")
303 
304  # Solve the planning problem
305  plan(args.runtime, args.planner, args.objective, args.file)
306 
307 
Optimal Rapidly-exploring Random Trees.
Definition: RRTstar.h:76
This class allows for the definition of multiobjective optimal planning problems. Objectives are adde...
PRM* planner.
Definition: PRMstar.h:65
Asymptotically Optimal Fast Marching Tree algorithm developed by L. Janson and M. Pavone...
Definition: FMT.h:90
std::function< Cost(const State *, const Goal *)> CostToGoHeuristic
The definition of a function which returns an admissible estimate of the optimal path cost from a giv...
Bidirectional Asymptotically Optimal Fast Marching Tree algorithm developed by J. Starek...
Definition: BFMT.h:82
Defines optimization objectives where path cost can be represented as a path integral over a cost fun...
Abstract definition for a class checking the validity of states. The implementation of this class mus...
Batch Informed Trees (BIT*)
Definition: BITstar.h:107
A state space representing Rn. The distance function is the L2 norm.
The base class for space information. This contains all the information about the space planning is d...
An optimization objective which corresponds to optimizing path length.
Definition of an abstract state.
Definition: State.h:49
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47