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) validityChecker = ob.StateValidityCheckerFn( partial(isStateValid, ss.getSpaceInformation())) ss.setStateValidityChecker(validityChecker) ode = oc.ODE(kinematicCarODE) odeSolver = oc.ODEBasicSolver(ss.getSpaceInformation(), ode) propagator = oc.ODESolver.getStatePropagator(odeSolver) ss.setStatePropagator(propagator) # 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) # attempt to solve the problem solved = ss.solve(120.0) if solved: # print the path to screen print("Found solution:\n%s" % ss.getSolutionPath().asGeometric().printAsMatrix())
def setSpace(self): self.space = ob.SE2StateSpace() # set the bounds for the R^2 part of SE(2) self.bounds = ob.RealVectorBounds(2) if not self.costMap: self.bounds.setLow(-8) self.bounds.setHigh(20) else: ox = self.costMap.getOriginX() oy = self.costMap.getOriginY() size_x = self.costMap.getSizeInMetersX() size_y = self.costMap.getSizeInMetersY() low = min(ox, oy) high = max(ox + size_x, oy + size_y) print("low", low) print("high", high) self.bounds.setLow(low) self.bounds.setHigh(high) self.space.setBounds(self.bounds) # create a control space self.cspace = oc.RealVectorControlSpace(self.space, 2) # set the bounds for the control space cbounds = ob.RealVectorBounds(2) cbounds.setLow(0, -1.5) cbounds.setHigh(0, 1.5) cbounds.setLow(1, -3) cbounds.setHigh(1, 3) self.cspace.setBounds(cbounds) # define a simple setup class self.ss = oc.SimpleSetup(self.cspace) validityChecker = ob.StateValidityCheckerFn( partial(self.isStateValid, self.ss.getSpaceInformation())) self.ss.setStateValidityChecker(validityChecker) ode = oc.ODE(self.kinematicCarODE) odeSolver = oc.ODEBasicSolver(self.ss.getSpaceInformation(), ode) propagator = oc.ODESolver.getStatePropagator(odeSolver) self.ss.setStatePropagator(propagator)