ConstrainedPlanningCommon.py
1 #!/usr/bin/env python
2 
3 
36 
37 # Author: Mark Moll
38 
39 from __future__ import print_function
40 try:
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
45 except 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
56 import datetime
57 
58 
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)
63  AT - Atlas
64  TB - Tangent Bundle.""")
65 
66 
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, "
72  "BITstar, "
73  "PRM, SPARS, "
74  "KPIECE1, BKPIECE1.")
75 
76 
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).")
92 
93 def list2vec(l):
94  ret = ou.vectorDouble()
95  for e in l:
96  ret.append(e)
97  return ret
98 
99 def clearSpaceAndPlanner(planner):
100  planner.getSpaceInformation().getStateSpace().clear()
101  planner.clear()
102 
103 
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 "
111  "atlas sampling.")
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 "
116  "uniform.")
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 "
121  "traversal.")
122 
123 
124 class ConstrainedProblem(object):
125 
126  def __init__(self, spaceType, space, constraint, options):
127  self.spaceType = spaceType
128  self.space = space
129  self.constraint = constraint
130  self.constraint.setTolerance(options.tolerance)
131  self.constraint.setMaxIterations(options.tries)
132  self.options = options
133  self.bench = None
134  self.request = None
135  self.pp = None
136 
137  if spaceType == "PJ":
138  ou.OMPL_INFORM("Using Projection-Based State Space!")
139  self.css = ob.ProjectedStateSpace(space, constraint)
141  elif spaceType == "AT":
142  ou.OMPL_INFORM("Using Atlas-Based State Space!")
143  self.css = ob.AtlasStateSpace(space, constraint)
145  elif spaceType == "TB":
146  ou.OMPL_INFORM("Using Tangent Bundle-Based State Space!")
147  self.css = ob.TangentBundleStateSpace(space, constraint)
149 
150  self.css.setup()
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)
159  if options.bias:
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)
164  self.css.setup()
165  self.ss = og.SimpleSetup(self.csi)
166 
167  def setStartAndGoalStates(self, start, goal):
168  # Create start and goal states
169  if self.spaceType == "AT" or self.spaceType == "TB":
170  self.css.anchorChart(start())
171  self.css.anchorChart(goal())
172 
173  # Setup problem
174  self.ss.setStartAndGoalStates(start, goal)
175 
176  def getPlanner(self, plannerName, projectionName=None):
177  planner = eval('og.%s(self.csi)' % plannerName)
178  try:
179  if self.options.range == 0:
180  if not self.spaceType == "PJ":
181  planner.setRange(self.css.getRho_s())
182  else:
183  planner.setRange(self.options.range)
184  except:
185  pass
186  try:
187  if projectionName:
188  planner.setProjectionEvaluator(projectionName)
189  except:
190  pass
191  return planner
192 
193  def setPlanner(self, plannerName, projectionName=None):
194  self.pp = self.getPlanner(plannerName, projectionName)
195  self.ss.setPlanner(self.pp)
196 
197  def solveOnce(self, output=False, name="ompl"):
198  self.ss.setup()
199  stat = self.ss.solve(self.options.time)
200 
201  if stat:
202  # Get solution and validate
203  path = self.ss.getSolutionPath()
204  if not path.check():
205  ou.OMPL_WARN("Path fails check!")
206 
207  if stat == ob.PlannerStatus.APPROXIMATE_SOLUTION:
208  ou.OMPL_WARN("Solution is approximate.")
209 
210  # Simplify solution and validate simplified solution path.
211  ou.OMPL_INFORM("Simplifying solution...")
212  self.ss.simplifySolution(5.)
213 
214  simplePath = self.ss.getSolutionPath()
215  ou.OMPL_INFORM("Simplified Path Length: %.3f -> %.3f" %
216  (path.length(), simplePath.length()))
217 
218  if not simplePath.check():
219  ou.OMPL_WARN("Simplified path fails check!")
220 
221  # Interpolate and validate interpolated solution path.
222  ou.OMPL_INFORM("Interpolating path...")
223  path.interpolate()
224 
225  if not path.check():
226  ou.OMPL_WARN("Interpolated simplified path fails check!")
227 
228  ou.OMPL_INFORM("Interpolating simplified path...")
229  simplePath.interpolate()
230 
231  if not simplePath.check():
232  ou.OMPL_WARN("Interpolated simplified path fails check!")
233 
234  if output:
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)
238 
239  ou.OMPL_INFORM(
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)
243  else:
244  ou.OMPL_WARN("No solution found.")
245 
246  return stat
247 
248  def setupBenchmark(self, planners, problem):
249  self.bench = ot.Benchmark(self.ss, problem)
250 
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)
258 
259  self.request = ot.Benchmark.Request()
260  self.request.maxTime = self.options.time
261  self.request.maxMem = 1e9
262  self.request.runCount = 100
263  self.request.timeBetweenUpdates = 0.1
264  self.request.saveConsoleOutput = False
265  for planner in planners:
266  self.bench.addPlanner(self.getPlanner(planner, problem))
267 
268  self.bench.setPreRunEvent(ot.PreSetupEvent(clearSpaceAndPlanner))
269 
270  def runBenchmark(self):
271  self.bench.benchmark(self.request)
272  filename = str(datetime.datetime.now()) + '_' + \
273  self.bench.getExperimentName() + '_' + self.spaceType
274  self.bench.saveResultsToFile(filename)
275 
276  def atlasStats(self):
277  # For atlas types, output information about size of atlas and amount of
278  # space explored
279  if self.spaceType == "AT" or self.spaceType == "TB":
280  ou.OMPL_INFORM("Atlas has %d charts" % self.css.getChartCount())
281  if self.spaceType == "AT":
282  ou.OMPL_INFORM("Atlas is approximately %.3f%% open" %
283  self.css.estimateFrontierPercent())
284 
285  def dumpGraph(self, name):
286  ou.OMPL_INFORM("Dumping planner graph to `%s_graph.graphml`." % name)
287  data = ob.PlannerData(self.csi)
288  self.pp.getPlannerData(data)
289 
290  with open("%s_graph.graphml" % name, "w") as graphfile:
291  print(data.printGraphML(), file=graphfile)
292 
293  if self.spaceType == "AT" or self.spaceType == "TB":
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)
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
ConstrainedStateSpace encapsulating a planner-agnostic atlas algorithm for planning on a constraint m...
def getPlanner(self, plannerName, projectionName=None)
ConstrainedStateSpace encapsulating a planner-agnostic lazy atlas algorithm for planning on a constra...
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:48
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
Space information for a tangent bundle-based state space. Implements more direct for getting motion s...
ConstrainedStateSpace encapsulating a projection-based methodology for planning with constraints...
Space information for a constrained state space. Implements more direct for getting motion states...