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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines