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):
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.SyclopRRT(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.")