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