All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
RigidBodyPlanningWithControls.py
1 #!/usr/bin/env python
2 
3 ######################################################################
4 # Software License Agreement (BSD License)
5 #
6 # Copyright (c) 2010, Rice University
7 # All rights reserved.
8 #
9 # Redistribution and use in source and binary forms, with or without
10 # modification, are permitted provided that the following conditions
11 # are met:
12 #
13 # * Redistributions of source code must retain the above copyright
14 # notice, this list of conditions and the following disclaimer.
15 # * Redistributions in binary form must reproduce the above
16 # copyright notice, this list of conditions and the following
17 # disclaimer in the documentation and/or other materials provided
18 # with the distribution.
19 # * Neither the name of the Rice University nor the names of its
20 # contributors may be used to endorse or promote products derived
21 # from this software without specific prior written permission.
22 #
23 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 # POSSIBILITY OF SUCH DAMAGE.
35 ######################################################################
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 ## @cond IGNORE
58 # a decomposition is only needed for SyclopRRT and SyclopEST
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 ## @endcond
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()
boost::function< void(const base::State *, const Control *, const double, base::State *)> StatePropagatorFn
A function that achieves state propagation.
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.
boost::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a boost function can be spec...
A control space representing Rn.
A state space representing SE(2)
Definition: SE2StateSpace.h:50
SyclopEST is Syclop with EST as its low-level tree planner.
Definition: SyclopEST.h:51
Definition of an abstract state.
Definition: State.h:50
The lower and upper bounds for an Rn space.