39 from math
import sin, cos
40 from functools
import partial
42 from ompl
import base
as ob
43 from ompl
import control
as oc
47 from os.path
import abspath, dirname, join
49 sys.path.insert(0, join(dirname(dirname(abspath(__file__))),
'py-bindings'))
50 from ompl
import base
as ob
51 from ompl
import control
as oc
56 def __init__(self, length, bounds):
57 super(MyDecomposition, self).__init__(length, 2, bounds)
58 def project(self, s, coord):
61 def sampleFullState(self, sampler, coord, s):
62 sampler.sampleUniform(s)
63 s.setXY(coord[0], coord[1])
66 def isStateValid(spaceInformation, state):
69 return spaceInformation.satisfiesBounds(state)
71 def propagate(start, control, duration, state):
72 state.setX(start.getX() + control[0] * duration * cos(start.getYaw()))
73 state.setY(start.getY() + control[0] * duration * sin(start.getYaw()))
74 state.setYaw(start.getYaw() + control[1] * duration)
84 space.setBounds(bounds)
93 cspace.setBounds(cbounds)
98 partial(isStateValid, ss.getSpaceInformation())))
114 ss.setStartAndGoalStates(start, goal, 0.05)
117 si = ss.getSpaceInformation()
122 decomp = MyDecomposition(32, bounds)
125 ss.setPlanner(planner)
127 si.setPropagationStepSize(.1)
130 solved = ss.solve(20.0)
134 print(
"Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
136 if __name__ ==
"__main__":
The lower and upper bounds for an Rn space.
A state space representing SE(2)
Definition of an abstract state.
A GridDecomposition is a Decomposition implemented using a grid.
A control space representing Rn.
Create the set of classes typically needed to solve a control problem.
SyclopEST is Syclop with EST as its low-level tree planner. .
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...
std::function< void(const base::State *, const Control *, const double, base::State *)> StatePropagatorFn
A function that achieves state propagation.