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)
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())
118 if __name__ ==
"__main__":