Loading...
Searching...
No Matches
ConstrainedPlanningCommon.py
1#!/usr/bin/env python
2
3
36
37# Author: Mark Moll
38
39from __future__ import print_function
40try:
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
45except ImportError:
46 # if the ompl module is not in the PYTHONPATH assume it is installed in a
47 # subdirectory of the parent directory called "py-bindings."
48 from os.path import abspath, dirname, join
49 import sys
50 sys.path.insert(
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
56import datetime
57
58
59def addSpaceOption(parser):
60 parser.add_argument("-s", "--space", default="PJ",
61 choices=["PJ", "AT", "TB"],
62 help="""Choose which constraint handling methodology to use. One of:
63 PJ - Projection (Default)
64 AT - Atlas
65 TB - Tangent Bundle.""")
66
67
68def addPlannerOption(parser):
69 parser.add_argument("-p", "--planner", default="RRT",
70 help="Comma-separated list of which motion planner to use (multiple if "
71 "benchmarking, one if planning).\n Choose from, e.g.:\n"
72 "RRT (Default), RRTConnect, RRTstar, "
73 "EST, BiEST, ProjEST, "
74 "BITstar, "
75 "PRM, SPARS, "
76 "KPIECE1, BKPIECE1.")
77
78
79def addConstrainedOptions(parser):
80 group = parser.add_argument_group("Constrained planning options")
81 group.add_argument("-d", "--delta", type=float, default=ob.CONSTRAINED_STATE_SPACE_DELTA,
82 help="Step-size for discrete geodesic on manifold.")
83 group.add_argument("--lambda", type=float, dest="lambda_", metavar="LAMBDA",
84 default=ob.CONSTRAINED_STATE_SPACE_LAMBDA,
85 help="Maximum `wandering` allowed during atlas traversal. Must be greater "
86 "than 1.")
87 group.add_argument("--tolerance", type=float, default=ob.CONSTRAINT_PROJECTION_TOLERANCE,
88 help="Constraint satisfaction tolerance.")
89 group.add_argument("--time", type=float, default=5.,
90 help="Planning time allowed.")
91 group.add_argument("--tries", type=int, default=ob.CONSTRAINT_PROJECTION_MAX_ITERATIONS,
92 help="Maximum number sample tries per sample.")
93 group.add_argument("-r", "--range", type=float, default=0.,
94 help="Planner `range` value for planners that support this parameter. "
95 "Automatically determined otherwise (when 0).")
96
97def list2vec(l):
98 ret = ou.vectorDouble()
99 for e in l:
100 ret.append(e)
101 return ret
102
103def clearSpaceAndPlanner(planner):
104 planner.getSpaceInformation().getStateSpace().clear()
105 planner.clear()
106
107
108def addAtlasOptions(parser):
109 group = parser.add_argument_group("Atlas options")
110 group.add_argument("--epsilon", type=float, default=ob.ATLAS_STATE_SPACE_EPSILON,
111 help="Maximum distance from an atlas chart to the manifold. Must be "
112 "positive.")
113 group.add_argument("--rho", type=float, default=ob.CONSTRAINED_STATE_SPACE_DELTA *
114 ob.ATLAS_STATE_SPACE_RHO_MULTIPLIER,
115 help="Maximum radius for an atlas chart. Must be positive.")
116 group.add_argument("--exploration", type=float, default=ob.ATLAS_STATE_SPACE_EXPLORATION,
117 help="Value in [0, 1] which tunes balance of refinement and exploration in "
118 "atlas sampling.")
119 group.add_argument("--alpha", type=float, default=ob.ATLAS_STATE_SPACE_ALPHA,
120 help="Maximum angle between an atlas chart and the manifold. Must be in "
121 "[0, PI/2].")
122 group.add_argument("--bias", action="store_true",
123 help="Sets whether the atlas should use frontier-biased chart sampling "
124 "rather than uniform.")
125 group.add_argument("--no-separate", action="store_true",
126 help="Sets that the atlas should not compute chart separating halfspaces.")
127 group.add_argument("--charts", type=int, default=ob.ATLAS_STATE_SPACE_MAX_CHARTS_PER_EXTENSION,
128 help="Maximum number of atlas charts that can be generated during one "
129 "manifold traversal.")
130
131
132class ConstrainedProblem(object):
133
134 def __init__(self, spaceType, space, constraint, options):
135 self.spaceType = spaceType
136 self.space = space
137 self.constraint = constraint
138 self.constraint.setTolerance(options.tolerance)
139 self.constraint.setMaxIterations(options.tries)
140 self.options = options
141 self.bench = None
142 self.request = None
143 self.pp = None
144
145 if spaceType == "PJ":
146 ou.OMPL_INFORM("Using Projection-Based State Space!")
147 self.css = ob.ProjectedStateSpace(space, constraint)
149 elif spaceType == "AT":
150 ou.OMPL_INFORM("Using Atlas-Based State Space!")
151 self.css = ob.AtlasStateSpace(space, constraint)
153 elif spaceType == "TB":
154 ou.OMPL_INFORM("Using Tangent Bundle-Based State Space!")
155 self.css = ob.TangentBundleStateSpace(space, constraint)
157
158 self.css.setup()
159 self.css.setDelta(options.delta)
160 self.css.setLambda(options.lambda_)
161 if not spaceType == "PJ":
162 self.css.setExploration(options.exploration)
163 self.css.setEpsilon(options.epsilon)
164 self.css.setRho(options.rho)
165 self.css.setAlpha(options.alpha)
166 self.css.setMaxChartsPerExtension(options.charts)
167 if options.bias:
168 self.css.setBiasFunction(lambda c, atlas=self.css:
169 atlas.getChartCount() - c.getNeighborCount() + 1.)
170 if spaceType == "AT":
171 self.css.setSeparated(not options.no_separate)
172 self.css.setup()
173 self.ss = og.SimpleSetup(self.csi)
174
175 def setStartAndGoalStates(self, start, goal):
176 # Create start and goal states
177 if self.spaceType == "AT" or self.spaceType == "TB":
178 self.css.anchorChart(start())
179 self.css.anchorChart(goal())
180
181 # Setup problem
182 self.ss.setStartAndGoalStates(start, goal)
183
184 def getPlanner(self, plannerName, projectionName=None):
185 planner = eval('og.%s(self.csi)' % plannerName)
186 try:
187 if self.options.range == 0:
188 if not self.spaceType == "PJ":
189 planner.setRange(self.css.getRho_s())
190 else:
191 planner.setRange(self.options.range)
192 except:
193 pass
194 try:
195 if projectionName:
196 planner.setProjectionEvaluator(projectionName)
197 except:
198 pass
199 return planner
200
201 def setPlanner(self, plannerName, projectionName=None):
202 self.pp = self.getPlanner(plannerName, projectionName)
203 self.ss.setPlanner(self.pp)
204
205 def solveOnce(self, output=False, name="ompl"):
206 self.ss.setup()
207 stat = self.ss.solve(self.options.time)
208
209 if stat:
210 # Get solution and validate
211 path = self.ss.getSolutionPath()
212 if not path.check():
213 ou.OMPL_WARN("Path fails check!")
214
215 if stat == ob.PlannerStatus.APPROXIMATE_SOLUTION:
216 ou.OMPL_WARN("Solution is approximate.")
217
218 # Simplify solution and validate simplified solution path.
219 ou.OMPL_INFORM("Simplifying solution...")
220 self.ss.simplifySolution(5.)
221
222 simplePath = self.ss.getSolutionPath()
223 ou.OMPL_INFORM("Simplified Path Length: %.3f -> %.3f" %
224 (path.length(), simplePath.length()))
225
226 if not simplePath.check():
227 ou.OMPL_WARN("Simplified path fails check!")
228
229 # Interpolate and validate interpolated solution path.
230 ou.OMPL_INFORM("Interpolating path...")
231 path.interpolate()
232
233 if not path.check():
234 ou.OMPL_WARN("Interpolated simplified path fails check!")
235
236 ou.OMPL_INFORM("Interpolating simplified path...")
237 simplePath.interpolate()
238
239 if not simplePath.check():
240 ou.OMPL_WARN("Interpolated simplified path fails check!")
241
242 if output:
243 ou.OMPL_INFORM("Dumping path to `%s_path.txt`." % name)
244 with open('%s_path.txt' % name, 'w') as pathfile:
245 print(path.printAsMatrix, file=pathfile)
246
247 ou.OMPL_INFORM(
248 "Dumping simplified path to `%s_simplepath.txt`." % name)
249 with open("%s_simplepath.txt" % name, 'w') as simplepathfile:
250 print(simplePath.printAsMatrix(), file=simplepathfile)
251 else:
252 ou.OMPL_WARN("No solution found.")
253
254 return stat
255
256 def setupBenchmark(self, planners, problem):
257 self.bench = ot.Benchmark(self.ss, problem)
258
259 self.bench.addExperimentParameter(
260 "n", "INTEGER", str(self.constraint.getAmbientDimension()))
261 self.bench.addExperimentParameter(
262 "k", "INTEGER", str(self.constraint.getManifoldDimension()))
263 self.bench.addExperimentParameter(
264 "n - k", "INTEGER", str(self.constraint.getCoDimension()))
265 self.bench.addExperimentParameter("space", "INTEGER", self.spaceType)
266
267 self.request = ot.Benchmark.Request()
268 self.request.maxTime = self.options.time
269 self.request.maxMem = 1e9
270 self.request.runCount = 100
271 self.request.timeBetweenUpdates = 0.1
272 self.request.saveConsoleOutput = False
273 for planner in planners:
274 self.bench.addPlanner(self.getPlanner(planner, problem))
275
276 self.bench.setPreRunEvent(ot.PreSetupEvent(clearSpaceAndPlanner))
277
278 def runBenchmark(self):
279 self.bench.benchmark(self.request)
280 filename = str(datetime.datetime.now()) + '_' + \
281 self.bench.getExperimentName() + '_' + self.spaceType
282 self.bench.saveResultsToFile(filename)
283
284 def atlasStats(self):
285 # For atlas types, output information about size of atlas and amount of
286 # space explored
287 if self.spaceType == "AT" or self.spaceType == "TB":
288 ou.OMPL_INFORM("Atlas has %d charts" % self.css.getChartCount())
289 if self.spaceType == "AT":
290 ou.OMPL_INFORM("Atlas is approximately %.3f%% open" %
291 self.css.estimateFrontierPercent())
292
293 def dumpGraph(self, name):
294 ou.OMPL_INFORM("Dumping planner graph to `%s_graph.graphml`." % name)
295 data = ob.PlannerData(self.csi)
296 self.pp.getPlannerData(data)
297
298 with open("%s_graph.graphml" % name, "w") as graphfile:
299 print(data.printGraphML(), file=graphfile)
300
301 if self.spaceType == "AT" or self.spaceType == "TB":
302 ou.OMPL_INFORM("Dumping atlas to `%s_atlas.ply`." % name)
303 with open("%s_atlas.ply" % name, "w") as atlasfile:
304 print(self.css.printPLY(), file=atlasfile)
getPlanner(self, plannerName, projectionName=None)
ConstrainedStateSpace encapsulating a planner-agnostic atlas algorithm for planning on a constraint m...
Space information for a constrained state space. Implements more direct for getting motion states.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
ConstrainedStateSpace encapsulating a projection-based methodology for planning with constraints.
Space information for a tangent bundle-based state space. Implements more direct for getting motion s...
ConstrainedStateSpace encapsulating a planner-agnostic lazy atlas algorithm for planning on a constra...
Create the set of classes typically needed to solve a geometric problem.
Definition SimpleSetup.h:63
Benchmark a set of planners on a problem instance.
Definition Benchmark.h:49