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
44 from ompl
import geometric
as og
48 from os.path
import abspath, dirname, join
50 sys.path.insert(0, join(dirname(dirname(abspath(__file__))),
'py-bindings'))
51 from ompl
import base
as ob
52 from ompl
import control
as oc
53 from ompl
import geometric
as og
55 def kinematicCarODE(q, u, qdot):
58 qdot[0] = u[0] * cos(theta)
59 qdot[1] = u[0] * sin(theta)
60 qdot[2] = u[0] * tan(u[1]) / carLength
63 def isStateValid(spaceInformation, state):
66 return spaceInformation.satisfiesBounds(state)
76 space.setBounds(bounds)
85 cspace.setBounds(cbounds)
90 ss.setStateValidityChecker(validityChecker)
91 ode = oc.ODE(kinematicCarODE)
93 propagator = oc.ODESolver.getStatePropagator(odeSolver)
94 ss.setStatePropagator(propagator)
109 ss.setStartAndGoalStates(start, goal, 0.05)
112 solved = ss.solve(120.0)
116 print(
"Found solution:\n%s" % ss.getSolutionPath().asGeometric().printAsMatrix())
118 if __name__ ==
"__main__":
Create the set of classes typically needed to solve a control problem.
A control space representing Rn.
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...
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...