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
00040 from functools import partial
00041 try:
00042 from ompl import util as ou
00043 from ompl import base as ob
00044 from ompl import control as oc
00045 from ompl import geometric as og
00046 except:
00047
00048
00049 from os.path import basename, abspath, dirname, join
00050 import sys
00051 sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings'))
00052 from ompl import util as ou
00053 from ompl import base as ob
00054 from ompl import control as oc
00055 from ompl import geometric as og
00056
00057
00058
00059 class MyDecomposition(oc.GridDecomposition):
00060 def __init__(self, length, bounds):
00061 super(MyDecomposition, self).__init__(length, 2, bounds)
00062 self.rng_ = ou.RNG()
00063 def project(self, s, coord):
00064 coord[0] = s.getX()
00065 coord[1] = s.getY()
00066 def sampleFromRegion(self, rid, sampler, s):
00067 sampler.sampleUniform(s)
00068 regionBounds = self.getRegionBounds(rid)
00069 s.setX(self.rng_.uniformReal(regionBounds.low[0], regionBounds.high[0]))
00070 s.setY(self.rng_.uniformReal(regionBounds.low[1], regionBounds.high[1]))
00071
00072
00073 def isStateValid(spaceInformation, state):
00074
00075
00076 return spaceInformation.satisfiesBounds(state)
00077
00078 def propagate(start, control, duration, state):
00079 state.setX( start.getX() + control[0] * duration * cos(start.getYaw()) )
00080 state.setY( start.getY() + control[0] * duration * sin(start.getYaw()) )
00081 state.setYaw(start.getYaw() + control[1] * duration)
00082
00083 def plan():
00084
00085 space = ob.SE2StateSpace()
00086
00087
00088 bounds = ob.RealVectorBounds(2)
00089 bounds.setLow(-1)
00090 bounds.setHigh(1)
00091 space.setBounds(bounds)
00092
00093
00094 cspace = oc.RealVectorControlSpace(space, 2)
00095
00096
00097 cbounds = ob.RealVectorBounds(2)
00098 cbounds.setLow(-.3)
00099 cbounds.setHigh(.3)
00100 cspace.setBounds(cbounds)
00101
00102
00103 ss = oc.SimpleSetup(cspace)
00104 ss.setStateValidityChecker(ob.StateValidityCheckerFn(partial(isStateValid, ss.getSpaceInformation())))
00105 ss.setStatePropagator(oc.StatePropagatorFn(propagate))
00106
00107
00108 start = ob.State(space)
00109 start().setX(-0.5);
00110 start().setY(0.0);
00111 start().setYaw(0.0);
00112
00113
00114 goal = ob.State(space);
00115 goal().setX(0.0);
00116 goal().setY(0.5);
00117 goal().setYaw(0.0);
00118
00119
00120 ss.setStartAndGoalStates(start, goal, 0.05)
00121
00122
00123 si = ss.getSpaceInformation()
00124
00125
00126
00127
00128 decomp = MyDecomposition(32, bounds)
00129 planner = oc.SyclopEST(si, decomp)
00130
00131 ss.setPlanner(planner)
00132
00133 si.setPropagationStepSize(.1)
00134
00135
00136 solved = ss.solve(20.0)
00137
00138 if solved:
00139
00140 print("Found solution:\n%s" % ss.getSolutionPath().asGeometric())
00141
00142 if __name__ == "__main__":
00143 plan()