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:
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 from sys import argv
54 import argparse
55 
56 
61 class ValidityChecker(ob.StateValidityChecker):
62  def __init__(self, si):
63  super(ValidityChecker, self).__init__(si)
64 
65  # Returns whether the given state's position overlaps the
66  # circular obstacle
67  def isValid(self, state):
68  return self.clearance(state) > 0.0
69 
70  # Returns the distance from the given state's position to the
71  # boundary of the circular obstacle.
72  def clearance(self, state):
73  # Extract the robot's (x,y) position from its state
74  x = state[0]
75  y = state[1]
76 
77  # Distance formula between two points, offset by the circle's
78  # radius
79  return sqrt((x-0.5)*(x-0.5) + (y-0.5)*(y-0.5)) - 0.25
80 
81 
82 
86 def getPathLengthObjective(si):
88 
89 
92 def getThresholdPathLengthObj(si):
94  obj.setCostThreshold(ob.Cost(1.51))
95  return obj
96 
97 
109 class ClearanceObjective(ob.StateCostIntegralObjective):
110  def __init__(self, si):
111  super(ClearanceObjective, self).__init__(si, True)
112  self.si_ = si
113 
114  # Our requirement is to maximize path clearance from obstacles,
115  # but we want to represent the objective as a path cost
116  # minimization. Therefore, we set each state's cost to be the
117  # reciprocal of its clearance, so that as state clearance
118  # increases, the state cost decreases.
119  def stateCost(self, s):
120  return ob.Cost(1 / self.si_.getStateValidityChecker().clearance(s))
121 
122 
124 def getClearanceObjective(si):
125  return ClearanceObjective(si)
126 
127 
138 def getBalancedObjective1(si):
139  lengthObj = ob.PathLengthOptimizationObjective(si)
140  clearObj = ClearanceObjective(si)
141 
143  opt.addObjective(lengthObj, 5.0)
144  opt.addObjective(clearObj, 1.0)
145 
146  return opt
147 
148 
156 
157 
158 
161 def getPathLengthObjWithCostToGo(si):
163  obj.setCostToGoHeuristic(ob.CostToGoHeuristic(ob.goalRegionCostToGo))
164  return obj
165 
166 
167 # Keep these in alphabetical order and all lower case
168 def allocatePlanner(si, plannerType):
169  if plannerType.lower() == "bfmtstar":
170  return og.BFMT(si)
171  elif plannerType.lower() == "bitstar":
172  return og.BITstar(si)
173  elif plannerType.lower() == "fmtstar":
174  return og.FMT(si)
175  elif plannerType.lower() == "informedrrtstar":
176  return og.InformedRRTstar(si)
177  elif plannerType.lower() == "prmstar":
178  return og.PRMstar(si)
179  elif plannerType.lower() == "rrtstar":
180  return og.RRTstar(si)
181  elif plannerType.lower() == "sorrtstar":
182  return og.SORRTstar(si)
183  else:
184  OMPL_ERROR("Planner-type is not implemented in allocation function.");
185 
186 
187 # Keep these in alphabetical order and all lower case
188 def allocateObjective(si, objectiveType):
189  if objectiveType.lower() == "pathclearance":
190  return getClearanceObjective(si)
191  elif objectiveType.lower() == "pathlength":
192  return getPathLengthObjective(si)
193  elif objectiveType.lower() == "thresholdpathlength":
194  return getThresholdPathLengthObj(si)
195  elif objectiveType.lower() == "weightedlengthandclearancecombo":
196  return getBalancedObjective1(si)
197  else:
198  OMPL_ERROR("Optimization-objective is not implemented in allocation function.");
199 
200 
201 
202 def plan(runTime, plannerType, objectiveType, fname):
203  # Construct the robot state space in which we're planning. We're
204  # planning in [0,1]x[0,1], a subset of R^2.
205  space = ob.RealVectorStateSpace(2)
206 
207  # Set the bounds of space to be in [0,1].
208  space.setBounds(0.0, 1.0)
209 
210  # Construct a space information instance for this state space
211  si = ob.SpaceInformation(space)
212 
213  # Set the object used to check which states in the space are valid
214  validityChecker = ValidityChecker(si)
215  si.setStateValidityChecker(validityChecker)
216 
217  si.setup()
218 
219  # Set our robot's starting state to be the bottom-left corner of
220  # the environment, or (0,0).
221  start = ob.State(space)
222  start[0] = 0.0
223  start[1] = 0.0
224 
225  # Set our robot's goal state to be the top-right corner of the
226  # environment, or (1,1).
227  goal = ob.State(space)
228  goal[0] = 1.0
229  goal[1] = 1.0
230 
231  # Create a problem instance
232  pdef = ob.ProblemDefinition(si)
233 
234  # Set the start and goal states
235  pdef.setStartAndGoalStates(start, goal)
236 
237  # Create the optimization objective specified by our command-line argument.
238  # This helper function is simply a switch statement.
239  pdef.setOptimizationObjective(allocateObjective(si, objectiveType))
240 
241  # Construct the optimal planner specified by our command line argument.
242  # This helper function is simply a switch statement.
243  optimizingPlanner = allocatePlanner(si, plannerType)
244 
245  # Set the problem instance for our planner to solve
246  optimizingPlanner.setProblemDefinition(pdef)
247  optimizingPlanner.setup()
248 
249  # attempt to solve the planning problem in the given runtime
250  solved = optimizingPlanner.solve(runTime)
251 
252  if solved:
253  # Output the length of the path found
254  print("{0} found solution of path length {1:.4f} with an optimization objective value of {2:.4f}".format(optimizingPlanner.getName(), pdef.getSolutionPath().length(), 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='(Optional) Specify the runtime in seconds. Defaults to 1 and must be greater than 0.')
270  parser.add_argument('-p', '--planner', default='RRTstar', choices=['BFMTstar', 'BITstar', 'FMTstar', 'InformedRRTstar', 'PRMstar', 'RRTstar', 'SORRTstar'], help='(Optional) Specify the optimal planner to use, defaults to RRTstar if not given.') # Alphabetical order
271  parser.add_argument('-o', '--objective', default='PathLength', choices=['PathClearance', 'PathLength', 'ThresholdPathLength', 'WeightedLengthAndClearanceCombo'], help='(Optional) Specify the optimization objective, defaults to PathLength if not given.') # Alphabetical order
272  parser.add_argument('-f', '--file', default=None, help='(Optional) Specify an output path for the found solution path.')
273  parser.add_argument('-i', '--info', type=int, default=0, choices=[0, 1, 2], help='(Optional) Set the OMPL log level. 0 for WARN, 1 for INFO, 2 for DEBUG. Defaults to WARN.')
274 
275  # Parse the arguments
276  args = parser.parse_args()
277 
278  # Check that time is positive
279  if args.runtime <= 0:
280  raise argparse.ArgumentTypeError("argument -t/--runtime: invalid choice: %r (choose a positive number greater than 0)"%(args.runtime,))
281 
282  # Set the log level
283  if args.info == 0:
284  ou.setLogLevel(ou.LOG_WARN)
285  elif args.info == 1:
286  ou.setLogLevel(ou.LOG_INFO)
287  elif args.info == 2:
288  ou.setLogLevel(ou.LOG_DEBUG)
289  else:
290  OMPL_ERROR("Invalid log-level integer.");
291 
292  # Solve the planning problem
293  plan(args.runtime, args.planner, args.objective, args.file)
294 
295 
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...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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