Obs = np.loadtxt( '/home/summit/Spidoo_ws/src/stiffness_observer/scripts/PLOT/GOOD/Obse_Stif_V8.txt' ) WithOb = np.loadtxt( '/home/summit/Spidoo_ws/src/stiffness_observer/scripts/PLOT/GOOD/Without_Stif_V8.txt' ) Obs2 = np.loadtxt( '/home/summit/Spidoo_ws/src/stiffness_observer/scripts/PLOT/GOOD/Obse_Stif_V5.txt' ) WithOb2 = np.loadtxt( '/home/summit/Spidoo_ws/src/stiffness_observer/scripts/PLOT/GOOD/Without_Stif_V5.txt' ) X4 = butter_lowpass_filter(Obs[:, 1], 0.04, 10, 8) Y4 = butter_lowpass_filter(Obs[:, 2], 0.04, 10, 8) X2 = butter_lowpass_filter(WithOb[:, 1], 0.04, 10, 8) Y2 = butter_lowpass_filter(WithOb[:, 2], 0.04, 10, 8) Xref = butter_lowpass_filter(Obs[:, 10], 0.04, 10, 8) Yref = butter_lowpass_filter(Obs[:, 11], 0.04, 10, 8) Xref = Xref[:] + X4[0] - Xref[0] Yref = Yref[:] + Y4[0] - Yref[0] #plt.plot(res1[:,10],res1[:,11], 'b', linewidth=4.0) plt.plot(Xref, Yref, 'g', linewidth=4.5) plt.plot(X4, Y4, 'r--', linewidth=4) plt.plot(X2, Y2, 'b--', linewidth=4)
while (i<len(Obs2[:,1])-1): s44[i+1]=s44[i]+hypot(Obs2[i+1,1]-Obs2[i,1],Obs2[i+1,2]-Obs2[i,2]) i=i+1 i=0 while (i<len(WithOb2[:,1])-1): s22[i+1]=s22[i]+hypot(WithOb2[i+1,1]-WithOb2[i,1],WithOb2[i+1,2]-WithOb2[i,2]) i=i+1 #############All errors ############################################################################ ########################################################################## y4= butter_lowpass_filter(Obs[:,4], 0.03, 200, 6) y2= butter_lowpass_filter(WithOb[:,4], 0.03, 200, 6) y44= butter_lowpass_filter(Obs2[:,4], 0.03, 200, 6) y22= butter_lowpass_filter(WithOb2[:,4], 0.03, 200, 6) plt.plot(s44[:],y44[:], 'r', linewidth=3) plt.plot(s22[:],y22[:], 'k--', linewidth=3) plt.plot(s4[:],y4[:], 'b', linewidth=3) plt.plot(s2[:],y2[:], 'g--', linewidth=3) plt.xlabel('Curvilinear abscissa [m]', fontsize=18) plt.ylabel('Lateral error [$m$]', fontsize=18)
from scipy import fftpack from pylab import * import shapely.geometry as geom from scipy import spatial from ButterFilter import butter_lowpass_filter kk = 180 / pi ######Trajectory plot ################################################################# res4 = np.loadtxt( '/home/summit/Spidoo_ws/src/spido_obskridssa/scripts/Plot/Without_contr_4_Rear.txt' ) X4 = butter_lowpass_filter(res4[:, 1], 0.04, 10, 8) Y4 = butter_lowpass_filter(res4[:, 2], 0.04, 10, 8) Xref = butter_lowpass_filter(res4[:, 8], 0.04, 10, 8) Yref = butter_lowpass_filter(res4[:, 9], 0.04, 10, 8) Xref = Xref[:] + X4[0] - Xref[0] Yref = Yref[:] + Y4[0] - Yref[0] #plt.plot(res1[:,10],res1[:,11], 'b', linewidth=4.0) plt.plot(X4, Y4, 'r--', linewidth=3.0) plt.plot(Xref, Yref, 'g', linewidth=3.0) plt.axis('equal') #plt.axis([-50, 10, -10, 70]) plt.yticks(color='k', size=40)
from scipy import fftpack from pylab import * import shapely.geometry as geom from scipy import spatial from ButterFilter import butter_lowpass_filter kk = 180 / pi ######Trajectory plot ################################################################# res4 = np.loadtxt( '/home/summit/Spidoo_ws/src/stiffness_observer/scripts/PLOT/LQR_Stif_V5.txt' ) X4 = butter_lowpass_filter(res4[:, 1], 0.04, 10, 8) Y4 = butter_lowpass_filter(res4[:, 2], 0.04, 10, 8) Xref = butter_lowpass_filter(res4[:, 10], 0.04, 10, 8) Yref = butter_lowpass_filter(res4[:, 11], 0.04, 10, 8) Xref = Xref[:] + X4[0] - Xref[0] Yref = Yref[:] + Y4[0] - Yref[0] #plt.plot(res1[:,10],res1[:,11], 'b', linewidth=4.0) plt.plot(X4, Y4, 'r--', linewidth=3.0) plt.plot(Xref, Yref, 'g', linewidth=3.0) plt.axis('equal') #plt.axis([-50, 10, -10, 70]) plt.yticks(color='k', size=40)
s2[i + 1] = s2[i] + hypot(WithOb[i + 1, 1] - WithOb[i, 1], WithOb[i + 1, 2] - WithOb[i, 2]) i = i + 1 s44 = np.zeros(len(sim[:, 1])) i = 0 while (i < len(sim[:, 1]) - 1): s44[i + 1] = s44[i] + hypot(sim[i + 1, 1] - sim[i, 1], sim[i + 1, 2] - sim[i, 2]) i = i + 1 print(len(s4)) ################## yf4 = butter_lowpass_filter(sim[:, 20], 0.02, 200, 6) yf5 = butter_lowpass_filter(sim[:, 21], 0.02, 200, 6) yf2 = butter_lowpass_filter(WithOb[:, 20], 0.02, 200, 6) yf3 = butter_lowpass_filter(WithOb[:, 21], 0.02, 200, 6) X4 = butter_lowpass_filter(FyF, 0.007, 10, 8) #for i in range(len(X4)/2): #if X4[i]>0: #X4[i]=X4[i]+100*sin(10**-1.5*i) #X4[len(X4)/2+i-1]=-yf4[len(X4)/2+i-1]+80*sin(10**-2*i) #### V=5m:s #for i in range(len(X4)): # if i<980:
q.append(Vpsi_gazebo[i]) print(np.shape(res[:, 3])) print(len(Vy_gazebo)) print(np.shape(Vx_gazebo)) for i in range(len(Vy_gazebo)): if i % 2 == 0: Vy = Vy_gazebo[i] * math.cos(res[ i / 2, 3] - pi) - Vx_gazebo[i] * math.sin(res[i / 2, 3] - pi) - 1.9 k.append(Vy) #for i in range(len(k)): # k[i]=-k[i] X4 = butter_lowpass_filter(k[:], 0.8, 10, 8) q4 = butter_lowpass_filter(q[:], 0.04, 10, 8) yf4 = butter_lowpass_filter(sim[:, 6], 0.02, 200, 6) ############################ Curvilinear abscissa ############################## s4 = np.zeros(len(res[:, 1])) i = 0 while (i < len(res[:, 1]) - 1): s4[i + 1] = s4[i] + hypot(res[i + 1, 1] - res[i, 1], res[i + 1, 2] - res[i, 2]) i = i + 1 s44 = np.zeros(len(sim[:, 1]))
from ButterFilter import butter_lowpass_filter kk = 180 / pi ###### Upload recorded files ################################################################# NLobs = np.loadtxt( '/home/summit/Spidoo_ws/src/spido_obskridssa/scripts/Plot/NLObser_4.txt' ) #NLobser_5 LinObs = np.loadtxt( '/home/summit/Spidoo_ws/src/spido_obskridssa/scripts/Plot/LinObser_4.txt') WithoutObs = np.loadtxt( '/home/summit/Spidoo_ws/src/spido_obskridssa/scripts/Plot/Without_contr_4.txt' ) XNL = butter_lowpass_filter(NLobs[:, 1], 0.04, 10, 8) YNL = butter_lowpass_filter(NLobs[:, 2], 0.04, 10, 8) Xlin = butter_lowpass_filter(LinObs[:, 1], 0.04, 10, 8) Ylin = butter_lowpass_filter(LinObs[:, 2], 0.04, 10, 8) Xwo = butter_lowpass_filter(WithoutObs[:, 1], 0.04, 10, 8) Ywo = butter_lowpass_filter(WithoutObs[:, 2], 0.04, 10, 8) Xref = butter_lowpass_filter(NLobs[:, 8], 0.04, 10, 8) Yref = butter_lowpass_filter(NLobs[:, 9], 0.04, 10, 8) Xref = Xref[:] + XNL[0] - Xref[0] Yref = Yref[:] + YNL[0] - Yref[0] ###### Trajectory plot #################################################################