wip

python/shapepath.py

@@ -233,7 +233,10 @@ class ShapePath(MotionBase):
       mode=-1 jog a 10mm square
       mode=0  linear motion
       mode=1  pvt motion
+              kwargs:  scale: scaling velocity (default=1. value=0 would stop at the point
       mode=2  spline motion
+      mode=3  pvt motion using inverse fft velocity
+              kwargs:  scale: scaling velocity (default=1. value=0 would stop at the point
       kwargs:
         pt2pt_time : time to move from one point to the next point
         sync_frq   : synchronization mark all n points
@@ -268,8 +271,9 @@ class ShapePath(MotionBase):
           prg.append('X%g Y%g'%tuple(pos[idx,:]))
         prg.append('dwell 100')
         prg.append('Gather.Enable=0')
-      elif mode==1: #### pvt motion
+      elif mode in (1,3): #### pvt motion
         pt2pt_time=kwargs.get('pt2pt_time', 100)
+        scale=kwargs.get('scale', 1.)
         self.meta['pt2pt_time']=pt2pt_time
         cnt=kwargs.get('cnt', 1)  # move path multiple times
         sync_frq=kwargs.get('sync_frq', 10)  # synchronization mark all n points
@@ -277,19 +281,25 @@ class ShapePath(MotionBase):
           pt=self.ptsCorr
         except AttributeError:
           pt=self.points
-        vel=pt[2:,:]-pt[:-2,:]
+
         #pv is an array of posx posy velx vely
         pv=np.ndarray(shape=(pt.shape[0]+2,4),dtype=pt.dtype)
         pv[:]=np.NaN
-        #pv[     0,(0,1)]=2*pt[0,:]-pt[1,:]
         pv[     0,(0,1)]=pt[0,:]
         pv[  1:-1,(0,1)]=pt
-        #pv[    -1,(0,1)]=2*pt[-1,:]-pt[-2,:]
         pv[    -1,(0,1)]=pt[-1,:]
         pv[(0,0,-1,-1),(2,3,2,3)]=0
-        dist=pv[2:,(0,1)] - pv[:-2,(0,1)]
-        pv[  1:-1,(2,3)] = 1000.*dist/(2.*pt2pt_time)
-
+        if mode==1: # set velocity to average from prev to next point
+          dist=pv[2:,(0,1)] - pv[:-2,(0,1)]
+          pv[  1:-1,(2,3)] = 1000.*dist/(2.*pt2pt_time)*scale
+        else: #mode=3: set velocity to the reconstructed inverse fourier transformation
+          k=pt.shape[0]
+          f=np.fft.fftfreq(k, d=1./k)
+          pf=np.fft.fft(pt.T)
+          pfd=pf*f*1j  # differentiate in fourier
+          pd=np.fft.ifft(pfd)
+          v=pd.real/(k*2*np.pi)
+          pv[  1:-1,(2,3)] = 1000.*v.T/(pt2pt_time)*scale # FACTORS HAS TO BEEN CHECKED
         prg.append('  linear abs')
         prg.append('X%g Y%g' % tuple(pv[0, (0,1)]))
         prg.append('dwell 10')

python/trajectory.py

@@ -17,6 +17,31 @@ import numpy as np
 import matplotlib as mpl
 import matplotlib.pyplot as plt
 
+np.set_printoptions(precision=3, suppress=True)
+
+def derivate_fft_test():
+  n=32.
+  frq=1.
+  t=np.arange(n)/n*2*np.pi
+  p=np.sin(t*frq)  # position array of trajectory
+
+  pf=np.fft.fft(p)
+  print (pf)
+  f=np.fft.fftfreq(pf.shape[0], d=1/n)
+
+  pfd=pf*f*1j #differentiate in fourier
+  print (pfd)
+  pd=np.fft.ifft(pfd)
+  print (pd)
+
+  ax=plt.figure().add_subplot(1, 1, 1)
+  ax.plot(t, p, '.-b', label='p')
+  ax.plot(t, pd, '.-r', label='pd')
+  ax.grid(True)
+  plt.show(block=False)
+
+  pass
+
 def gen_pvt(p,v,t,ts):
   '''generates a pvt motion
      p: position array
@@ -25,9 +50,8 @@ def gen_pvt(p,v,t,ts):
      ts: servo cycle time
      !!! it is assumed, that the time intervals are constant !!!
      '''
-  return
-  pvt=np.ndarray(len(tt))*0
-  t[-1]/ts
+  pvt=np.ndarray(int(t[-1]/ts))*0
+
 
   tt1=np.arange(0,t[1]-t[0],ts)
   for i in range(len(t)-1):
@@ -39,29 +63,31 @@ def gen_pvt(p,v,t,ts):
 
   return pvt
 
+#derivate_fft_test()
 w=40.  # ms step between samples
 ts=.2 # sampling time
 n=int(w/ts)# servo cycle between samples
-k=8  #number of unique samples
+k=32  #number of unique samples
 
 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=3.*np.sin(1.3+2.*t/(w*k)*2.*np.pi)+10.
+#p+=np.cos(1.5*t/(w*k)*2.*np.pi)
+p=np.cos(8*t*np.pi*2./(k*w))  #eine schwingung
+#np.random.seed(10)
+#p=np.random.random(k+1)*4.  #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=plt.figure().add_subplot(1, 1, 1)
 ax.xaxis.set_ticks(t)
 markerline, stemlines, baseline = ax.stem(t, p, '-')
 
 
-#best trajectory with lowest frequency
+#### best trajectory with lowest frequency ###
 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
@@ -73,41 +99,41 @@ ax.plot(tt,pp_ift,'-b',label='ift')
 #ax.xaxis.set_ticks(x)
 #markerline, stemlines, baseline = ax.stem(x, y, '-')
 
-#PVT move
+### PVT move ###
 p2=np.hstack((p[-2],p,p[1]))
-
 v=(p2[2:]-p2[:-2])/(w*2)
 
-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*(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
-
+pp_pvt=gen_pvt(p,v,t,ts)
 ax.plot(tt,pp_pvt,'-g',label='pvt')
 
-#PVT move with stop 
+### PVT move with stop ###
 v*=0
-pp_p0t=np.ndarray(len(tt))*0
-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
-  pp_p0t[i*n:(i+1)*n]=a*tt1**3+b*tt1**2+c*tt1+d
-
+pp_p0t=gen_pvt(p,v,t,ts)
 ax.plot(tt,pp_p0t,'-r',label='p0t')
 
+
+### PVT with ift velocity move -> PFT ###
+f=np.fft.fftfreq(k, d=1./k)
+p_pftf=np.fft.fft(p[:-1])
+p_pftfd=p_pftf*f*1j  # differentiate in fourier
+print (p_pftfd)
+p_pftd=np.fft.ifft(p_pftfd)
+print (p_pftd)
+
+p_pftd=np.hstack((p_pftd,p_pftd[0]))
+#ax2=plt.figure().add_subplot(1,1,1)
+#ax2.plot(t,p_pftd,'-b',label='dift')
+#ax2.grid(True)
+v=p_pftd.real/(k*2*np.pi)
+pp_pft=gen_pvt(p,v,t,ts)
+ax.plot(tt,pp_pft,'-c',label='pft')
+
 ax.legend(loc='best')
 plt.show(block=False)
 
 
+### frequency plots ###
+
 fig=plt.figure()
 ax=fig.add_subplot(1,1,1)#ax=plt.gca()
 
@@ -115,6 +141,7 @@ ax=fig.add_subplot(1,1,1)#ax=plt.gca()
 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)
+pp_pftf=np.fft.rfft(pp_pft)/(2*n)
 
 f=np.fft.rfftfreq(pp_ift.shape[0], d=ts*1E-3)
 f=f[1:] #remove dc value frequency
@@ -125,6 +152,8 @@ 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
+mag=abs(pp_pftf[1:])#; mag=20*np.log10(abs(mag))
+ax.semilogx(f,mag,'-c',label='pft')  # Bode magnitude plot
 #ax.yaxis.set_label_text('dB ampl')
 ax.yaxis.set_label_text('ampl')
 ax.xaxis.set_label_text('frequency [Hz]')