RigidBodyPlanningWithControls.py
1 #!/usr/bin/env python
2 
3 
36 
37 # Author: Mark Moll
38 
39 from math import sin, cos
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 
55 class MyDecomposition(oc.GridDecomposition):
56  def __init__(self, length, bounds):
57  super(MyDecomposition, self).__init__(length, 2, bounds)
58  def project(self, s, coord):
59  coord[0] = s.getX()
60  coord[1] = s.getY()
61  def sampleFullState(self, sampler, coord, s):
62  sampler.sampleUniform(s)
63  s.setXY(coord[0], coord[1])
64 
65 
66 def isStateValid(spaceInformation, state):
67  # perform collision checking or check if other constraints are
68  # satisfied
69  return spaceInformation.satisfiesBounds(state)
70 
71 def propagate(start, control, duration, state):
72  state.setX(start.getX() + control[0] * duration * cos(start.getYaw()))
73  state.setY(start.getY() + control[0] * duration * sin(start.getYaw()))
74  state.setYaw(start.getYaw() + control[1] * duration)
75 
76 def plan():
77  # construct the state space we are planning in
78  space = ob.SE2StateSpace()
79 
80  # set the bounds for the R^2 part of SE(2)
81  bounds = ob.RealVectorBounds(2)
82  bounds.setLow(-1)
83  bounds.setHigh(1)
84  space.setBounds(bounds)
85 
86  # create a control space
87  cspace = oc.RealVectorControlSpace(space, 2)
88 
89  # set the bounds for the control space
90  cbounds = ob.RealVectorBounds(2)
91  cbounds.setLow(-.3)
92  cbounds.setHigh(.3)
93  cspace.setBounds(cbounds)
94 
95  # define a simple setup class
96  ss = oc.SimpleSetup(cspace)
97  ss.setStateValidityChecker(ob.StateValidityCheckerFn( \
98  partial(isStateValid, ss.getSpaceInformation())))
99  ss.setStatePropagator(oc.StatePropagatorFn(propagate))
100 
101  # create a start state
102  start = ob.State(space)
103  start().setX(-0.5)
104  start().setY(0.0)
105  start().setYaw(0.0)
106 
107  # create a goal state
108  goal = ob.State(space)
109  goal().setX(0.0)
110  goal().setY(0.5)
111  goal().setYaw(0.0)
112 
113  # set the start and goal states
114  ss.setStartAndGoalStates(start, goal, 0.05)
115 
116  # (optionally) set planner
117  si = ss.getSpaceInformation()
118  #planner = oc.RRT(si)
119  #planner = oc.EST(si)
120  #planner = oc.KPIECE1(si) # this is the default
121  # SyclopEST and SyclopRRT require a decomposition to guide the search
122  decomp = MyDecomposition(32, bounds)
123  planner = oc.SyclopEST(si, decomp)
124  #planner = oc.SyclopRRT(si, decomp)
125  ss.setPlanner(planner)
126  # (optionally) set propagation step size
127  si.setPropagationStepSize(.1)
128 
129  # attempt to solve the problem
130  solved = ss.solve(20.0)
131 
132  if solved:
133  # print the path to screen
134  print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
135 
136 if __name__ == "__main__":
137  plan()
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:63
A GridDecomposition is a Decomposition implemented using a grid.
std::function< void(const base::State *, const Control *, const double, base::State *)> StatePropagatorFn
A function that achieves state propagation.
A control space representing Rn.
A state space representing SE(2)
Definition: SE2StateSpace.h:49
SyclopEST is Syclop with EST as its low-level tree planner.
Definition: SyclopEST.h:51
Definition of an abstract state.
Definition: State.h:49
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...