demos/RigidBodyPlanningWithControls.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 00040 from functools import partial 00041 try: 00042 from ompl import util as ou 00043 from ompl import base as ob 00044 from ompl import control as oc 00045 from ompl import geometric as og 00046 except: 00047 # if the ompl module is not in the PYTHONPATH assume it is installed in a 00048 # subdirectory of the parent directory called "py-bindings." 00049 from os.path import abspath, dirname, join 00050 import sys 00051 sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings')) 00052 from ompl import util as ou 00053 from ompl import base as ob 00054 from ompl import control as oc 00055 from ompl import geometric as og 00056 00057 ## @cond IGNORE 00058 # a decomposition is only needed for SyclopRRT and SyclopEST 00059 class MyDecomposition(oc.GridDecomposition): 00060 def __init__(self, length, bounds): 00061 super(MyDecomposition, self).__init__(length, 2, bounds) 00062 def project(self, s, coord): 00063 coord[0] = s.getX() 00064 coord[1] = s.getY() 00065 def sampleFullState(self, sampler, coord, s): 00066 sampler.sampleUniform(s) 00067 s.setXY(coord[0], coord[1]) 00068 ## @endcond 00069 00070 def isStateValid(spaceInformation, state): 00071 # perform collision checking or check if other constraints are 00072 # satisfied 00073 return spaceInformation.satisfiesBounds(state) 00074 00075 def propagate(start, control, duration, state): 00076 state.setX( start.getX() + control[0] * duration * cos(start.getYaw()) ) 00077 state.setY( start.getY() + control[0] * duration * sin(start.getYaw()) ) 00078 state.setYaw(start.getYaw() + control[1] * duration) 00079 00080 def plan(): 00081 # construct the state space we are planning in 00082 space = ob.SE2StateSpace() 00083 00084 # set the bounds for the R^2 part of SE(2) 00085 bounds = ob.RealVectorBounds(2) 00086 bounds.setLow(-1) 00087 bounds.setHigh(1) 00088 space.setBounds(bounds) 00089 00090 # create a control space 00091 cspace = oc.RealVectorControlSpace(space, 2) 00092 00093 # set the bounds for the control space 00094 cbounds = ob.RealVectorBounds(2) 00095 cbounds.setLow(-.3) 00096 cbounds.setHigh(.3) 00097 cspace.setBounds(cbounds) 00098 00099 # define a simple setup class 00100 ss = oc.SimpleSetup(cspace) 00101 ss.setStateValidityChecker(ob.StateValidityCheckerFn(partial(isStateValid, ss.getSpaceInformation()))) 00102 ss.setStatePropagator(oc.StatePropagatorFn(propagate)) 00103 00104 # create a start state 00105 start = ob.State(space) 00106 start().setX(-0.5); 00107 start().setY(0.0); 00108 start().setYaw(0.0); 00109 00110 # create a goal state 00111 goal = ob.State(space); 00112 goal().setX(0.0); 00113 goal().setY(0.5); 00114 goal().setYaw(0.0); 00115 00116 # set the start and goal states 00117 ss.setStartAndGoalStates(start, goal, 0.05) 00118 00119 # (optionally) set planner 00120 si = ss.getSpaceInformation() 00121 #planner = oc.RRT(si) 00122 #planner = oc.EST(si) 00123 #planner = oc.KPIECE1(si) # this is the default 00124 # SyclopEST and SyclopRRT require a decomposition to guide the search 00125 decomp = MyDecomposition(32, bounds) 00126 planner = oc.SyclopEST(si, decomp) 00127 #planner = oc.SyclopRRT(si, decomp) 00128 ss.setPlanner(planner) 00129 # (optionally) set propagation step size 00130 si.setPropagationStepSize(.1) 00131 00132 # attempt to solve the problem 00133 solved = ss.solve(20.0) 00134 00135 if solved: 00136 # print the path to screen 00137 print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix()) 00138 00139 if __name__ == "__main__": 00140 plan()