OptimalPlanning.py
1 #!/usr/bin/env python
2 
3 
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 
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 
85 def getPathLengthObjective(si):
87 
88 
91 def getThresholdPathLengthObj(si):
93  obj.setCostThreshold(ob.Cost(1.51))
94  return obj
95 
96 
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 
123 def getClearanceObjective(si):
124  return ClearanceObjective(si)
125 
126 
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 
155 
156 
157 
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 
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