def setPlanner(self):
# (optionally) set planner
self.si = self.ss.getSpaceInformation()
if self.plannerType.lower() == "rrt":
planner = oc.RRT(self.si)
elif self.plannerType.lower() == "est":
planner = oc.EST(self.si)
elif self.plannerType.lower() == "kpiece1":
planner = oc.KPIECE1(self.si)
elif self.plannerType.lower() == "syclopest":
# SyclopEST and SyclopRRT require a decomposition to guide the search
decomp = MyDecomposition(32, self.bounds)
planner = oc.SyclopEST(self.si, decomp)
elif self.plannerType.lower() == "sycloprrt":
# SyclopEST and SyclopRRT require a decomposition to guide the search
decomp = MyDecomposition(32, self.bounds)
planner = oc.SyclopRRT(self.si, decomp)
else:
OMPL_ERROR(
"Planner-type is not implemented in ompl control function.")
decomp = MyDecomposition(32, self.bounds)
planner = oc.SyclopEST(self.si, decomp)
self.ss.setPlanner(planner)
# (optionally) set propagation step size
self.si.setPropagationStepSize(.05)
def newplanner(self, si):
planner = oc.KPIECE1(si)
cdim = ou.vectorDouble()
cdim.extend([1, 1])
ope = MyProjectionEvaluator(si.getStateSpace(), cdim)
planner.setProjectionEvaluator(ope)
return planner
def benchmark(ss):
# instantiating the benchmark object
b = ot.Benchmark(ss, "benchmarking")
# getting the space information
si = ss.getSpaceInformation()
# adding the planners
b.addPlanner(oc.EST(si))
b.addPlanner(oc.KPIECE1(si))
b.addPlanner(oc.PDST(si))
b.addPlanner(oc.RRT(si))
b.addPlanner(oc.SST(si))
# instantiating the benchmark request
req = ot.Benchmark.Request()
req.maxTime = 60.0 # time limit allowed for every planner execution (seconds)
req.maxMem = 1000.0 # maximum memory allowed for every planner execution (MB)
req.runCount = 1 # number of runs for every planner execution
req.displayProgress = True
# running the benchmark
b.benchmark(req)
# saving the result to a file of the form ompl_host_time.log
b.saveResultsToFile()
print("Benchmarks done!")
def allocate_planner(si, planner_type, decomp):
if planner_type.lower() == "est":
return oc.EST(si)
elif planner_type.lower() == "kpiece":
return oc.KPIECE1(si)
elif planner_type.lower() == "pdst":
return og.PDST(si)
elif planner_type.lower() == "rrt":
return oc.RRT(si)
elif planner_type.lower() == "syclopest":
return oc.SyclopEST(si, decomp)
elif planner_type.lower() == "sycloprrt":
return oc.SyclopRRT(si, decomp)
else:
ou.OMPL_ERROR(
"Planner-type is not implemented in allocation function.")
def plan():
# construct the state space we are planning in
ripl = app.SE2RigidBodyPlanning()
ripl.setEnvironmentMesh('./mesh/UniqueSolutionMaze_env.dae')
ripl.setRobotMesh('./mesh/car2_planar_robot.dae')
# space = ob.SE2StateSpace()
space = ripl.getStateSpace()
# set the bounds for the R^2 part of SE(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-55)
bounds.setHigh(55)
space.setBounds(bounds)
# create a control space
cspace = oc.RealVectorControlSpace(space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(0, -3.0)
cbounds.setHigh(0, 3.0)
cbounds.setLow(1, 3.0)
cbounds.setHigh(1, 3.0)
cspace.setBounds(cbounds)
# define a simple setup class
ss = oc.SimpleSetup(cspace)
# ss.setStateValidityChecker(ob.StateValidityCheckerFn( \
# partial(isStateValid, ss.getSpaceInformation())))
sp_info = ss.getSpaceInformation()
extractor = ripl.getGeometricStateExtractor()
enabled = ripl.isSelfCollisionEnabled()
checker = ripl.allocStateValidityChecker(sp_info, extractor, enabled)
ss.setStateValidityChecker(checker)
ss.setStatePropagator(oc.StatePropagatorFn(propagate))
# create a start state
start = ob.State(space)
start().setX(3)
start().setY(-3)
start().setYaw(0.0)
# create a goal state
goal = ob.State(space)
goal().setX(45)
goal().setY(25)
goal().setYaw(0.0)
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# (optionally) set planner
si = ss.getSpaceInformation()
# planner = oc.PDST(si)
# planner = oc.RRT(si)
# planner = oc.EST(si)
planner = oc.KPIECE1(si) # this is the default
# SyclopEST and SyclopRRT require a decomposition to guide the search
decomp = MyDecomposition(32, bounds)
# planner = oc.SyclopEST(si, decomp)
# planner = oc.SyclopRRT(si, decomp)
ss.setPlanner(planner)
# (optionally) set propagation step size
si.setPropagationStepSize(.1)
# attempt to solve the problem
solved = ss.solve(20.0)
if solved:
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
with open('path.txt', 'w') as outFile:
outFile.write(ss.getSolutionPath().printAsMatrix())
def setKPIECE1Planner(self, goal_bias=0.05, border_fraction=0.80):
self.planner = oc.KPIECE1(self.car.getSpaceInformation())
self.planner.setGoalBias(goal_bias)
self.planner.setBorderFraction(border_fraction)
self.car.setPlanner(self.planner)