demos/OptimalPlanning.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: Luis G. Torres, Mark Moll 00038 00039 try: 00040 from ompl import util as ou 00041 from ompl import base as ob 00042 from ompl import geometric as og 00043 except: 00044 # if the ompl module is not in the PYTHONPATH assume it is installed in a 00045 # subdirectory of the parent directory called "py-bindings." 00046 from os.path import abspath, dirname, join 00047 import sys 00048 sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings')) 00049 from ompl import util as ou 00050 from ompl import base as ob 00051 from ompl import geometric as og 00052 from math import sqrt 00053 from sys import argv 00054 00055 ## @cond IGNORE 00056 # Our "collision checker". For this demo, our robot's state space 00057 # lies in [0,1]x[0,1], with a circular obstacle of radius 0.25 00058 # centered at (0.5,0.5). Any states lying in this circular region are 00059 # considered "in collision". 00060 class ValidityChecker(ob.StateValidityChecker): 00061 def __init__(self, si): 00062 super(ValidityChecker, self).__init__(si) 00063 00064 # Returns whether the given state's position overlaps the 00065 # circular obstacle 00066 def isValid(self, state): 00067 return self.clearance(state) > 0.0 00068 00069 # Returns the distance from the given state's position to the 00070 # boundary of the circular obstacle. 00071 def clearance(self, state): 00072 # Extract the robot's (x,y) position from its state 00073 x = state[0] 00074 y = state[1] 00075 00076 # Distance formula between two points, offset by the circle's 00077 # radius 00078 return sqrt((x-0.5)*(x-0.5) + (y-0.5)*(y-0.5)) - 0.25 00079 00080 00081 ## Returns a structure representing the optimization objective to use 00082 # for optimal motion planning. This method returns an objective 00083 # which attempts to minimize the length in configuration space of 00084 # computed paths. 00085 def getPathLengthObjective(si): 00086 return ob.PathLengthOptimizationObjective(si) 00087 00088 ## Returns an optimization objective which attempts to minimize path 00089 # length that is satisfied when a path of length shorter than 1.51 00090 # is found. 00091 def getThresholdPathLengthObj(si): 00092 obj = ob.PathLengthOptimizationObjective(si) 00093 obj.setCostThreshold(ob.Cost(1.51)) 00094 return obj 00095 00096 ## Defines an optimization objective which attempts to steer the 00097 # robot away from obstacles. To formulate this objective as a 00098 # minimization of path cost, we can define the cost of a path as a 00099 # summation of the costs of each of the states along the path, where 00100 # each state cost is a function of that state's clearance from 00101 # obstacles. 00102 # 00103 # The class StateCostIntegralObjective represents objectives as 00104 # summations of state costs, just like we require. All we need to do 00105 # then is inherit from that base class and define our specific state 00106 # cost function by overriding the stateCost() method. 00107 # 00108 class ClearanceObjective(ob.StateCostIntegralObjective): 00109 def __init__(self, si): 00110 super(ClearanceObjective, self).__init__(si, True) 00111 self.si_ = si 00112 00113 # Our requirement is to maximize path clearance from obstacles, 00114 # but we want to represent the objective as a path cost 00115 # minimization. Therefore, we set each state's cost to be the 00116 # reciprocal of its clearance, so that as state clearance 00117 # increases, the state cost decreases. 00118 def stateCost(self, s): 00119 return ob.Cost(1 / self.si_.getStateValidityChecker().clearance(s)) 00120 00121 ## Return an optimization objective which attempts to steer the robot 00122 # away from obstacles. 00123 def getClearanceObjective(si): 00124 return ClearanceObjective(si) 00125 00126 ## Create an optimization objective which attempts to optimize both 00127 # path length and clearance. We do this by defining our individual 00128 # objectives, then adding them to a MultiOptimizationObjective 00129 # object. This results in an optimization objective where path cost 00130 # is equivalent to adding up each of the individual objectives' path 00131 # costs. 00132 # 00133 # When adding objectives, we can also optionally specify each 00134 # objective's weighting factor to signify how important it is in 00135 # optimal planning. If no weight is specified, the weight defaults to 00136 # 1.0. 00137 def getBalancedObjective1(si): 00138 lengthObj = ob.PathLengthOptimizationObjective(si) 00139 clearObj = ClearanceObjective(si) 00140 00141 opt = ob.MultiOptimizationObjective(si) 00142 opt.addObjective(lengthObj, 5.0) 00143 opt.addObjective(clearObj, 1.0) 00144 00145 return opt 00146 00147 ## Create an optimization objective equivalent to the one returned by 00148 # getBalancedObjective1(), but use an alternate syntax. 00149 # THIS DOESN'T WORK YET. THE OPERATORS SOMEHOW AREN'T EXPORTED BY Py++. 00150 # def getBalancedObjective2(si): 00151 # lengthObj = ob.PathLengthOptimizationObjective(si) 00152 # clearObj = ClearanceObjective(si) 00153 # 00154 # return 5.0*lengthObj + clearObj 00155 00156 00157 ## Create an optimization objective for minimizing path length, and 00158 # specify a cost-to-go heuristic suitable for this optimal planning 00159 # problem. 00160 def getPathLengthObjWithCostToGo(si): 00161 obj = ob.PathLengthOptimizationObjective(si) 00162 obj.setCostToGoHeuristic(ob.CostToGoHeuristic(ob.goalRegionCostToGo)) 00163 return obj 00164 00165 def plan(fname = None): 00166 # Construct the robot state space in which we're planning. We're 00167 # planning in [0,1]x[0,1], a subset of R^2. 00168 space = ob.RealVectorStateSpace(2) 00169 00170 # Set the bounds of space to be in [0,1]. 00171 space.setBounds(0.0, 1.0) 00172 00173 # Construct a space information instance for this state space 00174 si = ob.SpaceInformation(space) 00175 00176 # Set the object used to check which states in the space are valid 00177 validityChecker = ValidityChecker(si) 00178 si.setStateValidityChecker(validityChecker) 00179 00180 si.setup() 00181 00182 # Set our robot's starting state to be the bottom-left corner of 00183 # the environment, or (0,0). 00184 start = ob.State(space) 00185 start[0] = 0.0 00186 start[1] = 0.0 00187 00188 # Set our robot's goal state to be the top-right corner of the 00189 # environment, or (1,1). 00190 goal = ob.State(space) 00191 goal[0] = 1.0 00192 goal[1] = 1.0 00193 00194 # Create a problem instance 00195 pdef = ob.ProblemDefinition(si) 00196 00197 # Set the start and goal states 00198 pdef.setStartAndGoalStates(start, goal) 00199 00200 # Since we want to find an optimal plan, we need to define what 00201 # is optimal with an OptimizationObjective structure. Un-comment 00202 # exactly one of the following 6 lines to see some examples of 00203 # optimization objectives. 00204 pdef.setOptimizationObjective(getPathLengthObjective(si)) 00205 # pdef.setOptimizationObjective(getThresholdPathLengthObj(si)) 00206 # pdef.setOptimizationObjective(getClearanceObjective(si)) 00207 # pdef.setOptimizationObjective(getBalancedObjective1(si)) 00208 # pdef.setOptimizationObjective(getBalancedObjective2(si)) 00209 # pdef.setOptimizationObjective(getPathLengthObjWithCostToGo(si)) 00210 00211 # Construct our optimal planner using the RRTstar algorithm. 00212 optimizingPlanner = og.RRTstar(si) 00213 00214 # Set the problem instance for our planner to solve 00215 optimizingPlanner.setProblemDefinition(pdef) 00216 optimizingPlanner.setup() 00217 00218 # attempt to solve the planning problem within one second of 00219 # planning time 00220 solved = optimizingPlanner.solve(10.0) 00221 00222 if solved: 00223 # Output the length of the path found 00224 print("Found solution of path length %g" % pdef.getSolutionPath().length()) 00225 00226 # If a filename was specified, output the path as a matrix to 00227 # that file for visualization 00228 if fname: 00229 with open(fname,'w') as outFile: 00230 outFile.write(pdef.getSolutionPath().printAsMatrix()) 00231 else: 00232 print("No solution found.") 00233 00234 if __name__ == "__main__": 00235 fname = None if len(argv)<2 else argv[1] 00236 plan(fname) 00237 00238 ## @endcond