def newplanner(self, si):
planner = oc.RRT(si)
return planner
def setRRTPlanner(self, goal_bias=0.05):
self.planner = oc.RRT(self.car.getSpaceInformation())
self.planner.setGoalBias(goal_bias)
self.car.setPlanner(self.planner)
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)
start_goal_pairs = get_fixed_start_goal_pairs(bounds)
starts, goals = [], []
for i, (s, g) in enumerate(start_goal_pairs):
if i == 100:
# create a start state
start = ob.State(space)
start().setX(s[0])
start().setY(s[1])
start().setYaw(0.0)
# create a goal state
goal = ob.State(space)
goal().setX(g[0])
goal().setY(g[1])
goal().setYaw(0.0)
print("low[0], high[0]", bounds.low[0], bounds.high[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("start: ", start, "goal: ", goal)
print("Found solution:\n%s" %
ss.getSolutionPath().printAsMatrix())
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(-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, 0.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())))
ss.setStatePropagator(oc.StatePropagatorFn(propagate))
# create a start state
start = ob.State(space)
start().setX(0.0)
start().setY(0.0)
start().setYaw(0.0)
# create a goal state
goal = ob.State(space)
goal().setX(24.0)
goal().setY(6.0)
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(40.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())
# define start state
start = ob.State(SE2)
start().setX(0)
start().setY(0)
start().setYaw(0)
# define goal state
goal = ob.State(SE2)
goal().setX(2)
goal().setY(2)
goal().setYaw(pi)
# set the start & goal states
setup.setStartAndGoalStates(start, goal, .1)
# set the planner
planner = oc.RRT(setup.getSpaceInformation())
setup.setPlanner(planner)
# try to solve the problem
if setup.solve(20):
# print the (approximate) solution path: print states along the path
# and controls required to get from one state to the next
path = setup.getSolutionPath()
#path.interpolate(); # uncomment if you want to plot the path
print(path.printAsMatrix())
if not setup.haveExactSolutionPath():
print("Solution is approximate. Distance to actual goal is %g" %
setup.getProblemDefinition().getSolutionDifference())