OptimalPlanning.py
1 #!/usr/bin/env python
2 
3 
36 
37 # Author: Luis G. Torres, Mark Moll
38 
39 import sys
40 try:
41  from ompl import util as ou
42  from ompl import base as ob
43  from ompl import geometric as og
44 except ImportError:
45  # if the ompl module is not in the PYTHONPATH assume it is installed in a
46  # subdirectory of the parent directory called "py-bindings."
47  from os.path import abspath, dirname, join
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  sys.float_info.min))
118 
119 
121 def getClearanceObjective(si):
122  return ClearanceObjective(si)
123 
124 
135 def getBalancedObjective1(si):
136  lengthObj = ob.PathLengthOptimizationObjective(si)
137  clearObj = ClearanceObjective(si)
138 
140  opt.addObjective(lengthObj, 5.0)
141  opt.addObjective(clearObj, 1.0)
142 
143  return opt
144 
145 
153 
154 
155 
158 def getPathLengthObjWithCostToGo(si):
160  obj.setCostToGoHeuristic(ob.CostToGoHeuristic(ob.goalRegionCostToGo))
161  return obj
162 
163 
164 # Keep these in alphabetical order and all lower case
165 def allocatePlanner(si, plannerType):
166  if plannerType.lower() == "bfmtstar":
167  return og.BFMT(si)
168  elif plannerType.lower() == "bitstar":
169  return og.BITstar(si)
170  elif plannerType.lower() == "fmtstar":
171  return og.FMT(si)
172  elif plannerType.lower() == "informedrrtstar":
173  return og.InformedRRTstar(si)
174  elif plannerType.lower() == "prmstar":
175  return og.PRMstar(si)
176  elif plannerType.lower() == "rrtstar":
177  return og.RRTstar(si)
178  elif plannerType.lower() == "sorrtstar":
179  return og.SORRTstar(si)
180  else:
181  ou.OMPL_ERROR("Planner-type is not implemented in allocation function.")
182 
183 
184 # Keep these in alphabetical order and all lower case
185 def allocateObjective(si, objectiveType):
186  if objectiveType.lower() == "pathclearance":
187  return getClearanceObjective(si)
188  elif objectiveType.lower() == "pathlength":
189  return getPathLengthObjective(si)
190  elif objectiveType.lower() == "thresholdpathlength":
191  return getThresholdPathLengthObj(si)
192  elif objectiveType.lower() == "weightedlengthandclearancecombo":
193  return getBalancedObjective1(si)
194  else:
195  ou.OMPL_ERROR("Optimization-objective is not implemented in allocation function.")
196 
197 
198 
199 def plan(runTime, plannerType, objectiveType, fname):
200  # Construct the robot state space in which we're planning. We're
201  # planning in [0,1]x[0,1], a subset of R^2.
202  space = ob.RealVectorStateSpace(2)
203 
204  # Set the bounds of space to be in [0,1].
205  space.setBounds(0.0, 1.0)
206 
207  # Construct a space information instance for this state space
208  si = ob.SpaceInformation(space)
209 
210  # Set the object used to check which states in the space are valid
211  validityChecker = ValidityChecker(si)
212  si.setStateValidityChecker(validityChecker)
213 
214  si.setup()
215 
216  # Set our robot's starting state to be the bottom-left corner of
217  # the environment, or (0,0).
218  start = ob.State(space)
219  start[0] = 0.0
220  start[1] = 0.0
221 
222  # Set our robot's goal state to be the top-right corner of the
223  # environment, or (1,1).
224  goal = ob.State(space)
225  goal[0] = 1.0
226  goal[1] = 1.0
227 
228  # Create a problem instance
229  pdef = ob.ProblemDefinition(si)
230 
231  # Set the start and goal states
232  pdef.setStartAndGoalStates(start, goal)
233 
234  # Create the optimization objective specified by our command-line argument.
235  # This helper function is simply a switch statement.
236  pdef.setOptimizationObjective(allocateObjective(si, objectiveType))
237 
238  # Construct the optimal planner specified by our command line argument.
239  # This helper function is simply a switch statement.
240  optimizingPlanner = allocatePlanner(si, plannerType)
241 
242  # Set the problem instance for our planner to solve
243  optimizingPlanner.setProblemDefinition(pdef)
244  optimizingPlanner.setup()
245 
246  # attempt to solve the planning problem in the given runtime
247  solved = optimizingPlanner.solve(runTime)
248 
249  if solved:
250  # Output the length of the path found
251  print('{0} found solution of path length {1:.4f} with an optimization ' \
252  'objective value of {2:.4f}'.format( \
253  optimizingPlanner.getName(), \
254  pdef.getSolutionPath().length(), \
255  pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value()))
256 
257  # If a filename was specified, output the path as a matrix to
258  # that file for visualization
259  if fname:
260  with open(fname, 'w') as outFile:
261  outFile.write(pdef.getSolutionPath().printAsMatrix())
262  else:
263  print("No solution found.")
264 
265 if __name__ == "__main__":
266  # Create an argument parser
267  parser = argparse.ArgumentParser(description='Optimal motion planning demo program.')
268 
269  # Add a filename argument
270  parser.add_argument('-t', '--runtime', type=float, default=1.0, help=\
271  '(Optional) Specify the runtime in seconds. Defaults to 1 and must be greater than 0.')
272  parser.add_argument('-p', '--planner', default='RRTstar', \
273  choices=['BFMTstar', 'BITstar', 'FMTstar', 'InformedRRTstar', 'PRMstar', 'RRTstar', \
274  'SORRTstar'], \
275  help='(Optional) Specify the optimal planner to use, defaults to RRTstar if not given.')
276  parser.add_argument('-o', '--objective', default='PathLength', \
277  choices=['PathClearance', 'PathLength', 'ThresholdPathLength', \
278  'WeightedLengthAndClearanceCombo'], \
279  help='(Optional) Specify the optimization objective, defaults to PathLength if not given.')
280  parser.add_argument('-f', '--file', default=None, \
281  help='(Optional) Specify an output path for the found solution path.')
282  parser.add_argument('-i', '--info', type=int, default=0, choices=[0, 1, 2], \
283  help='(Optional) Set the OMPL log level. 0 for WARN, 1 for INFO, 2 for DEBUG.' \
284  ' Defaults to WARN.')
285 
286  # Parse the arguments
287  args = parser.parse_args()
288 
289  # Check that time is positive
290  if args.runtime <= 0:
291  raise argparse.ArgumentTypeError(
292  "argument -t/--runtime: invalid choice: %r (choose a positive number greater than 0)" \
293  % (args.runtime,))
294 
295  # Set the log level
296  if args.info == 0:
297  ou.setLogLevel(ou.LOG_WARN)
298  elif args.info == 1:
299  ou.setLogLevel(ou.LOG_INFO)
300  elif args.info == 2:
301  ou.setLogLevel(ou.LOG_DEBUG)
302  else:
303  ou.OMPL_ERROR("Invalid log-level integer.")
304 
305  # Solve the planning problem
306  plan(args.runtime, args.planner, args.objective, args.file)
307 
308 
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