wip

python/trajectory.py

@@ -17,25 +17,56 @@ import numpy as np
 import matplotlib as mpl
 import matplotlib.pyplot as plt
 
+def gen_pvt(p,v,t,ts):
+  '''generates a pvt motion
+     p: position array
+     v: velocity array
+     t: time array
+     ts: servo cycle time
+     !!! it is assumed, that the time intervals are constant !!!
+     '''
+  return
+  pvt=np.ndarray(len(tt))*0
+  t[-1]/ts
+
+  tt1=np.arange(0,t[1]-t[0],ts)
+  for i in range(len(t)-1):
+    d=p[i]
+    c=v[i]
+    a=(-2*(p[i+1]-p[i]-v[i]*w)+w*(v[i+1]-v[i]))/w**3
+    b=(3*w*(p[i+1]-p[i]-v[i]*w)-w**2*(v[i+1]-v[i]))/w**3
+    pvt[i*n:(i+1)*n]=a*tt1**3+b*tt1**2+c*tt1+d
+
+  return pvt
+
 w=40.  # ms step between samples
 ts=.2 # sampling time
-x = np.arange(0, 400, w)
-y=np.cos(x)
+n=int(w/ts)# servo cycle between samples
+k=8  #number of unique samples
 
-xx = np.arange(0, 400, ts)
+t = np.arange(0, w*(k+1), w)  #time array of trajectory
 
+#p=3.*np.cos(t)+4.             #position array of trajectory
+np.random.seed(10)
+p=np.random.random(k+1)*4.  #position array of trajectory
+#p=3.*np.sin(1.3+2.*t/(w*k)*2.*np.pi)+10.         #position array of trajectory
+#p+=np.cos(1.5*t/(w*k)*2.*np.pi)         #position array of trajectory
+
+
+p[-1]=p[0]                # put the first position at the end
+
+tt = np.arange(t[0],t[-1], ts) #time array of servo cycles
 ax=plt.gca()
-ax.xaxis.set_ticks(x)
-markerline, stemlines, baseline = ax.stem(x, y, '-')
-
-yf=np.fft.fft(y)
+ax.xaxis.set_ticks(t)
+markerline, stemlines, baseline = ax.stem(t, p, '-')
 
 
 #best trajectory with lowest frequency
-y_iftf=np.hstack((yf,np.zeros(len(xx)-len(x))))
-y_ift=np.fft.ifft(y_iftf)*w/ts
-ax.plot(xx,y_ift,'-b',label='ift')
+p_iftf=np.fft.fft(p[:-1])
+ft=np.hstack((p_iftf[:k/2],np.zeros((n-1)*k),p_iftf[k/2:]))
+pp_ift=np.fft.ifft(ft)*n
 
+ax.plot(tt,pp_ift,'-b',label='ift')
 
 #plt.figure()
 #ax=plt.gca()
@@ -43,33 +74,35 @@ ax.plot(xx,y_ift,'-b',label='ift')
 #markerline, stemlines, baseline = ax.stem(x, y, '-')
 
 #PVT move
-t=np.hstack((y[-1:],y,y[:1]))
+p2=np.hstack((p[-2],p,p[1]))
 
-n=int(w/ts)
-v=(t[2:]-t[:-2])/(w*2)
+v=(p2[2:]-p2[:-2])/(w*2)
 
-y_pvt=np.ndarray(len(xx))*0
-xx1=xx[:n]
-for i in range(len(x)-1):
-  d=y[i]
+gen_pvt(p,v,t,ts)
+
+
+pp_pvt=np.ndarray(len(tt))*0
+tt1=tt[:n]
+for i in range(len(t)-1):
+  d=p[i]
   c=v[i]
-  a=( -2*(y[i+1]-y[i]-v[i]*w)+   w*(v[i+1]-v[i]))/w**3
-  b=(3*w*(y[i+1]-y[i]-v[i]*w)-w**2*(v[i+1]-v[i]))/w**3
-  y_pvt[i*n:(i+1)*n]=a*xx1**3+b*xx1**2+c*xx1+d
+  a=( -2*(p[i+1]-p[i]-v[i]*w)+   w*(v[i+1]-v[i]))/w**3
+  b=(3*w*(p[i+1]-p[i]-v[i]*w)-w**2*(v[i+1]-v[i]))/w**3
+  pp_pvt[i*n:(i+1)*n]=a*tt1**3+b*tt1**2+c*tt1+d
 
