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 plan():
# construct the state space we are planning in
space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-1)
bounds.setHigh(1)
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(-.3)
cbounds.setHigh(.3)
cspace.setBounds(cbounds)
# define a simple setup class
ss = oc.SimpleSetup(cspace)
ss.setStateValidityChecker(ob.StateValidityCheckerFn( \
partial(isStateValid, ss.getSpaceInformation())))
ss.setStatePropagator(oc.StatePropagatorFn(propagate))
# create a start state
start = ob.State(space)
start().setX(-0.5)
start().setY(0.0)
start().setYaw(0.0)
# create a goal state
goal = ob.State(space)
goal().setX(0.0)
goal().setY(0.5)
goal().setYaw(0.0)
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# (optionally) set planner
si = ss.getSpaceInformation()
#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())
def newplanner(self, si):
spacebounds = si.getStateSpace().getBounds()
bounds = ob.RealVectorBounds(2)
bounds.setLow(0, spacebounds.low[0]);
bounds.setLow(1, spacebounds.low[1]);
bounds.setHigh(0, spacebounds.high[0]);
bounds.setHigh(1, spacebounds.high[1]);
# Create a 10x10 grid decomposition for Syclop
decomp = SyclopDecomposition(10, bounds)
planner = oc.SyclopEST(si, decomp)
# Set syclop parameters conducive to a tiny workspace
planner.setNumFreeVolumeSamples(1000)
planner.setNumRegionExpansions(10)
planner.setNumTreeExpansions(5)
return planner
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.")
