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 basename, 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)
68 def postProp(control, state):
79 space.setBounds(bounds)
88 cspace.setBounds(cbounds)
93 ss.setStateValidityChecker(validityChecker)
94 ode = oc.ODE(kinematicCarODE)
96 ss.setStatePropagator(odeSolver.getStatePropagator(oc.PostPropagationEvent(postProp)))
111 ss.setStartAndGoalStates(start, goal, 0.05)
114 solved = ss.solve(120.0)
118 print(
"Found solution:\n%s" % ss.getSolutionPath().asGeometric())
120 if __name__ ==
"__main__":