demos/RigidBodyPlanningWithODESolverAndControls.py
00001 #!/usr/bin/env python 00002 00003 ###################################################################### 00004 # Software License Agreement (BSD License) 00005 # 00006 # Copyright (c) 2010, Rice University 00007 # All rights reserved. 00008 # 00009 # Redistribution and use in source and binary forms, with or without 00010 # modification, are permitted provided that the following conditions 00011 # are met: 00012 # 00013 # * Redistributions of source code must retain the above copyright 00014 # notice, this list of conditions and the following disclaimer. 00015 # * Redistributions in binary form must reproduce the above 00016 # copyright notice, this list of conditions and the following 00017 # disclaimer in the documentation and/or other materials provided 00018 # with the distribution. 00019 # * Neither the name of the Rice University nor the names of its 00020 # contributors may be used to endorse or promote products derived 00021 # from this software without specific prior written permission. 00022 # 00023 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 # POSSIBILITY OF SUCH DAMAGE. 00035 ###################################################################### 00036 00037 # Author: Mark Moll 00038 00039 from math import sin, cos, tan 00040 from functools import partial 00041 try: 00042 from ompl import base as ob 00043 from ompl import control as oc 00044 from ompl import geometric as og 00045 except: 00046 # if the ompl module is not in the PYTHONPATH assume it is installed in a 00047 # subdirectory of the parent directory called "py-bindings." 00048 from os.path import abspath, dirname, join 00049 import sys 00050 sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings')) 00051 from ompl import base as ob 00052 from ompl import control as oc 00053 from ompl import geometric as og 00054 00055 def kinematicCarODE(q, u, qdot): 00056 theta = q[2]; 00057 carLength = 0.2; 00058 qdot[0] = u[0] * cos(theta) 00059 qdot[1] = u[0] * sin(theta) 00060 qdot[2] = u[0] * tan(u[1]) / carLength 00061 00062 00063 def isStateValid(spaceInformation, state): 00064 # perform collision checking or check if other constraints are 00065 # satisfied 00066 return spaceInformation.satisfiesBounds(state) 00067 00068 def plan(): 00069 # construct the state space we are planning in 00070 space = ob.SE2StateSpace() 00071 00072 # set the bounds for the R^2 part of SE(2) 00073 bounds = ob.RealVectorBounds(2) 00074 bounds.setLow(-1) 00075 bounds.setHigh(1) 00076 space.setBounds(bounds) 00077 00078 # create a control space 00079 cspace = oc.RealVectorControlSpace(space, 2) 00080 00081 # set the bounds for the control space 00082 cbounds = ob.RealVectorBounds(2) 00083 cbounds.setLow(-.3) 00084 cbounds.setHigh(.3) 00085 cspace.setBounds(cbounds) 00086 00087 # define a simple setup class 00088 ss = oc.SimpleSetup(cspace) 00089 validityChecker = ob.StateValidityCheckerFn(partial(isStateValid, ss.getSpaceInformation())) 00090 ss.setStateValidityChecker(validityChecker) 00091 ode = oc.ODE(kinematicCarODE) 00092 odeSolver = oc.ODEBasicSolver(ss.getSpaceInformation(), ode) 00093 propagator = oc.ODESolver.getStatePropagator(odeSolver) 00094 ss.setStatePropagator(propagator) 00095 00096 # create a start state 00097 start = ob.State(space) 00098 start().setX(-0.5); 00099 start().setY(0.0); 00100 start().setYaw(0.0); 00101 00102 # create a goal state 00103 goal = ob.State(space); 00104 goal().setX(0.0); 00105 goal().setY(0.5); 00106 goal().setYaw(0.0); 00107 00108 # set the start and goal states 00109 ss.setStartAndGoalStates(start, goal, 0.05) 00110 00111 # attempt to solve the problem 00112 solved = ss.solve(120.0) 00113 00114 if solved: 00115 # print the path to screen 00116 print("Found solution:\n%s" % ss.getSolutionPath().asGeometric().printAsMatrix()) 00117 00118 if __name__ == "__main__": 00119 plan()