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
52 from random
import choice
57 def __init__(self, si):
58 super(RandomWalkPlanner, self).__init__(si,
"RandomWalkPlanner")
60 self.sampler_ = si.allocStateSampler()
63 pdef = self.getProblemDefinition()
65 si = self.getSpaceInformation()
66 pi = self.getPlannerInputStates()
69 self.states_.append(st)
75 rstate = si.allocState()
77 self.sampler_.sampleUniform(rstate)
79 if si.checkMotion(self.states_[-1], rstate):
80 self.states_.append(rstate)
81 sat = goal.isSatisfied(rstate)
82 dist = goal.distanceGoal(rstate)
85 solution = len(self.states_)
89 approxsol = len(self.states_)
97 for s
in self.states_[:solution]:
99 pdef.addSolutionPath(path)
104 super(RandomWalkPlanner, self).clear()
109 def isStateValid(state):
113 return x*x + y*y + z*z > .8
122 space.setBounds(bounds)
134 ss.setStartAndGoalStates(start, goal, .05)
137 ss.setPlanner(planner)
139 result = ss.solve(10.0)
141 if result.getStatus() == ob.PlannerStatus.APPROXIMATE_SOLUTION:
142 print(
"Solution is approximate")
144 ss.simplifySolution()
146 print(ss.getSolutionPath())
149 if __name__ ==
"__main__":
boost::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a boost function can be spec...
Create the set of classes typically needed to solve a geometric problem.
Base class for a planner.
A class to store the exit status of Planner::solve()
A state space representing Rn. The distance function is the L2 norm.
Definition of an abstract state.
The lower and upper bounds for an Rn space.
Definition of a geometric path.