OptimalPlanning.py
1 #!/usr/bin/env python
2 
3 ######################################################################
4 # Software License Agreement (BSD License)
5 #
6 # Copyright (c) 2010, Rice University
7 # All rights reserved.
8 #
9 # Redistribution and use in source and binary forms, with or without
10 # modification, are permitted provided that the following conditions
11 # are met:
12 #
13 # * Redistributions of source code must retain the above copyright
14 # notice, this list of conditions and the following disclaimer.
15 # * Redistributions in binary form must reproduce the above
16 # copyright notice, this list of conditions and the following
17 # disclaimer in the documentation and/or other materials provided
18 # with the distribution.
19 # * Neither the name of the Rice University nor the names of its
20 # contributors may be used to endorse or promote products derived
21 # from this software without specific prior written permission.
22 #
23 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 # POSSIBILITY OF SUCH DAMAGE.
35 ######################################################################
36 
37 # Author: Luis G. Torres, Mark Moll
38 
39 try:
40  from ompl import util as ou
41  from ompl import base as ob
42  from ompl import geometric as og
43 except:
44  # if the ompl module is not in the PYTHONPATH assume it is installed in a
45  # subdirectory of the parent directory called "py-bindings."
46  from os.path import abspath, dirname, join
47  import sys
48  sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings'))
49  from ompl import util as ou
50  from ompl import base as ob
51  from ompl import geometric as og
52 from math import sqrt
53 from sys import argv
54 
55 ## @cond IGNORE
56 # Our "collision checker". For this demo, our robot's state space
57 # lies in [0,1]x[0,1], with a circular obstacle of radius 0.25
58 # centered at (0.5,0.5). Any states lying in this circular region are
59 # considered "in collision".
60 class ValidityChecker(ob.StateValidityChecker):
61  def __init__(self, si):
62  super(ValidityChecker, self).__init__(si)
63 
64  # Returns whether the given state's position overlaps the
65  # circular obstacle
66  def isValid(self, state):
67  return self.clearance(state) > 0.0
68 
69  # Returns the distance from the given state's position to the
70  # boundary of the circular obstacle.
71  def clearance(self, state):
72  # Extract the robot's (x,y) position from its state
73  x = state[0]
74  y = state[1]
75 
76  # Distance formula between two points, offset by the circle's
77  # radius
78  return sqrt((x-0.5)*(x-0.5) + (y-0.5)*(y-0.5)) - 0.25
79 
80 
81 ## Returns a structure representing the optimization objective to use
82 # for optimal motion planning. This method returns an objective
83 # which attempts to minimize the length in configuration space of
84 # computed paths.
85 def getPathLengthObjective(si):
87 
88 ## Returns an optimization objective which attempts to minimize path
89 # length that is satisfied when a path of length shorter than 1.51
90 # is found.
91 def getThresholdPathLengthObj(si):
93  obj.setCostThreshold(ob.Cost(1.51))
94  return obj
95 
96 ## Defines an optimization objective which attempts to steer the
97 # robot away from obstacles. To formulate this objective as a
98 # minimization of path cost, we can define the cost of a path as a
99 # summation of the costs of each of the states along the path, where
100 # each state cost is a function of that state's clearance from
101 # obstacles.
102 #
103 # The class StateCostIntegralObjective represents objectives as
104 # summations of state costs, just like we require. All we need to do
105 # then is inherit from that base class and define our specific state
106 # cost function by overriding the stateCost() method.
107 #
108 class ClearanceObjective(ob.StateCostIntegralObjective):
109  def __init__(self, si):
110  super(ClearanceObjective, self).__init__(si, True)
111  self.si_ = si
112 
113  # Our requirement is to maximize path clearance from obstacles,
114  # but we want to represent the objective as a path cost
115  # minimization. Therefore, we set each state's cost to be the
116  # reciprocal of its clearance, so that as state clearance
117  # increases, the state cost decreases.
118  def stateCost(self, s):
119  return ob.Cost(1 / self.si_.getStateValidityChecker().clearance(s))
120 
121 ## Return an optimization objective which attempts to steer the robot
122 # away from obstacles.
123 def getClearanceObjective(si):
124  return ClearanceObjective(si)
125 
126 ## Create an optimization objective which attempts to optimize both
127 # path length and clearance. We do this by defining our individual
128 # objectives, then adding them to a MultiOptimizationObjective
129 # object. This results in an optimization objective where path cost
130 # is equivalent to adding up each of the individual objectives' path
131 # costs.
132 #
133 # When adding objectives, we can also optionally specify each
134 # objective's weighting factor to signify how important it is in
135 # optimal planning. If no weight is specified, the weight defaults to
136 # 1.0.
137 def getBalancedObjective1(si):
138  lengthObj = ob.PathLengthOptimizationObjective(si)
139  clearObj = ClearanceObjective(si)
140 
142  opt.addObjective(lengthObj, 5.0)
143  opt.addObjective(clearObj, 1.0)
144 
145  return opt
146 
147 ## Create an optimization objective equivalent to the one returned by
148 # getBalancedObjective1(), but use an alternate syntax.
149 # THIS DOESN'T WORK YET. THE OPERATORS SOMEHOW AREN'T EXPORTED BY Py++.
150 # def getBalancedObjective2(si):
151 # lengthObj = ob.PathLengthOptimizationObjective(si)
152 # clearObj = ClearanceObjective(si)
153 #
154 # return 5.0*lengthObj + clearObj
155 
156 
157 ## Create an optimization objective for minimizing path length, and
158 # specify a cost-to-go heuristic suitable for this optimal planning
159 # problem.
160 def getPathLengthObjWithCostToGo(si):
162  obj.setCostToGoHeuristic(ob.CostToGoHeuristic(ob.goalRegionCostToGo))
163  return obj
164 
165 def plan(fname = None):
166  # Construct the robot state space in which we're planning. We're
167  # planning in [0,1]x[0,1], a subset of R^2.
168  space = ob.RealVectorStateSpace(2)
169 
170  # Set the bounds of space to be in [0,1].
171  space.setBounds(0.0, 1.0)
172 
173  # Construct a space information instance for this state space
174  si = ob.SpaceInformation(space)
175 
176  # Set the object used to check which states in the space are valid
177  validityChecker = ValidityChecker(si)
178  si.setStateValidityChecker(validityChecker)
179 
180  si.setup()
181 
182  # Set our robot's starting state to be the bottom-left corner of
183  # the environment, or (0,0).
184  start = ob.State(space)
185  start[0] = 0.0
186  start[1] = 0.0
187 
188  # Set our robot's goal state to be the top-right corner of the
189  # environment, or (1,1).
190  goal = ob.State(space)
191  goal[0] = 1.0
192  goal[1] = 1.0
193 
194  # Create a problem instance
195  pdef = ob.ProblemDefinition(si)
196 
197  # Set the start and goal states
198  pdef.setStartAndGoalStates(start, goal)
199 
200  # Since we want to find an optimal plan, we need to define what
201  # is optimal with an OptimizationObjective structure. Un-comment
202  # exactly one of the following 6 lines to see some examples of
203  # optimization objectives.
204  pdef.setOptimizationObjective(getPathLengthObjective(si))
205  # pdef.setOptimizationObjective(getThresholdPathLengthObj(si))
206  # pdef.setOptimizationObjective(getClearanceObjective(si))
207  # pdef.setOptimizationObjective(getBalancedObjective1(si))
208  # pdef.setOptimizationObjective(getBalancedObjective2(si))
209  # pdef.setOptimizationObjective(getPathLengthObjWithCostToGo(si))
210 
211  # Construct our optimal planner using the RRTstar algorithm.
212  optimizingPlanner = og.RRTstar(si)
213 
214  # Set the problem instance for our planner to solve
215  optimizingPlanner.setProblemDefinition(pdef)
216  optimizingPlanner.setup()
217 
218  # attempt to solve the planning problem within one second of
219  # planning time
220  solved = optimizingPlanner.solve(10.0)
221 
222  if solved:
223  # Output the length of the path found
224  print("Found solution of path length %g" % pdef.getSolutionPath().length())
225 
226  # If a filename was specified, output the path as a matrix to
227  # that file for visualization
228  if fname:
229  with open(fname,'w') as outFile:
230  outFile.write(pdef.getSolutionPath().printAsMatrix())
231  else:
232  print("No solution found.")
233 
234 if __name__ == "__main__":
235  fname = None if len(argv)<2 else argv[1]
236  plan(fname)
237 
238 ## @endcond
Optimal Rapidly-exploring Random Trees.
Definition: RRTstar.h:76
This class allows for the definition of multiobjective optimal planning problems. Objectives are adde...
boost::function< Cost(const State *, const Goal *)> CostToGoHeuristic
The definition of a function which returns an admissible estimate of the optimal path cost from a giv...
Defines optimization objectives where path cost can be represented as a path integral over a cost fun...
Abstract definition for a class checking the validity of states. The implementation of this class mus...
A state space representing Rn. The distance function is the L2 norm.
The base class for space information. This contains all the information about the space planning is d...
An optimization objective which corresponds to optimizing path length.
Definition of an abstract state.
Definition: State.h:50
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47