From ebfa99d27cc02739140d0a425a7cb738b1cccb5e Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Fri, 11 Jan 2019 12:10:31 +0100 Subject: [PATCH] wip --- python/shapepath.py | 24 +++++++---- python/trajectory.py | 95 +++++++++++++++++++++++++++++--------------- 2 files changed, 79 insertions(+), 40 deletions(-) diff --git a/python/shapepath.py b/python/shapepath.py index 73e4f62..4ea01fe 100755 --- a/python/shapepath.py +++ b/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') diff --git a/python/trajectory.py b/python/trajectory.py index 1315681..dadcdf2 100755 --- a/python/trajectory.py +++ b/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]')