ConstrainedPlanningCommon.py

#!/usr/bin/env python3
 

 
# Author: Mark Moll, Weihang Guo
 
from __future__ import print_function
from ompl import util as ou
from ompl import base as ob
from ompl import geometric as og
from ompl import tools as ot
 
import datetime
 
 
def addSpaceOption(parser):
    parser.add_argument(
        "-s",
        "--space",
        default="PJ",
        choices=["PJ", "AT", "TB"],
        help="""Choose which constraint handling methodology to use. One of:
        PJ - Projection (Default)
        AT - Atlas
        TB - Tangent Bundle.""",
    )
 
 
def addPlannerOption(parser):
    parser.add_argument(
        "-p",
        "--planner",
        default="RRT",
        help="Comma-separated list of which motion planner to use (multiple if "
        "benchmarking, one if planning).\n Choose from, e.g.:\n"
        "RRT (Default), RRTConnect, RRTstar, "
        "EST, BiEST, ProjEST, "
        "BITstar, "
        "PRM, SPARS, "
        "KPIECE1, BKPIECE1.",
    )
 
 
def addConstrainedOptions(parser):
    group = parser.add_argument_group("Constrained planning options")
    group.add_argument(
        "-d",
        "--delta",
        type=float,
        default=ob.CONSTRAINED_STATE_SPACE_DELTA,
        help="Step-size for discrete geodesic on manifold.",
    )
    group.add_argument(
        "--lambda",
        type=float,
        dest="lambda_",
        metavar="LAMBDA",
        default=ob.CONSTRAINED_STATE_SPACE_LAMBDA,
        help="Maximum `wandering` allowed during atlas traversal. Must be greater "
        "than 1.",
    )
    group.add_argument(
        "--tolerance",
        type=float,
        default=ob.CONSTRAINT_PROJECTION_TOLERANCE,
        help="Constraint satisfaction tolerance.",
    )
    group.add_argument("--time", type=float, default=5.0, help="Planning time allowed.")
    group.add_argument(
        "--tries",
        type=int,
        default=ob.CONSTRAINT_PROJECTION_MAX_ITERATIONS,
        help="Maximum number sample tries per sample.",
    )
    group.add_argument(
        "-r",
        "--range",
        type=float,
        default=0.0,
        help="Planner `range` value for planners that support this parameter. "
        "Automatically determined otherwise (when 0).",
    )
 
 
def list2vec(l):
    ret = ou.vectorDouble()
    for e in l:
        ret.append(e)
    return ret
 
 
def clearSpaceAndPlanner(planner):
    planner.getSpaceInformation().getStateSpace().clear()
    planner.clear()
 
 
def addAtlasOptions(parser):
    group = parser.add_argument_group("Atlas options")
    group.add_argument(
        "--epsilon",
        type=float,
        default=ob.ATLAS_STATE_SPACE_EPSILON,
        help="Maximum distance from an atlas chart to the manifold. Must be positive.",
    )
    group.add_argument(
        "--rho",
        type=float,
        default=ob.CONSTRAINED_STATE_SPACE_DELTA * ob.ATLAS_STATE_SPACE_RHO_MULTIPLIER,
        help="Maximum radius for an atlas chart. Must be positive.",
    )
    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 "
        "atlas sampling.",
    )
    group.add_argument(
        "--alpha",
        type=float,
        default=ob.ATLAS_STATE_SPACE_ALPHA,
        help="Maximum angle between an atlas chart and the manifold. Must be in "
        "[0, PI/2].",
    )
    group.add_argument(
        "--bias",
        action="store_true",
        help="Sets whether the atlas should use frontier-biased chart sampling "
        "rather than uniform.",
    )
    group.add_argument(
        "--no-separate",
        action="store_true",
        help="Sets that the atlas should not compute chart separating halfspaces.",
    )
    group.add_argument(
        "--charts",
        type=int,
        default=ob.ATLAS_STATE_SPACE_MAX_CHARTS_PER_EXTENSION,
        help="Maximum number of atlas charts that can be generated during one "
        "manifold traversal.",
    )
 
 
class ConstrainedProblem(object):
    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(
                    ob.AtlasChartBiasFunction(
                        lambda c, atlas=self.css: atlas.getChartCount()
                        - c.getNeighborCount()
                        + 1.0
                    )
                )
            if spaceType == "AT":
                self.css.setSeparated(not options.no_separate)
            self.css.setup()
        self.ss = og.SimpleSetup(self.csi)
 
    def setStartAndGoalStates(self, start, goal):
        sstart = self.css.allocState()
        sgoal = self.css.allocState()
 
        sstart.copy(start)
        sgoal.copy(goal)
 
        # Create start and goal states
        if self.spaceType == "AT" or self.spaceType == "TB":
            self.css.anchorChart(sstart)
            self.css.anchorChart(sgoal)
 
        # Setup problem
        self.ss.setStartAndGoalStates(sstart, sgoal)
 
    def getPlanner(self, plannerName, projectionName=None):
        planner = eval("og.%s(self.csi)" % plannerName)
        try:
            if self.options.range == 0:
                if not self.spaceType == "PJ":
                    planner.setRange(self.css.getRho_s())
            else:
                planner.setRange(self.options.range)
        except:
            pass
        try:
            if projectionName:
                planner.setProjectionEvaluator(projectionName)
        except:
            pass
        return planner
 
    def setPlanner(self, plannerName, projectionName=None):
        self.pp = self.getPlanner(plannerName, projectionName)
        self.ss.setPlanner(self.pp)
 
    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.0)
 
            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
 
    def setupBenchmark(self, planners, problem):
        if not ot:
            print("Benchmarking not available, no ompl.tools")
            sys.exit(0)
 
        self.bench = ot.Benchmark(self.ss, problem)
 
        self.bench.addExperimentParameter(
            "n", "INTEGER", str(self.constraint.getAmbientDimension())
        )
        self.bench.addExperimentParameter(
            "k", "INTEGER", str(self.constraint.getManifoldDimension())
        )
        self.bench.addExperimentParameter(
            "n - k", "INTEGER", str(self.constraint.getCoDimension())
        )
        self.bench.addExperimentParameter("space", "INTEGER", self.spaceType)
 
        self.request = ot.Benchmark.Request()
        self.request.maxTime = self.options.time
        self.request.maxMem = 1e9
        self.request.runCount = 100
        self.request.timeBetweenUpdates = 0.1
        self.request.saveConsoleOutput = False
        for planner in planners:
            self.bench.addPlanner(self.getPlanner(planner, problem))
 
        self.bench.setPreRunEvent(ot.PreSetupEvent(clearSpaceAndPlanner))
 
    def runBenchmark(self):
        self.bench.benchmark(self.request)
        filename = (
            str(datetime.datetime.now())
            + "_"
            + self.bench.getExperimentName()
            + "_"
            + self.spaceType
        )
        self.bench.saveResultsToFile(filename)
 
    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)