From 5aacb85c8d81518cbca8bb9bcdcf05613cbc5ae5 Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Mon, 14 Jan 2019 15:58:35 +0100 Subject: [PATCH] working trajectory generation with ift in shapepath --- python/shapepath.py | 153 ++++++++++++++++++++++++++++++++++++------- python/trajectory.py | 6 +- 2 files changed, 134 insertions(+), 25 deletions(-) diff --git a/python/shapepath.py b/python/shapepath.py index 4ea01fe..3ef9da5 100755 --- a/python/shapepath.py +++ b/python/shapepath.py @@ -9,11 +9,12 @@ shape an optimal path with given points verbose bits: - 1 basic info - 2 plot sorting steps - 4 list program - 4 upload progress - 8 plot gather path + 1 basic info + 2 plot sorting steps + 4 list program + 4 upload progress + 8 plot gather path + 16 plot pvt trajectory (before motion) Gather motor order @@ -53,6 +54,90 @@ from pbtools.misc.gather import Gather from MXMotion import MotionBase +def gen_pvt(p,v, p2pt, 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 !!! + ''' + n=int(p2pt/ts) + pvt=np.ndarray((p.shape[0]-1)*n)*0 + + tt=np.arange(0,p2pt,ts) + for i in range(p.shape[0]-1): + d=p[i] + c=v[i] + a=(-2*(p[i+1]-p[i]-v[i]*p2pt)+p2pt*(v[i+1]-v[i]))/p2pt**3 + b=(3*p2pt*(p[i+1]-p[i]-v[i]*p2pt)-p2pt**2*(v[i+1]-v[i]))/p2pt**3 + pvt[i*n:(i+1)*n]=a*tt**3+b*tt**2+c*tt+d + + return pvt + + +def debugplot_pvt(pv, meta): + # pv is an array of posx posy velx vely + #pv=pv[5:10,:] + #pv=pv[5:-4,:] + p2pt=meta['pt2pt_time'] # ms step between samples + ts=meta['timebase'] # sampling time in ms + n=int(p2pt/ts) # servo cycle between samples + k=pv.shape[0] # number of unique samples + t=np.arange(0, p2pt*k, p2pt) # time array of trajectory + ppx=gen_pvt(pv[:,0], pv[:,2], p2pt, ts) + ppy=gen_pvt(pv[:,1], pv[:,3], p2pt, ts) + tt=np.arange(0, n*(k-1))*ts # time array of trajectory + + fig=plt.figure() + ax1=fig.add_subplot(2, 1, 1) + ax2=fig.add_subplot(2, 1, 2) + #ax.xaxis.set_ticks(t) + ax1.stem(t, pv[:,0], '-r') + ax2.stem(t, pv[:,1], '-g') + + ax1.plot(tt, ppx, '-r', label='x') + ax2.plot(tt, ppy, '-g', label='y') + #ax.legend(loc='best') + + ax=plt.figure().add_subplot(1, 1, 1) + ax.plot(pv[:,0], pv[:,1], '.r', label='pft') + ax.plot(ppx, ppy, '-c', label='pft') + ax.invert_xaxis() + ax.invert_yaxis() + plt.axis('equal') + + ax.legend(loc='best') + plt.show(block=False) + + # ### frequency plots ### + # fig=plt.figure() + # ax=fig.add_subplot(1,1,1)#ax=plt.gca() + # + # #remove linear slope + # sx=ppx-(pv[-1,0]-pv[0,0])*np.arange(ppx.shape[0]) + # sy=ppy-(pv[-1,1]-pv[0,1])*np.arange(ppy.shape[0]) + # + # #normalize with l -> value of k means amplitude of k at a given frequency + # ppxf=np.fft.rfft(sx)/(2*n) + # ppyf=np.fft.rfft(sy)/(2*n) + # + # f=np.fft.rfftfreq(ppx.shape[0], d=ts*1E-3) + # f=f[1:] #remove dc value frequency + # + # mag=abs(ppxf[1:])#; mag=20*np.log10(abs(mag)) + # ax.semilogx(f,mag,'-b',label='ppx') # Bode magnitude plot + # mag=abs(ppyf[1:])#; mag=20*np.log10(abs(mag)) + # ax.semilogx(f,mag,'-g',label='ppy') # 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) + + class ShapePath(MotionBase): def __init__(self,comm, gather, verbose): MotionBase.__init__(self,comm, gather, verbose) @@ -243,7 +328,8 @@ class ShapePath(MotionBase): ''' prg=['close all buffers','open prog %d'%(prgId)] comm=self.comm - gpascii=comm.gpascii + if comm is not None: + gpascii=comm.gpascii # this uses Coord[1].Tm and limits with MaxSpeed if mode==-1: #### jog a 10mm square pos=self.points @@ -273,15 +359,16 @@ class ShapePath(MotionBase): prg.append('Gather.Enable=0') elif mode in (1,3): #### pvt motion pt2pt_time=kwargs.get('pt2pt_time', 100) + ts=self.meta['timebase'] 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 + CoordFeedTime=1000. #Defaut deltatau value try: pt=self.ptsCorr except AttributeError: pt=self.points - #pv is an array of posx posy velx vely pv=np.ndarray(shape=(pt.shape[0]+2,4),dtype=pt.dtype) pv[:]=np.NaN @@ -291,15 +378,25 @@ class ShapePath(MotionBase): pv[(0,0,-1,-1),(2,3,2,3)]=0 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 + pv[ 1:-1,(2,3)] = dist/(2.*pt2pt_time)*scale #um/ms else: #mode=3: set velocity to the reconstructed inverse fourier transformation - k=pt.shape[0] + p=pt.T.copy() #copy + k=p.shape[1] + stp=((p[:,-1]-p[:,0])/(k-1)) #calculate steepness point to point + p[0,:]-=stp[0]*np.arange(k) + p[1,:]-=stp[1]*np.arange(k) f=np.fft.fftfreq(k, d=1./k) - pf=np.fft.fft(pt.T) + pf=np.fft.fft(p) 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 + v=pd.real.T/(k*2*np.pi)+stp/pt2pt_time + pv[ 1:-1,(2,3)] = v*scale + verb=self.verbose + if verb&16: + debugplot_pvt(pv, self.meta) + plt.show() + + pv[1:-1, (2, 3)]*=CoordFeedTime #scaling for Deltatau prg.append(' linear abs') prg.append('X%g Y%g' % tuple(pv[0, (0,1)])) prg.append('dwell 10') @@ -365,14 +462,9 @@ class ShapePath(MotionBase): fh=open(fnPrg,'w') fh.write('\n'.join(prg)) fh.close() - gpascii.send_block(prg) + if comm is not None: + gpascii.send_block(prg) - #if self.host is not None: - # cmd ='gpasciiCommander --host '+self.host+' '+ fnPrg - # print(cmd) - # p = sprc.Popen(cmd, shell=True)#, stdout=sprc.PIPE, stderr=sprc.STDOUT) - # #res=p.stdout.readlines(); print res - # retval = p.wait() self.prg=prg def gather_upload(self,fnRec=None): @@ -632,7 +724,7 @@ if __name__=='__main__': def run_test(args): - #args.host=None + args.host=None if args.host is None: comm=gather=None else: @@ -663,18 +755,35 @@ if __name__=='__main__': #sp.plot_gather() #return fn='/tmp/shapepath' - fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/record/grid_delay_0002' + #fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/record/grid_delay_0002' # /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/record/chip000_sortedXXXX_9x9' #fn='/sf/bernina/data/p17592/res/20181203/imprints/chip000_preloc_min10um_sortedX_refs_goaround/chip000_sortedY_9x9.npz' #fn='PBMotionAnalyzer/records/rand50um_25Hz' #sp.setup_coord_trf() #sp.points=np.zeros((2,2)) - #sp.meta={'timebase':.2} + sp.meta={'timebase':.2} #sp.gather_upload(fnRec=fn+'.npz') #sp.plot_gather() #return xy=False + sp.gen_rand_points(n=14, scale=1000);sp.sort_points(xy=xy) + #print(sp.points[:,0]) + #sp.gen_swissmx_points(width=1000, ofs=(-500, 0)); + #sp.points=np.array([[0.,1.],[1.,0.],[0.,-1.],[-1.,0.]]) + sp.points=np.array([[0.,1.],[1.,0.],[0.,-1.],[-1.,0.], + [0., 1.], [1., 0.], [0., -1.], [-1., 0.], + [0., 1.], [1., 0.], [0., -1.], [-1., 0.], + [0., 1.], [1., 0.], [0., -1.], [-1., 0.], + [0., 1.], [1., 0.], [0., -1.], [-1., 0.], + [0., 1.], [1., 0.], [0., -1.], [-1., 0.], [0., 1.]]) + #sp.gen_grid_points(w=10,h=10,pitch=100,rnd=0,ofs=(0,0));sp.sort_points(False); + #sp.gen_grid_points(w=1,h=10,pitch=100,rnd=0,ofs=(0,0)) + sp.setup_motion(fnPrg=fn + '.prg', mode=1, pt2pt_time=40,scale=0) + sp.setup_motion(fnPrg=fn + '.prg', mode=1, pt2pt_time=40,scale=1) + sp.setup_motion(fnPrg=fn + '.prg', mode=3, pt2pt_time=40,scale=1) + exit(0) + # fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/data/'+time.strftime('%y-%m-%d-%H_%M_%S') #>>>point source and sorting<<< diff --git a/python/trajectory.py b/python/trajectory.py index dadcdf2..695e47a 100755 --- a/python/trajectory.py +++ b/python/trajectory.py @@ -74,9 +74,9 @@ t = np.arange(0, w*(k+1), w) #time array of trajectory #p=3.*np.cos(t)+4. #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=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