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 time
import sleep
64 def __init__(self, si):
65 super(MyValidStateSampler, self).__init__(si)
66 self.name_ =
"my sampler" 73 def sample(self, state):
74 z = self.rng_.uniformReal(-1, 1)
76 if z > .25
and z < .5:
77 x = self.rng_.uniformReal(0, 1.8)
78 y = self.rng_.uniformReal(0, .2)
79 i = self.rng_.uniformInt(0, 3)
93 state[0] = self.rng_.uniformReal(-1, 1)
94 state[1] = self.rng_.uniformReal(-1, 1)
102 def isStateValid(state):
110 return not (fabs(state[0] < .8)
and fabs(state[1] < .8)
and \
111 state[2] > .25
and state[2] < .5)
114 def allocOBValidStateSampler(si):
120 def allocMyValidStateSampler(si):
121 return MyValidStateSampler(si)
123 def plan(samplerIndex):
131 space.setBounds(bounds)
152 ss.setStartAndGoalStates(start, goal)
155 si = ss.getSpaceInformation()
156 if samplerIndex == 1:
159 elif samplerIndex == 2:
165 ss.setPlanner(planner)
168 solved = ss.solve(10.0)
170 print(
"Found solution:")
172 print(ss.getSolutionPath())
174 print(
"No solution found")
177 if __name__ ==
'__main__':
178 print(
"Using default uniform sampler:")
180 print(
"\nUsing obstacle-based sampler:")
182 print(
"\nUsing my sampler:")
Generate valid samples using obstacle based sampling. First sample an invalid state, then sample a valid state. Then, interpolate from the invalid state to the valid state, returning the first valid state encountered.
Create the set of classes typically needed to solve a geometric problem.
Abstract definition of a state sampler.
A state space representing Rn. The distance function is the L2 norm.
std::function< ValidStateSamplerPtr(const SpaceInformation *)> ValidStateSamplerAllocator
Definition of a function that can allocate a valid state sampler.
Definition of an abstract state.
The lower and upper bounds for an Rn space.
Probabilistic RoadMap planner.
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...