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 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)
62 def project(self, s, coord):
65 def sampleFullState(self, sampler, coord, s):
66 sampler.sampleUniform(s)
67 s.setXY(coord[0], coord[1])
70 def isStateValid(spaceInformation, state):
73 return spaceInformation.satisfiesBounds(state)
75 def propagate(start, control, duration, state):
76 state.setX( start.getX() + control[0] * duration * cos(start.getYaw()) )
77 state.setY( start.getY() + control[0] * duration * sin(start.getYaw()) )
78 state.setYaw(start.getYaw() + control[1] * duration)
88 space.setBounds(bounds)
97 cspace.setBounds(cbounds)
117 ss.setStartAndGoalStates(start, goal, 0.05)
120 si = ss.getSpaceInformation()
125 decomp = MyDecomposition(32, bounds)
128 ss.setPlanner(planner)
130 si.setPropagationStepSize(.1)
133 solved = ss.solve(20.0)
137 print(
"Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
139 if __name__ ==
"__main__":
Create the set of classes typically needed to solve a control problem.
A GridDecomposition is a Decomposition implemented using a grid.
std::function< void(const base::State *, const Control *, const double, base::State *)> StatePropagatorFn
A function that achieves state propagation.
A control space representing Rn.
A state space representing SE(2)
SyclopEST is Syclop with EST as its low-level tree planner.
Definition of an abstract state.
The lower and upper bounds for an Rn space.
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...