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 control as oc
52 
53 def kinematicCarODE(q, u, qdot):
54  theta = q[2]
55  carLength = 0.2
56  qdot[0] = u[0] * cos(theta)
57  qdot[1] = u[0] * sin(theta)
58  qdot[2] = u[0] * tan(u[1]) / carLength
59 
60 
61 def isStateValid(spaceInformation, state):
62  # perform collision checking or check if other constraints are
63  # satisfied
64  return spaceInformation.satisfiesBounds(state)
65 
66 def plan():
67  # construct the state space we are planning in
68  space = ob.SE2StateSpace()
69 
70  # set the bounds for the R^2 part of SE(2)
71  bounds = ob.RealVectorBounds(2)
72  bounds.setLow(-1)
73  bounds.setHigh(1)
74  space.setBounds(bounds)
75 
76  # create a control space
77  cspace = oc.RealVectorControlSpace(space, 2)
78 
79  # set the bounds for the control space
80  cbounds = ob.RealVectorBounds(2)
81  cbounds.setLow(-.3)
82  cbounds.setHigh(.3)
83  cspace.setBounds(cbounds)
84 
85  # define a simple setup class
86  ss = oc.SimpleSetup(cspace)
87  validityChecker = ob.StateValidityCheckerFn(partial(isStateValid, ss.getSpaceInformation()))
88  ss.setStateValidityChecker(validityChecker)
89  ode = oc.ODE(kinematicCarODE)
90  odeSolver = oc.ODEBasicSolver(ss.getSpaceInformation(), ode)
91  propagator = oc.ODESolver.getStatePropagator(odeSolver)
92  ss.setStatePropagator(propagator)
93 
94  # create a start state
95  start = ob.State(space)
96  start().setX(-0.5)
97  start().setY(0.0)
98  start().setYaw(0.0)
99 
100  # create a goal state
101  goal = ob.State(space)
102  goal().setX(0.0)
103  goal().setY(0.5)
104  goal().setYaw(0.0)
105 
106  # set the start and goal states
107  ss.setStartAndGoalStates(start, goal, 0.05)
108 
109  # attempt to solve the problem
110  solved = ss.solve(120.0)
111 
112  if solved:
113  # print the path to screen
114  print("Found solution:\n%s" % ss.getSolutionPath().asGeometric().printAsMatrix())
115 
116 if __name__ == "__main__":
117  plan()
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:63
A control space representing Rn.
A state space representing SE(2)
Definition: SE2StateSpace.h:49
Definition of an abstract state.
Definition: State.h:49
Basic solver for ordinary differential equations of the type q' = f(q, u), where q is the current sta...
Definition: ODESolver.h:195
The lower and upper bounds for an Rn space.
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...