def plan():
# construct the state space we are planning in
ss = app.KinematicCarPlanning()
ss.setEnvironmentMesh('./mesh/UniqueSolutionMaze_env.dae')
ss.setRobotMesh('./mesh/car2_planar_robot.dae')
# space = ob.SE2StateSpace()
# create a start state
start = ob.State(ss.getSpaceInformation())
start().setX(3)
start().setY(-3)
start().setYaw(0.0)
# create a goal state
goal = ob.State(ss.getSpaceInformation())
goal().setX(45)
goal().setY(25)
goal().setYaw(0.0)
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# 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 __init__(self):
"""
Create a new Kinematic Car Planning Demo.
"""
self.car = oa.KinematicCarPlanning()
self.state_space = self.car.getStateSpace()
self.setBounds()
self.setStart()
self.setGoal()
def plan():
# construct the state space we are planning in
ss = app.KinematicCarPlanning()
ss.setEnvironmentMesh('./mesh/UniqueSolutionMaze_env.dae')
ss.setRobotMesh('./mesh/car2_planar_robot.dae')
# space = ob.SE2StateSpace()
cspace = ss.getControlSpace()
# 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)
# create a start state
start = ob.State(ss.getSpaceInformation())
start().setX(3)
start().setY(-3)
start().setYaw(0.0)
# create a goal state
goal = ob.State(ss.getSpaceInformation())
goal().setX(45)
goal().setY(25)
goal().setYaw(0.0)
#si = ss.getSpaceInformation()
information = ss.getSpaceInformation()
information.setStatePropagator(oc.StatePropagatorFn(propagate))
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# 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 plan(options, params):
print(params)
""" Entry point for the planning """
# construct the state space we are planning in
ss = app.KinematicCarPlanning()
ss.setEnvironmentMesh(params["robot_mesh"])
ss.setRobotMesh(params["env_mesh"])
# setting the real vector state space bounds
se2bounds = ob.RealVectorBounds(2)
se2bounds.setLow(params["lower_bound"])
se2bounds.setHigh(params["upper_bound"])
ss.getStateSpace().setBounds(se2bounds)
# set the bounds for the control space
cspace = ss.getControlSpace()
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(0, -3.0)
cbounds.setHigh(0, 3.0)
cbounds.setLow(1, params["lower_bound_cspace"])
cbounds.setHigh(1, 3.0)
cspace.setBounds(cbounds)
# create a start state
start = ob.State(ss.getSpaceInformation())
start().setX(params["start_x"])
start().setY(params["start_y"])
start().setYaw(0.0)
# create a goal state
goal = ob.State(ss.getSpaceInformation())
goal().setX(params["goal_x"])
goal().setY(params["goal_y"])
goal().setYaw(0.0)
# space information
si = ss.getSpaceInformation()
si.setStatePropagator(oc.StatePropagatorFn(propagate))
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
if not options.bench:
planOnce(ss)
else:
benchmark(ss)
def kinematicCarDemo():
setup = oa.KinematicCarPlanning()
# comment out next two lines if you want to ignore obstacles
setup.setRobotMesh("2D/car2_planar_robot.dae")
setup.setEnvironmentMesh("2D/Maze_planar_env.dae")
SE2 = setup.getStateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(-55)
bounds.setHigh(55)
SE2.setBounds(bounds)
# define start state
start = ob.State(SE2)
start().setX(0.01)
start().setY(-0.15)
start().setYaw(0)
# define goal state
goal = ob.State(SE2)
goal().setX(45.01)
goal().setY(-0.15)
goal().setYaw(0.0)
# set the start & goal states
setup.setStartAndGoalStates(start, goal, 1.0)
# set the planner
planner = oc.RRT(setup.getSpaceInformation())
setup.setPlanner(planner)
# try to solve the problem
print("\n\n***** Planning for a %s *****\n" % setup.getName())
print(setup)
if setup.solve(10):
# 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()
# 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())
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())
if __name__ == '__main__':
car = oa.KinematicCarPlanning()
kinematicCarDemo(car)
ompl_app_root = dirname(dirname(dirname(abspath(__file__))))
try:
from ompl import base as ob
from ompl import geometric as og
from ompl import control as oc
from ompl import app as oa
except:
sys.path.insert(0, join(ompl_app_root, 'ompl/py-bindings'))
from ompl import base as ob
from ompl import geometric as og
from ompl import control as oc
from ompl import app as oa
setup = oa.KinematicCarPlanning()
SE2 = setup.getStateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(-10)
bounds.setHigh(10)
SE2.setBounds(bounds)
# define start state
start = ob.State(SE2)
start().setX(0)
start().setY(0)
start().setYaw(0)
# define goal state
goal = ob.State(SE2)