41 from ompl
import util
as ou
42 from ompl
import base
as ob
43 from ompl
import geometric
as og
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
63 def isValid(self, state):
64 return self.clearance(state) > 0.0
68 def clearance(self, state):
75 return sqrt((x-0.5)*(x-0.5) + (y-0.5)*(y-0.5)) - 0.25
82 def getPathLengthObjective(si):
88 def getThresholdPathLengthObj(si):
90 obj.setCostThreshold(
ob.Cost(1.51))
106 def __init__(self, si):
107 super(ClearanceObjective, self).__init__(si,
True)
115 def stateCost(self, s):
116 return ob.Cost(1 / (self.si_.getStateValidityChecker().clearance(s) +
121 def getClearanceObjective(si):
122 return ClearanceObjective(si)
135 def getBalancedObjective1(si):
137 clearObj = ClearanceObjective(si)
140 opt.addObjective(lengthObj, 5.0)
141 opt.addObjective(clearObj, 1.0)
158 def getPathLengthObjWithCostToGo(si):
165 def allocatePlanner(si, plannerType):
166 if plannerType.lower() ==
"bfmtstar":
168 elif plannerType.lower() ==
"bitstar":
170 elif plannerType.lower() ==
"fmtstar":
172 elif plannerType.lower() ==
"informedrrtstar":
174 elif plannerType.lower() ==
"prmstar":
176 elif plannerType.lower() ==
"rrtstar":
178 elif plannerType.lower() ==
"sorrtstar":
181 ou.OMPL_ERROR(
"Planner-type is not implemented in allocation function.")
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)
195 ou.OMPL_ERROR(
"Optimization-objective is not implemented in allocation function.")
199 def plan(runTime, plannerType, objectiveType, fname):
205 space.setBounds(0.0, 1.0)
211 validityChecker = ValidityChecker(si)
212 si.setStateValidityChecker(validityChecker)
232 pdef.setStartAndGoalStates(start, goal)
236 pdef.setOptimizationObjective(allocateObjective(si, objectiveType))
240 optimizingPlanner = allocatePlanner(si, plannerType)
243 optimizingPlanner.setProblemDefinition(pdef)
244 optimizingPlanner.setup()
247 solved = optimizingPlanner.solve(runTime)
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()))
260 with open(fname,
'w')
as outFile:
261 outFile.write(pdef.getSolutionPath().printAsMatrix())
263 print(
"No solution found.")
265 if __name__ ==
"__main__":
267 parser = argparse.ArgumentParser(description=
'Optimal motion planning demo program.')
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', \
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.')
287 args = parser.parse_args()
290 if args.runtime <= 0:
291 raise argparse.ArgumentTypeError(
292 "argument -t/--runtime: invalid choice: %r (choose a positive number greater than 0)" \
297 ou.setLogLevel(ou.LOG_WARN)
299 ou.setLogLevel(ou.LOG_INFO)
301 ou.setLogLevel(ou.LOG_DEBUG)
303 ou.OMPL_ERROR(
"Invalid log-level integer.")
306 plan(args.runtime, args.planner, args.objective, args.file)
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
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.
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.
Bidirectional Asymptotically Optimal Fast Marching Tree algorithm developed by J. Starek,...
Batch Informed Trees (BIT*)
Asymptotically Optimal Fast Marching Tree algorithm developed by L. Janson and M. Pavone.
Optimal Rapidly-exploring Random Trees.
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...