DynamicCarPlanning.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 def dynamicCarDemo(setup):
31  print("\n\n***** Planning for a %s *****\n" % setup.getName())
32  # plan for dynamic car in SE(2)
33  stateSpace = setup.getStateSpace()
34 
35  # set the bounds for the R^2 part of SE(2)
36  bounds = ob.RealVectorBounds(2)
37  bounds.setLow(-10)
38  bounds.setHigh(10)
39  stateSpace.getSubspace(0).setBounds(bounds)
40 
41  # define start state
42  start = ob.State(stateSpace)
43  start[0] = start[1] = start[2] = start[3] = start[4] = 0.
44 
45  # define goal state
46  goal = ob.State(stateSpace)
47  goal[0] = goal[1] = 8.
48  goal[2] = 0
49  goal[3] = goal[4] = 0.
50 
51  # set the start & goal states
52  setup.setStartAndGoalStates(start, goal, .5)
53 
54  # set the planner
55  planner = oc.RRT(setup.getSpaceInformation())
56  setup.setPlanner(planner)
57 
58  # try to solve the problem
59  if setup.solve(40):
60  # print the (approximate) solution path: print states along the path
61  # and controls required to get from one state to the next
62  path = setup.getSolutionPath()
63  #path.interpolate(); # uncomment if you want to plot the path
64  print(path.printAsMatrix())
65  if not setup.haveExactSolutionPath():
66  print("Solution is approximate. Distance to actual goal is %g" %
67  setup.getProblemDefinition().getSolutionDifference())
68 
69 if __name__ == '__main__':
70  car = oa.DynamicCarPlanning()
71  dynamicCarDemo(car)
Definition of an abstract state.
Definition: State.h:49
The lower and upper bounds for an Rn space.
Rapidly-exploring Random Tree.
Definition: RRT.h:65