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 from math import pi
18 
19 ompl_app_root = dirname(dirname(dirname(abspath(__file__))))
20 
21 try:
22  from ompl import base as ob
23  from ompl import geometric as og
24  from ompl import control as oc
25  from ompl import app as oa
26 except:
27  sys.path.insert(0, join(ompl_app_root, 'ompl/py-bindings' ) )
28  from ompl import base as ob
29  from ompl import geometric as og
30  from ompl import control as oc
31  from ompl import app as oa
32 
33 def dynamicCarDemo(setup):
34  print("\n\n***** Planning for a %s *****\n" % setup.getName())
35  # plan for dynamic car in SE(2)
36  stateSpace = setup.getStateSpace()
37 
38  # set the bounds for the R^2 part of SE(2)
39  bounds = ob.RealVectorBounds(2)
40  bounds.setLow(-10)
41  bounds.setHigh(10)
42  stateSpace.getSubspace(0).setBounds(bounds)
43 
44  # define start state
45  start = ob.State(stateSpace)
46  start[0] = start[1] = start[2] = start[3] = start[4] = 0.
47 
48  # define goal state
49  goal = ob.State(stateSpace)
50  goal[0] = goal[1] = 8.
51  goal[2] = 0
52  goal[3] = goal[4] = 0.
53 
54  # set the start & goal states
55  setup.setStartAndGoalStates(start, goal, .5)
56 
57  # set the planner
58  planner = oc.RRT(setup.getSpaceInformation())
59  setup.setPlanner(planner)
60 
61  # try to solve the problem
62  if setup.solve(40):
63  # print the (approximate) solution path: print states along the path
64  # and controls required to get from one state to the next
65  path = setup.getSolutionPath()
66  #path.interpolate(); # uncomment if you want to plot the path
67  print(path.printAsMatrix())
68  if not setup.haveExactSolutionPath():
69  print("Solution is approximate. Distance to actual goal is %g" %
70  setup.getProblemDefinition().getSolutionDifference())
71 
72 if __name__ == '__main__':
73  car = oa.DynamicCarPlanning()
74  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