OptimalPlanning.py
1#!/usr/bin/env python
2
3
36
37# Author: Luis G. Torres, Mark Moll
38
39import sys
40try:
41 from ompl import util as ou
42 from ompl import base as ob
43 from ompl import geometric as og
44except 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
52from math import sqrt
53import argparse
54
55
60class 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
82def getPathLengthObjective(si):
84
85
88def getThresholdPathLengthObj(si):
90 obj.setCostThreshold(ob.Cost(1.51))
91 return obj
92
93
105class 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
121def getClearanceObjective(si):
122 return ClearanceObjective(si)
123
124
135def getBalancedObjective1(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
158def getPathLengthObjWithCostToGo(si):
160 obj.setCostToGoHeuristic(ob.CostToGoHeuristic(ob.goalRegionCostToGo))
161 return obj
162
163
164# Keep these in alphabetical order and all lower case
165def 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
185def 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
199def 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
265if __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
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
This class allows for the definition of multiobjective optimal planning problems. Objectives are adde...
An optimization objective which corresponds to optimizing path length.
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
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...
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...
Definition of an abstract state.
Definition: State.h:50
Bidirectional Asymptotically Optimal Fast Marching Tree algorithm developed by J. Starek,...
Definition: BFMT.h:83
Batch Informed Trees (BIT*)
Definition: BITstar.h:93
Asymptotically Optimal Fast Marching Tree algorithm developed by L. Janson and M. Pavone.
Definition: FMT.h:91
PRM* planner.
Definition: PRMstar.h:66
Optimal Rapidly-exploring Random Trees.
Definition: RRTstar.h:75
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...