39 from math
import sin, cos, tan
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 geometric
as og
52 from ompl
import control
as oc
54 def kinematicCarODE(q, u, qdot):
57 qdot[0] = u[0] * cos(theta)
58 qdot[1] = u[0] * sin(theta)
59 qdot[2] = u[0] * tan(u[1]) / carLength
62 def isStateValid(spaceInformation, state):
65 return spaceInformation.satisfiesBounds(state)
75 space.setBounds(bounds)
84 cspace.setBounds(cbounds)
89 ss.setStateValidityChecker(validityChecker)
90 ode = oc.ODE(kinematicCarODE)
92 propagator = oc.ODESolver.getStatePropagator(odeSolver)
93 ss.setStatePropagator(propagator)
108 ss.setStartAndGoalStates(start, goal, 0.05)
111 solved = ss.solve(120.0)
115 print(
"Found solution:\n%s" % ss.getSolutionPath().asGeometric().printAsMatrix())
117 if __name__ ==
"__main__":
The lower and upper bounds for an Rn space.
A state space representing SE(2)
Definition of an abstract state.
Basic solver for ordinary differential equations of the type q' = f(q, u), where q is the current sta...
A control space representing Rn.
Create the set of classes typically needed to solve a control problem.
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...