39 from __future__
import print_function
41 from ompl
import util
as ou
42 from ompl
import base
as ob
43 from ompl
import geometric
as og
44 from ompl
import tools
as ot
48 from os.path
import abspath, dirname, join
51 0, join(dirname(dirname(dirname(abspath(__file__)))),
'py-bindings'))
52 from ompl
import util
as ou
53 from ompl
import base
as ob
54 from ompl
import geometric
as og
55 from ompl
import tools
as ot
59 def addSpaceOption(parser):
60 parser.add_argument(
"-s",
"--space", default=
"PJ",
61 choices=[
"PJ",
"AT",
"TB"], help=
"""Choose which constraint handling methodology to use. One of:
62 PJ - Projection (Default)
64 TB - Tangent Bundle.""")
67 def addPlannerOption(parser):
68 parser.add_argument(
"-p",
"--planner", default=
"RRT",
69 help=
"Comma-separated list of which motion planner to use (multiple if benchmarking, one if planning).\n Choose from, e.g.:\n"
70 "RRT (Default), RRTConnect, RRTstar, "
71 "EST, BiEST, ProjEST, "
77 def addConstrainedOptions(parser):
78 group = parser.add_argument_group(
"Constrained planning options")
79 group.add_argument(
"-d",
"--delta", type=float, default=ob.CONSTRAINED_STATE_SPACE_DELTA,
80 help=
"Step-size for discrete geodesic on manifold.")
81 group.add_argument(
"--lambda", type=float, dest=
"lambda_", metavar=
"LAMBDA", default=ob.CONSTRAINED_STATE_SPACE_LAMBDA,
82 help=
"Maximum `wandering` allowed during atlas traversal. Must be greater than 1.")
83 group.add_argument(
"--tolerance", type=float, default=ob.CONSTRAINT_PROJECTION_TOLERANCE,
84 help=
"Constraint satisfaction tolerance.")
85 group.add_argument(
"--time", type=float, default=5.,
86 help=
"Planning time allowed.")
87 group.add_argument(
"--tries", type=int, default=ob.CONSTRAINT_PROJECTION_MAX_ITERATIONS,
88 help=
"Maximum number sample tries per sample.")
89 group.add_argument(
"-r",
"--range", type=float, default=0.,
90 help=
"Planner `range` value for planners that support this parameter. Automatically determined "
91 "otherwise (when 0).")
94 ret = ou.vectorDouble()
99 def clearSpaceAndPlanner(planner):
100 planner.getSpaceInformation().getStateSpace().clear()
104 def addAtlasOptions(parser):
105 group = parser.add_argument_group(
"Atlas options")
106 group.add_argument(
"--epsilon", type=float, default=ob.ATLAS_STATE_SPACE_EPSILON,
107 help=
"Maximum distance from an atlas chart to the manifold. Must be positive.")
108 group.add_argument(
"--rho", type=float, default=ob.CONSTRAINED_STATE_SPACE_DELTA *
109 ob.ATLAS_STATE_SPACE_RHO_MULTIPLIER, help=
"Maximum radius for an atlas chart. Must be positive.")
110 group.add_argument(
"--exploration", type=float, default=ob.ATLAS_STATE_SPACE_EXPLORATION, help=
"Value in [0, 1] which tunes balance of refinement and exploration in "
112 group.add_argument(
"--alpha", type=float, default=ob.ATLAS_STATE_SPACE_ALPHA,
113 help=
"Maximum angle between an atlas chart and the manifold. Must be in [0, PI/2].")
114 group.add_argument(
"--bias", action=
"store_true",
115 help=
"Sets whether the atlas should use frontier-biased chart sampling rather than "
117 group.add_argument(
"--no-separate", action=
"store_true",
118 help=
"Sets that the atlas should not compute chart separating halfspaces.")
119 group.add_argument(
"--charts", type=int, default=ob.ATLAS_STATE_SPACE_MAX_CHARTS_PER_EXTENSION,
120 help=
"Maximum number of atlas charts that can be generated during one manifold "
126 def __init__(self, spaceType, space, constraint, options):
130 self.
constraint.setTolerance(options.tolerance)
131 self.
constraint.setMaxIterations(options.tries)
137 if spaceType ==
"PJ":
138 ou.OMPL_INFORM(
"Using Projection-Based State Space!")
141 elif spaceType ==
"AT":
142 ou.OMPL_INFORM(
"Using Atlas-Based State Space!")
145 elif spaceType ==
"TB":
146 ou.OMPL_INFORM(
"Using Tangent Bundle-Based State Space!")
151 self.
css.setDelta(options.delta)
152 self.
css.setLambda(options.lambda_)
153 if not spaceType ==
"PJ":
154 self.
css.setExploration(options.exploration)
155 self.
css.setEpsilon(options.epsilon)
156 self.
css.setRho(options.rho)
157 self.
css.setAlpha(options.alpha)
158 self.
css.setMaxChartsPerExtension(options.charts)
160 self.
css.setBiasFunction(
lambda c, atlas=self.
css:
161 atlas.getChartCount() - c.getNeighborCount() + 1.)
162 if spaceType ==
"AT":
163 self.
css.setSeparated(
not options.no_separate)
167 def setStartAndGoalStates(self, start, goal):
170 self.
css.anchorChart(start())
171 self.
css.anchorChart(goal())
174 self.
ss.setStartAndGoalStates(start, goal)
176 def getPlanner(self, plannerName, projectionName=None):
177 planner = eval(
'og.%s(self.csi)' % plannerName)
181 planner.setRange(self.
css.getRho_s())
183 planner.setRange(self.
options.range)
188 planner.setProjectionEvaluator(projectionName)
193 def setPlanner(self, plannerName, projectionName=None):
195 self.
ss.setPlanner(self.
pp)
197 def solveOnce(self, output=False, name="ompl"):
203 path = self.
ss.getSolutionPath()
205 ou.OMPL_WARN(
"Path fails check!")
207 if stat == ob.PlannerStatus.APPROXIMATE_SOLUTION:
208 ou.OMPL_WARN(
"Solution is approximate.")
211 ou.OMPL_INFORM(
"Simplifying solution...")
212 self.
ss.simplifySolution(5.)
214 simplePath = self.
ss.getSolutionPath()
215 ou.OMPL_INFORM(
"Simplified Path Length: %.3f -> %.3f" %
216 (path.length(), simplePath.length()))
218 if not simplePath.check():
219 ou.OMPL_WARN(
"Simplified path fails check!")
222 ou.OMPL_INFORM(
"Interpolating path...")
226 ou.OMPL_WARN(
"Interpolated simplified path fails check!")
228 ou.OMPL_INFORM(
"Interpolating simplified path...")
229 simplePath.interpolate()
231 if not simplePath.check():
232 ou.OMPL_WARN(
"Interpolated simplified path fails check!")
235 ou.OMPL_INFORM(
"Dumping path to `%s_path.txt`." % name)
236 with open(
'%s_path.txt' % name,
'w')
as pathfile:
237 print(path.printAsMatrix, file=pathfile)
240 "Dumping simplified path to `%s_simplepath.txt`." % name)
241 with open(
"%s_simplepath.txt" % name,
'w')
as simplepathfile:
242 print(simplePath.printAsMatrix(), file=simplepathfile)
244 ou.OMPL_WARN(
"No solution found.")
248 def setupBenchmark(self, planners, problem):
251 self.
bench.addExperimentParameter(
252 "n",
"INTEGER", str(self.
constraint.getAmbientDimension()))
253 self.
bench.addExperimentParameter(
254 "k",
"INTEGER", str(self.
constraint.getManifoldDimension()))
255 self.
bench.addExperimentParameter(
256 "n - k",
"INTEGER", str(self.
constraint.getCoDimension()))
257 self.
bench.addExperimentParameter(
"space",
"INTEGER", self.
spaceType)
259 self.
request = ot.Benchmark.Request()
263 self.
request.timeBetweenUpdates = 0.1
264 self.
request.saveConsoleOutput =
False
265 for planner
in planners:
268 self.
bench.setPreRunEvent(ot.PreSetupEvent(clearSpaceAndPlanner))
270 def runBenchmark(self):
272 filename = str(datetime.datetime.now()) +
'_' + \
274 self.
bench.saveResultsToFile(filename)
276 def atlasStats(self):
280 ou.OMPL_INFORM(
"Atlas has %d charts" % self.
css.getChartCount())
282 ou.OMPL_INFORM(
"Atlas is approximately %.3f%% open" %
283 self.
css.estimateFrontierPercent())
285 def dumpGraph(self, name):
286 ou.OMPL_INFORM(
"Dumping planner graph to `%s_graph.graphml`." % name)
288 self.
pp.getPlannerData(data)
290 with open(
"%s_graph.graphml" % name,
"w")
as graphfile:
291 print(data.printGraphML(), file=graphfile)
294 ou.OMPL_INFORM(
"Dumping atlas to `%s_atlas.ply`." % name)
295 with open(
"%s_atlas.ply" % name,
"w")
as atlasfile:
296 print(self.
css.printPLY(), file=atlasfile)