-ax.plot(xx,y_pvt,'-g',label='pvt')
+ax.plot(tt,pp_pvt,'-g',label='pvt')
 
 #PVT move with stop 
 v*=0
-y_p0t=np.ndarray(len(xx))*0
-for i in range(len(x)-1):
-  d=y[i]
+pp_p0t=np.ndarray(len(tt))*0
+for i in range(len(t)-1):
+  d=p[i]
   c=v[i]
-  a=( -2*(y[i+1]-y[i]-v[i]*w)+   w*(v[i+1]-v[i]))/w**3
-  b=(3*w*(y[i+1]-y[i]-v[i]*w)-w**2*(v[i+1]-v[i]))/w**3
-  y_p0t[i*n:(i+1)*n]=a*xx1**3+b*xx1**2+c*xx1+d
+  a=( -2*(p[i+1]-p[i]-v[i]*w)+   w*(v[i+1]-v[i]))/w**3
+  b=(3*w*(p[i+1]-p[i]-v[i]*w)-w**2*(v[i+1]-v[i]))/w**3
+  pp_p0t[i*n:(i+1)*n]=a*tt1**3+b*tt1**2+c*tt1+d
 
-ax.plot(xx,y_p0t,'-r',label='p0t')
+ax.plot(tt,pp_p0t,'-r',label='p0t')
 
 ax.legend(loc='best')
 plt.show(block=False)
@@ -78,26 +111,28 @@ plt.show(block=False)
 fig=plt.figure()
 ax=fig.add_subplot(1,1,1)#ax=plt.gca()
 
-y_iftf=np.fft.fft(y_ift)
-y_pvtf=np.fft.fft(y_pvt)
-y_p0tf=np.fft.fft(y_p0t)
+#normalize with l -> value of k means amplitude of k at a given frequency
+pp_iftf=np.fft.rfft(pp_ift)/(2*n)
+pp_pvtf=np.fft.rfft(pp_pvt)/(2*n)
+pp_p0tf=np.fft.rfft(pp_p0t)/(2*n)
 
+f=np.fft.rfftfreq(pp_ift.shape[0], d=ts*1E-3)
+f=f[1:] #remove dc value frequency
 
-#f=np.arange(0,1E3/(2*ts),1E3/(2*ts*(len(xx)-1)))
-f=np.linspace(0,1E3/(2*ts),len(xx))
-
-db_mag=20*np.log10(abs(y_iftf))
-ax.semilogx(f,db_mag,'-b',label='ift')  # Bode magnitude plot
-db_mag=20*np.log10(abs(y_pvtf))
-ax.semilogx(f,db_mag,'-g',label='pvt')  # Bode magnitude plot
-db_mag=20*np.log10(abs(y_p0tf))
-ax.semilogx(f,db_mag,'-r',label='p0t')  # Bode magnitude plot
-ax.yaxis.set_label_text('dB ampl')
+mag=abs(pp_iftf[1:])#; mag=20*np.log10(abs(mag))
+ax.semilogx(f,mag,'-b',label='ift')  # Bode magnitude plot
+mag=abs(pp_pvtf[1:])#; mag=20*np.log10(abs(mag))
+ax.semilogx(f,mag,'-g',label='pvt')  # Bode magnitude plot
+mag=abs(pp_p0tf[1:])#; mag=20*np.log10(abs(mag))
+ax.semilogx(f,mag,'-r',label='p0t')  # Bode magnitude plot
+#ax.yaxis.set_label_text('dB ampl')
+ax.yaxis.set_label_text('ampl')
 ax.xaxis.set_label_text('frequency [Hz]')
 plt.grid(True)
 
 ax.legend(loc='best')
 plt.show(block=False)
+plt.show()