40 from ompl
import util
as ou
41 from ompl
import base
as ob
42 from ompl
import geometric
as og
46 from os.path
import abspath, dirname, join
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
61 def __init__(self, si):
62 super(ValidityChecker, self).__init__(si)
66 def isValid(self, state):
67 return self.clearance(state) > 0.0
71 def clearance(self, state):
78 return sqrt((x-0.5)*(x-0.5) + (y-0.5)*(y-0.5)) - 0.25
85 def getPathLengthObjective(si):
91 def getThresholdPathLengthObj(si):
93 obj.setCostThreshold(
ob.Cost(1.51))
109 def __init__(self, si):
110 super(ClearanceObjective, self).__init__(si,
True)
118 def stateCost(self, s):
119 return ob.Cost(1 / self.si_.getStateValidityChecker().clearance(s))
123 def getClearanceObjective(si):
124 return ClearanceObjective(si)
137 def getBalancedObjective1(si):
139 clearObj = ClearanceObjective(si)
142 opt.addObjective(lengthObj, 5.0)
143 opt.addObjective(clearObj, 1.0)
160 def getPathLengthObjWithCostToGo(si):
165 def plan(fname = None):
171 space.setBounds(0.0, 1.0)
177 validityChecker = ValidityChecker(si)
178 si.setStateValidityChecker(validityChecker)
198 pdef.setStartAndGoalStates(start, goal)
204 pdef.setOptimizationObjective(getPathLengthObjective(si))
215 optimizingPlanner.setProblemDefinition(pdef)
216 optimizingPlanner.setup()
220 solved = optimizingPlanner.solve(10.0)
224 print(
"Found solution of path length %g" % pdef.getSolutionPath().length())
229 with open(fname,
'w')
as outFile:
230 outFile.write(pdef.getSolutionPath().printAsMatrix())
232 print(
"No solution found.")
234 if __name__ ==
"__main__":
235 fname =
None if len(argv)<2
else argv[1]
Optimal Rapidly-exploring Random Trees.
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.
An optimization objective which corresponds to optimizing path length.
Definition of an abstract state.
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.