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 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 control as oc
24  from ompl import app as oa
25 except ImportError:
26  sys.path.insert(0, join(ompl_app_root, 'ompl/py-bindings'))
27  from ompl import base as ob
28  from ompl import control as oc
29  from ompl import app as oa
30 
31 setup = oa.KinematicCarPlanning()
32 SE2 = setup.getStateSpace()
33 
34 bounds = ob.RealVectorBounds(2)
35 bounds.setLow(-10)
36 bounds.setHigh(10)
37 SE2.setBounds(bounds)
38 
39 # define start state
40 start = ob.State(SE2)
41 start().setX(0)
42 start().setY(0)
43 start().setYaw(0)
44 
45 # define goal state
46 goal = ob.State(SE2)
47 goal().setX(2)
48 goal().setY(2)
49 goal().setYaw(pi)
50 
51 # set the start & goal states
52 setup.setStartAndGoalStates(start, goal, .1)
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(20):
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())
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