def atlasStats(self):
# For atlas types, output information about size of atlas and amount of
# space explored
if self.spaceType == "AT" or self.spaceType == "TB":
ou.OMPL_INFORM("Atlas has %d charts" % self.css.getChartCount())
if self.spaceType == "AT":
ou.OMPL_INFORM("Atlas is approximately %.3f%% open" %
self.css.estimateFrontierPercent())
def dumpGraph(self, name):
ou.OMPL_INFORM("Dumping planner graph to `%s_graph.graphml`." % name)
data = ob.PlannerData(self.csi)
self.pp.getPlannerData(data)
with open("%s_graph.graphml" % name, "w") as graphfile:
print(data.printGraphML(), file=graphfile)
if self.spaceType == "AT" or self.spaceType == "TB":
ou.OMPL_INFORM("Dumping atlas to `%s_atlas.ply`." % name)
with open("%s_atlas.ply" % name, "w") as atlasfile:
print(self.css.printPLY(), file=atlasfile)
def __init__(self, spaceType, space, constraint, options):
self.spaceType = spaceType
self.space = space
self.constraint = constraint
self.constraint.setTolerance(options.tolerance)
self.constraint.setMaxIterations(options.tries)
self.options = options
self.bench = None
self.request = None
self.pp = None
if spaceType == "PJ":
ou.OMPL_INFORM("Using Projection-Based State Space!")
self.css = ob.ProjectedStateSpace(space, constraint)
self.csi = ob.ConstrainedSpaceInformation(self.css)
elif spaceType == "AT":
ou.OMPL_INFORM("Using Atlas-Based State Space!")
self.css = ob.AtlasStateSpace(space, constraint)
self.csi = ob.ConstrainedSpaceInformation(self.css)
elif spaceType == "TB":
ou.OMPL_INFORM("Using Tangent Bundle-Based State Space!")
self.css = ob.TangentBundleStateSpace(space, constraint)
self.csi = ob.TangentBundleSpaceInformation(self.css)
self.css.setup()
self.css.setDelta(options.delta)
self.css.setLambda(options.lambda_)
if not spaceType == "PJ":
self.css.setExploration(options.exploration)
self.css.setEpsilon(options.epsilon)
self.css.setRho(options.rho)
self.css.setAlpha(options.alpha)
self.css.setMaxChartsPerExtension(options.charts)
if options.bias:
self.css.setBiasFunction(
lambda c, atlas=self.css: atlas.getChartCount(
) - c.getNeighborCount() + 1.)
if spaceType == "AT":
self.css.setSeparated(not options.no_separate)
self.css.setup()
self.ss = og.SimpleSetup(self.csi)
def solveOnce(self, output=False, name="ompl"):
self.ss.setup()
stat = self.ss.solve(self.options.time)
if stat:
# Get solution and validate
path = self.ss.getSolutionPath()
if not path.check():
ou.OMPL_WARN("Path fails check!")
if stat == ob.PlannerStatus.APPROXIMATE_SOLUTION:
ou.OMPL_WARN("Solution is approximate.")
# Simplify solution and validate simplified solution path.
ou.OMPL_INFORM("Simplifying solution...")
self.ss.simplifySolution(5.)
simplePath = self.ss.getSolutionPath()
ou.OMPL_INFORM("Simplified Path Length: %.3f -> %.3f" %
(path.length(), simplePath.length()))
if not simplePath.check():
ou.OMPL_WARN("Simplified path fails check!")
# Interpolate and validate interpolated solution path.
ou.OMPL_INFORM("Interpolating path...")
path.interpolate()
if not path.check():
ou.OMPL_WARN("Interpolated simplified path fails check!")
ou.OMPL_INFORM("Interpolating simplified path...")
simplePath.interpolate()
if not simplePath.check():
ou.OMPL_WARN("Interpolated simplified path fails check!")
if output:
ou.OMPL_INFORM("Dumping path to `%s_path.txt`." % name)
with open('%s_path.txt' % name, 'w') as pathfile:
print(path.printAsMatrix, file=pathfile)
ou.OMPL_INFORM(
"Dumping simplified path to `%s_simplepath.txt`." % name)
with open("%s_simplepath.txt" % name, 'w') as simplepathfile:
print(simplePath.printAsMatrix(), file=simplepathfile)
else:
ou.OMPL_WARN("No solution found.")
return stat
