39 from math
import sin, cos
40 from functools
import partial
42 from ompl
import util
as ou
43 from ompl
import base
as ob
44 from ompl
import control
as oc
45 from ompl
import geometric
as og
49 from os.path
import basename, abspath, dirname, join
51 sys.path.insert(0, join(dirname(dirname(abspath(__file__))),
'py-bindings'))
52 from ompl
import util
as ou
53 from ompl
import base
as ob
54 from ompl
import control
as oc
55 from ompl
import geometric
as og
60 def __init__(self, length, bounds):
61 super(MyDecomposition, self).__init__(length, 2, bounds)
63 def project(self, s, coord):
66 def sampleFromRegion(self, rid, sampler, s):
67 sampler.sampleUniform(s)
68 regionBounds = self.getRegionBounds(rid)
69 s.setX(self.rng_.uniformReal(regionBounds.low[0], regionBounds.high[0]))
70 s.setY(self.rng_.uniformReal(regionBounds.low[1], regionBounds.high[1]))
73 def isStateValid(spaceInformation, state):
76 return spaceInformation.satisfiesBounds(state)
78 def propagate(start, control, duration, state):
79 state.setX( start.getX() + control[0] * duration * cos(start.getYaw()) )
80 state.setY( start.getY() + control[0] * duration * sin(start.getYaw()) )
81 state.setYaw(start.getYaw() + control[1] * duration)
91 space.setBounds(bounds)
100 cspace.setBounds(cbounds)
120 ss.setStartAndGoalStates(start, goal, 0.05)
123 si = ss.getSpaceInformation()
128 decomp = MyDecomposition(32, bounds)
131 ss.setPlanner(planner)
133 si.setPropagationStepSize(.1)
136 solved = ss.solve(20.0)
140 print(
"Found solution:\n%s" % ss.getSolutionPath().asGeometric())
142 if __name__ ==
"__main__":