RigidBodyPlanningWithODESolverAndControls.py
1 #!/usr/bin/env python
2 
3 
36 
37 # Author: Mark Moll
38 
39 from math import sin, cos, tan
40 from functools import partial
41 try:
42  from ompl import base as ob
43  from ompl import control as oc
44 except ImportError:
45  # if the ompl module is not in the PYTHONPATH assume it is installed in a
46  # subdirectory of the parent directory called "py-bindings."
47  from os.path import abspath, dirname, join
48  import sys
49  sys.path.insert(0, join(dirname(dirname(abspath(__file__))), 'py-bindings'))
50  from ompl import base as ob
51  from ompl import geometric as og # needed for asGeometric()
52  from ompl import control as oc
53 
54 def kinematicCarODE(q, u, qdot):
55  theta = q[2]
56  carLength = 0.2
57  qdot[0] = u[0] * cos(theta)
58  qdot[1] = u[0] * sin(theta)
59  qdot[2] = u[0] * tan(u[1]) / carLength
60 
61 
62 def isStateValid(spaceInformation, state):
63  # perform collision checking or check if other constraints are
64  # satisfied
65  return spaceInformation.satisfiesBounds(state)
66 
67 def plan():
68  # construct the state space we are planning in
69  space = ob.SE2StateSpace()
70 
71  # set the bounds for the R^2 part of SE(2)
72  bounds = ob.RealVectorBounds(2)
73  bounds.setLow(-1)
74  bounds.setHigh(1)
75  space.setBounds(bounds)
76 
77  # create a control space
78  cspace = oc.RealVectorControlSpace(space, 2)
79 
80  # set the bounds for the control space
81  cbounds = ob.RealVectorBounds(2)
82  cbounds.setLow(-.3)
83  cbounds.setHigh(.3)
84  cspace.setBounds(cbounds)
85 
86  # define a simple setup class
87  ss = oc.SimpleSetup(cspace)
88  validityChecker = ob.StateValidityCheckerFn(partial(isStateValid, ss.getSpaceInformation()))
89  ss.setStateValidityChecker(validityChecker)
90  ode = oc.ODE(kinematicCarODE)
91  odeSolver = oc.ODEBasicSolver(ss.getSpaceInformation(), ode)
92  propagator = oc.ODESolver.getStatePropagator(odeSolver)
93  ss.setStatePropagator(propagator)
94 
95  # create a start state
96  start = ob.State(space)
97  start().setX(-0.5)
98  start().setY(0.0)
99  start().setYaw(0.0)
100 
101  # create a goal state
102  goal = ob.State(space)
103  goal().setX(0.0)
104  goal().setY(0.5)
105  goal().setYaw(0.0)
106 
107  # set the start and goal states
108  ss.setStartAndGoalStates(start, goal, 0.05)
109 
110  # attempt to solve the problem
111  solved = ss.solve(120.0)
112 
113  if solved:
114  # print the path to screen
115  print("Found solution:\n%s" % ss.getSolutionPath().asGeometric().printAsMatrix())
116 
117 if __name__ == "__main__":
118  plan()
Definition of an abstract state.
Definition: State.h:114
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:127
A state space representing SE(2)
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...
A control space representing Rn.
Basic solver for ordinary differential equations of the type q' = f(q, u), where q is the current sta...
Definition: ODESolver.h:264
The lower and upper bounds for an Rn space.