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
56 def __init__(self, si):
57 super(RandomWalkPlanner, self).__init__(si,
"RandomWalkPlanner")
59 self.sampler_ = si.allocStateSampler()
62 pdef = self.getProblemDefinition()
64 si = self.getSpaceInformation()
65 pi = self.getPlannerInputStates()
68 self.states_.append(st)
74 rstate = si.allocState()
76 self.sampler_.sampleUniform(rstate)
78 if si.checkMotion(self.states_[-1], rstate):
79 self.states_.append(rstate)
80 sat = goal.isSatisfied(rstate)
81 dist = goal.distanceGoal(rstate)
84 solution = len(self.states_)
88 approxsol = len(self.states_)
96 for s
in self.states_[:solution]:
98 pdef.addSolutionPath(path)
103 super(RandomWalkPlanner, self).clear()
108 def isStateValid(state):
112 return x*x + y*y + z*z > .8
121 space.setBounds(bounds)
133 ss.setStartAndGoalStates(start, goal, .05)
136 ss.setPlanner(planner)
138 result = ss.solve(10.0)
140 if result.getStatus() == ob.PlannerStatus.APPROXIMATE_SOLUTION:
141 print(
"Solution is approximate")
143 ss.simplifySolution()
145 print(ss.getSolutionPath())
148 if __name__ ==
"__main__":
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.
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...