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 basename, 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)
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()
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:")