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)