All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
RigidBodyPlanningWithODESolverAndControls.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, tan
40 from functools import partial
41 try:
42  from ompl import base as ob
43  from ompl import control as oc
44  from ompl import geometric as og
45 except:
46  # if the ompl module is not in the PYTHONPATH assume it is installed in a
47  # subdirectory of the parent directory called "py-bindings."
48  from os.path import basename, abspath, dirname, join
49  import sys
50  sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings'))
51  from ompl import base as ob
52  from ompl import control as oc
53  from ompl import geometric as og
54 
55 def kinematicCarODE(q, u, qdot):
56  theta = q[2];
57  carLength = 0.2;
58  qdot[0] = u[0] * cos(theta)
59  qdot[1] = u[0] * sin(theta)
60  qdot[2] = u[0] * tan(u[1]) / carLength
61 
62 
63 def isStateValid(spaceInformation, state):
64  # perform collision checking or check if other constraints are
65  # satisfied
66  return spaceInformation.satisfiesBounds(state)
67 
68 def postProp(control, state):
69  return
70 
71 def plan():
72  # construct the state space we are planning in
73  space = ob.SE2StateSpace()
74 
75  # set the bounds for the R^2 part of SE(2)
76  bounds = ob.RealVectorBounds(2)
77  bounds.setLow(-1)
78  bounds.setHigh(1)
79  space.setBounds(bounds)
80 
81  # create a control space
82  cspace = oc.RealVectorControlSpace(space, 2)
83 
84  # set the bounds for the control space
85  cbounds = ob.RealVectorBounds(2)
86  cbounds.setLow(-.3)
87  cbounds.setHigh(.3)
88  cspace.setBounds(cbounds)
89 
90  # define a simple setup class
91  ss = oc.SimpleSetup(cspace)
92  validityChecker = ob.StateValidityCheckerFn(partial(isStateValid, ss.getSpaceInformation()))
93  ss.setStateValidityChecker(validityChecker)
94  ode = oc.ODE(kinematicCarODE)
95  odeSolver = oc.ODEBasicSolver(ss.getSpaceInformation(), ode)
96  ss.setStatePropagator(odeSolver.getStatePropagator(oc.PostPropagationEvent(postProp)))
97 
98  # create a start state
99  start = ob.State(space)
100  start().setX(-0.5);
101  start().setY(0.0);
102  start().setYaw(0.0);
103 
104  # create a goal state
105  goal = ob.State(space);
106  goal().setX(0.0);
107  goal().setY(0.5);
108  goal().setYaw(0.0);
109 
110  # set the start and goal states
111  ss.setStartAndGoalStates(start, goal, 0.05)
112 
113  # attempt to solve the problem
114  solved = ss.solve(120.0)
115 
116  if solved:
117  # print the path to screen
118  print("Found solution:\n%s" % ss.getSolutionPath().asGeometric())
119 
120 if __name__ == "__main__":
121  plan()