KinematicCarPlanning.py
1 #!/usr/bin/env python
2 
3 
12 
13 # Author: Mark Moll
14 
15 import sys
16 from os.path import abspath, dirname, join
17 
18 ompl_app_root = dirname(dirname(dirname(abspath(__file__))))
19 
20 try:
21  from ompl import base as ob
22  from ompl import control as oc
23  from ompl import app as oa
24 except ImportError:
25  sys.path.insert(0, join(ompl_app_root, "ompl/py-bindings"))
26  from ompl import base as ob
27  from ompl import control as oc
28  from ompl import app as oa
29 
30 
31 def kinematicCarDemo():
32  setup = oa.KinematicCarPlanning()
33  # comment out next two lines if you want to ignore obstacles
34  setup.setRobotMesh("2D/car2_planar_robot.dae")
35  setup.setEnvironmentMesh("2D/Maze_planar_env.dae")
36  SE2 = setup.getStateSpace()
37 
38  bounds = ob.RealVectorBounds(2)
39  bounds.setLow(-55)
40  bounds.setHigh(55)
41  SE2.setBounds(bounds)
42 
43  # define start state
44  start = ob.State(SE2)
45  start().setX(0.01)
46  start().setY(-0.15)
47  start().setYaw(0)
48 
49  # define goal state
50  goal = ob.State(SE2)
51  goal().setX(45.01)
52  goal().setY(-0.15)
53  goal().setYaw(0.0)
54 
55  # set the start & goal states
56  setup.setStartAndGoalStates(start, goal, 1.0)
57 
58  # set the planner
59  planner = oc.RRT(setup.getSpaceInformation())
60  setup.setPlanner(planner)
61 
62  # try to solve the problem
63  print("\n\n***** Planning for a %s *****\n" % setup.getName())
64  print(setup)
65  if setup.solve(10):
66  # print the (approximate) solution path: print states along the path
67  # and controls required to get from one state to the next
68  path = setup.getSolutionPath()
69  path.interpolate()
70  # path.interpolate(); # uncomment if you want to plot the path
71  print(path.printAsMatrix())
72  if not setup.haveExactSolutionPath():
73  print(
74  "Solution is approximate. Distance to actual goal is %g"
75  % setup.getProblemDefinition().getSolutionDifference()
76  )
77 
78 
79 if __name__ == "__main__":
80  kinematicCarDemo()
Definition of an abstract state.
Definition: State.h:50
Rapidly-exploring Random Tree.
Definition: RRT.h:66
The lower and upper bounds for an Rn space.