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 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 setup = oa.KinematicCarPlanning()
34 SE2 = setup.getStateSpace()
35 
36 bounds = ob.RealVectorBounds(2)
37 bounds.setLow(-10)
38 bounds.setHigh(10)
39 SE2.setBounds(bounds)
40 
41 # define start state
42 start = ob.State(SE2)
43 start().setX(0)
44 start().setY(0)
45 start().setYaw(0)
46 
47 # define goal state
48 goal = ob.State(SE2)
49 goal().setX(2)
50 goal().setY(2)
51 goal().setYaw(pi)
52 
53 # set the start & goal states
54 setup.setStartAndGoalStates(start, goal, .1)
55 
56 # set the planner
57 planner = oc.RRT(setup.getSpaceInformation())
58 setup.setPlanner(planner)
59 
60 # try to solve the problem
61 if setup.solve(20):
62  # print the (approximate) solution path: print states along the path
63  # and controls required to get from one state to the next
64  path = setup.getSolutionPath()
65  #path.interpolate(); # uncomment if you want to plot the path
66  print(path.printAsMatrix())
67  if not setup.haveExactSolutionPath():
68  print("Solution is approximate. Distance to actual goal is %g" %
69  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