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.")