demos/RigidBodyPlanning.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 try: 00040 from ompl import base as ob 00041 from ompl import geometric as og 00042 except: 00043 # if the ompl module is not in the PYTHONPATH assume it is installed in a 00044 # subdirectory of the parent directory called "py-bindings." 00045 from os.path import abspath, dirname, join 00046 import sys 00047 sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings')) 00048 from ompl import util as ou 00049 from ompl import base as ob 00050 from ompl import geometric as og 00051 00052 def isStateValid(state): 00053 # Some arbitrary condition on the state (note that thanks to 00054 # dynamic type checking we can just call getX() and do not need 00055 # to convert state to an SE2State.) 00056 return state.getX() < .6 00057 00058 def planWithSimpleSetup(): 00059 # create an SE2 state space 00060 space = ob.SE2StateSpace() 00061 00062 # set lower and upper bounds 00063 bounds = ob.RealVectorBounds(2) 00064 bounds.setLow(-1) 00065 bounds.setHigh(1) 00066 space.setBounds(bounds) 00067 00068 # create a simple setup object 00069 ss = og.SimpleSetup(space) 00070 ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) 00071 00072 start = ob.State(space) 00073 # we can pick a random start state... 00074 start.random() 00075 # ... or set specific values 00076 start().setX(.5) 00077 00078 goal = ob.State(space) 00079 # we can pick a random goal state... 00080 goal.random() 00081 # ... or set specific values 00082 goal().setX(-.5) 00083 00084 ss.setStartAndGoalStates(start, goal) 00085 00086 # this will automatically choose a default planner with 00087 # default parameters 00088 solved = ss.solve(1.0) 00089 00090 if solved: 00091 # try to shorten the path 00092 ss.simplifySolution() 00093 # print the simplified path 00094 print(ss.getSolutionPath()) 00095 00096 00097 def planTheHardWay(): 00098 # create an SE2 state space 00099 space = ob.SE2StateSpace() 00100 # set lower and upper bounds 00101 bounds = ob.RealVectorBounds(2) 00102 bounds.setLow(-1) 00103 bounds.setHigh(1) 00104 space.setBounds(bounds) 00105 # construct an instance of space information from this state space 00106 si = ob.SpaceInformation(space) 00107 # set state validity checking for this space 00108 si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) 00109 # create a random start state 00110 start = ob.State(space) 00111 start.random() 00112 # create a random goal state 00113 goal = ob.State(space) 00114 goal.random() 00115 # create a problem instance 00116 pdef = ob.ProblemDefinition(si) 00117 # set the start and goal states 00118 pdef.setStartAndGoalStates(start, goal) 00119 # create a planner for the defined space 00120 planner = og.RRTConnect(si) 00121 # set the problem we are trying to solve for the planner 00122 planner.setProblemDefinition(pdef) 00123 # perform setup steps for the planner 00124 planner.setup() 00125 # print the settings for this space 00126 print(si.settings()) 00127 # print the problem settings 00128 print(pdef) 00129 # attempt to solve the problem within one second of planning time 00130 solved = planner.solve(1.0) 00131 00132 if solved: 00133 # get the goal representation from the problem definition (not the same as the goal state) 00134 # and inquire about the found path 00135 path = pdef.getSolutionPath() 00136 print("Found solution:\n%s" % path) 00137 else: 00138 print("No solution found") 00139 00140 00141 if __name__ == "__main__": 00142 planWithSimpleSetup() 00143 print("") 00144 planTheHardWay()