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 util as ou
43  from ompl import base as ob
44  from ompl import control as oc
45  from ompl import geometric as og
46 except:
47  # if the ompl module is not in the PYTHONPATH assume it is installed in a
48  # subdirectory of the parent directory called "py-bindings."
49  from os.path import abspath, dirname, join
50  import sys
51  sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings'))
52  from ompl import util as ou
53  from ompl import base as ob
54  from ompl import control as oc
55  from ompl import geometric as og
56 
57 
59 class MyDecomposition(oc.GridDecomposition):
60  def __init__(self, length, bounds):
61  super(MyDecomposition, self).__init__(length, 2, bounds)
62  def project(self, s, coord):
63  coord[0] = s.getX()
64  coord[1] = s.getY()
65  def sampleFullState(self, sampler, coord, s):
66  sampler.sampleUniform(s)
67  s.setXY(coord[0], coord[1])
68 
69 
70 def isStateValid(spaceInformation, state):
71  # perform collision checking or check if other constraints are
72  # satisfied
73  return spaceInformation.satisfiesBounds(state)
74 
75 def propagate(start, control, duration, state):
76  state.setX( start.getX() + control[0] * duration * cos(start.getYaw()) )
77  state.setY( start.getY() + control[0] * duration * sin(start.getYaw()) )
78  state.setYaw(start.getYaw() + control[1] * duration)
79 
80 def plan():
81  # construct the state space we are planning in
82  space = ob.SE2StateSpace()
83 
84  # set the bounds for the R^2 part of SE(2)
85  bounds = ob.RealVectorBounds(2)
86  bounds.setLow(-1)
87  bounds.setHigh(1)
88  space.setBounds(bounds)
89 
90  # create a control space
91  cspace = oc.RealVectorControlSpace(space, 2)
92 
93  # set the bounds for the control space
94  cbounds = ob.RealVectorBounds(2)
95  cbounds.setLow(-.3)
96  cbounds.setHigh(.3)
97  cspace.setBounds(cbounds)
98 
99  # define a simple setup class
100  ss = oc.SimpleSetup(cspace)
101  ss.setStateValidityChecker(ob.StateValidityCheckerFn(partial(isStateValid, ss.getSpaceInformation())))
102  ss.setStatePropagator(oc.StatePropagatorFn(propagate))
103 
104  # create a start state
105  start = ob.State(space)
106  start().setX(-0.5);
107  start().setY(0.0);
108  start().setYaw(0.0);
109 
110  # create a goal state
111  goal = ob.State(space);
112  goal().setX(0.0);
113  goal().setY(0.5);
114  goal().setYaw(0.0);
115 
116  # set the start and goal states
117  ss.setStartAndGoalStates(start, goal, 0.05)
118 
119  # (optionally) set planner
120  si = ss.getSpaceInformation()
121  #planner = oc.RRT(si)
122  #planner = oc.EST(si)
123  #planner = oc.KPIECE1(si) # this is the default
124  # SyclopEST and SyclopRRT require a decomposition to guide the search
125  decomp = MyDecomposition(32, bounds)
126  planner = oc.SyclopEST(si, decomp)
127  #planner = oc.SyclopRRT(si, decomp)
128  ss.setPlanner(planner)
129  # (optionally) set propagation step size
130  si.setPropagationStepSize(.1)
131 
132  # attempt to solve the problem
133  solved = ss.solve(20.0)
134 
135  if solved:
136  # print the path to screen
137  print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
138 
139 if __name__ == "__main__":
140  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...