diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..fd20fdd --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ + +*.pyc diff --git a/Codes.python/p34/compound.py b/Codes.python/p34/compound.py new file mode 100644 index 0000000..1d383e2 --- /dev/null +++ b/Codes.python/p34/compound.py @@ -0,0 +1,25 @@ +import numpy as np +from math import* +def compound(Xwa,Xab): + Xwb = np.array(np.zeros([3,3])) + rot = np.array([[cos(Xwa[2,0]),-sin(Xwa[2,0] )], + [sin(Xwa[2,0]), cos(Xwa[2,0])]]) + Xwb[0:2] = rot.dot(np.array([Xab[0],Xab[1]])) + np.tile([Xwa[0:2]],(1, len(Xab[0]))) + row = np.shape(Xab) + if row[0] == 3: + Xwb[2] = piTopi(Xwa[2] + Xab[2]) + return Xwb + +def piTopi(angle): + twopi = 2*pi + angle = angle - twopi * int(angle/twopi) + i1 = np.where(angle >= pi) + angle[i1] = angle[i1] - twopi + + i2 = np.where(angle < -pi) + angle[i2] = angle[i2] + twopi + return angle + +# a =np.array([[5],[3],[5*pi/18]]) +# b =np.array([[4],[2],[pi/9]]) +# print compound(a,b) \ No newline at end of file diff --git a/Codes.python/p34/p34.py b/Codes.python/p34/p34.py new file mode 100644 index 0000000..5c086ed --- /dev/null +++ b/Codes.python/p34/p34.py @@ -0,0 +1,72 @@ +import compound +from math import * +from numpy import * +import scipy.io as sio +import matplotlib.pyplot as plt + +def compSteer(x,wp,iwp,G,dt): + maxG = 30*pi/180 + rateG = 20*pi/180 + minD = 0.5 + cwp = wp[:,iwp-1] + d2 = (cwp[0]-x[0])**2 + (cwp[1]-x[1])**2 + if d2 < minD**2: + iwp = iwp + 1 + if iwp > len(wp[0]): + iwp = 0 + cwp = wp[:,iwp-1] + + deltaG = compound.piTopi(atan2(cwp[1]-x[1],cwp[0]-x[0])-x[2]-G) + maxDelta = rateG * dt + + if abs(deltaG) > maxDelta: + deltaG = sign(deltaG)*maxDelta + + G = G + deltaG + if abs(G) > maxG: + G = sign(G)*maxG + + return G,iwp + +if __name__=="__main__": + #load tim.mat for getting wp + load_fn = 'tim.mat' + load_data = sio.loadmat(load_fn) + wp = load_data['wp'] + + steering = 0 # initial steering + velocity = 4.0 # the velocity of the vehicle + wheelbase = 8 # the length of wheelbase + dt = 0.05 # time interval + iWp = 1 # index to first waypoint + iPos = 0 + rob = array([[0,-8,-8],[0,-4,4]]) + rob_plot_array = array([[0,-wheelbase,-wheelbase,0],[0,-4,4,0]]) + + #Initialize figure + plt.figure() + plt.xlabel('X(m)') + plt.ylabel('Y(m)') + axes = plt.axes(xlim=(-110,100),ylim=(-110,100)) + axes.plot(wp[0],wp[1],'*g--' ,markersize = 10 ,linewidth = 2) + axes.plot(wp[0],wp[1],'c:',linewidth = 2) + robplot ,= axes.plot(rob_plot_array[0],rob_plot_array[1],'b') + pos = zeros([3, 1]) + path = zeros([3,3390]) + + while iWp!= 0: + steering, iWp = compSteer(pos, wp, iWp, steering, dt) + pos[0] = pos[0] + velocity * dt * cos(steering + pos[2, :]) + pos[1] = pos[1] + velocity * dt * sin(steering + pos[2, :]) + pos[2] = pos[2] + velocity * dt * sin(steering) / wheelbase + pos[2] = compound.piTopi(pos[2]) + robPos = compound.compound(pos, rob) + path[:, iPos + 1] = pos[:, 0] + iPos += 1 + axes.plot(path[0, 0:iPos], path[1, 0:iPos], 'r-',linewidth = 2) + axes.plot(robPos[0,0],robPos[1,0]) + b = robPos[0:2, 0:1] + a = robPos[0:2] + c = hstack((a, b)) + robplot.set_data(c[0], c[1]) + plt.pause(0.00001) diff --git a/Codes.python/p34/tim.mat b/Codes.python/p34/tim.mat new file mode 100644 index 0000000..e406a8f Binary files /dev/null and b/Codes.python/p34/tim.mat differ