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"],
62  help="""Choose which constraint handling methodology to use. One of:
63  PJ - Projection (Default)
64  AT - Atlas
65  TB - Tangent Bundle.""")
66 
67 
68 def 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 
79 def 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 
97 def list2vec(l):
98  ret = ou.vectorDouble()
99  for e in l:
100  ret.append(e)
101  return ret
102 
103 def clearSpaceAndPlanner(planner):
104  planner.getSpaceInformation().getStateSpace().clear()
105  planner.clear()
106 
107 
108 def 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 
132 class 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)
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:126
ConstrainedStateSpace encapsulating a planner-agnostic atlas algorithm for planning on a constraint m...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
Space information for a constrained state space. Implements more direct for getting motion states.
def getPlanner(self, plannerName, projectionName=None)
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:112
ConstrainedStateSpace encapsulating a projection-based methodology for planning with constraints.
ConstrainedStateSpace encapsulating a planner-agnostic lazy atlas algorithm for planning on a constra...
Space information for a tangent bundle-based state space. Implements more direct for getting motion s...