00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 from math import sin, cos, tan
00040 from functools import partial
00041 try:
00042 from ompl import base as ob
00043 from ompl import control as oc
00044 from ompl import geometric as og
00045 except:
00046
00047
00048 from os.path import basename, abspath, dirname, join
00049 import sys
00050 sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings'))
00051 from ompl import base as ob
00052 from ompl import control as oc
00053 from ompl import geometric as og
00054
00055 def kinematicCarODE(q, u, qdot):
00056 theta = q[2];
00057 carLength = 0.2;
00058 qdot[0] = u[0] * cos(theta)
00059 qdot[1] = u[0] * sin(theta)
00060 qdot[2] = u[0] * tan(u[1]) / carLength
00061
00062
00063 def isStateValid(spaceInformation, state):
00064
00065
00066 return spaceInformation.satisfiesBounds(state)
00067
00068 def postProp(control, state):
00069 return
00070
00071 def plan():
00072
00073 space = ob.SE2StateSpace()
00074
00075
00076 bounds = ob.RealVectorBounds(2)
00077 bounds.setLow(-1)
00078 bounds.setHigh(1)
00079 space.setBounds(bounds)
00080
00081
00082 cspace = oc.RealVectorControlSpace(space, 2)
00083
00084
00085 cbounds = ob.RealVectorBounds(2)
00086 cbounds.setLow(-.3)
00087 cbounds.setHigh(.3)
00088 cspace.setBounds(cbounds)
00089
00090
00091 ss = oc.SimpleSetup(cspace)
00092 validityChecker = ob.StateValidityCheckerFn(partial(isStateValid, ss.getSpaceInformation()))
00093 ss.setStateValidityChecker(validityChecker)
00094 ode = oc.ODE(kinematicCarODE)
00095 odeSolver = oc.ODEBasicSolver(ss.getSpaceInformation(), ode)
00096 ss.setStatePropagator(odeSolver.getStatePropagator(oc.PostPropagationEvent(postProp)))
00097
00098
00099 start = ob.State(space)
00100 start().setX(-0.5);
00101 start().setY(0.0);
00102 start().setYaw(0.0);
00103
00104
00105 goal = ob.State(space);
00106 goal().setX(0.0);
00107 goal().setY(0.5);
00108 goal().setYaw(0.0);
00109
00110
00111 ss.setStartAndGoalStates(start, goal, 0.05)
00112
00113
00114 solved = ss.solve(120.0)
00115
00116 if solved:
00117
00118 print("Found solution:\n%s" % ss.getSolutionPath().asGeometric())
00119
00120 if __name__ == "__main__":
00121 plan()