#!/usr/bin/env python # *-----------------------------------------------------------------------* # | | # | Copyright (c) 2016 by Paul Scherrer Institute (http://www.psi.ch) | # | | # | Author Thierry Zamofing (thierry.zamofing@psi.ch) | # *-----------------------------------------------------------------------* ''' 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 #config file example: { "points": [ [100,523],[635,632],[756,213], "sequencer":[ 'gen_grid_points(w=10,h=10,pitch=100,rnd=.2)', 'sort_points()', 'gen_prog(file="'+fn+'.prg")', 'plot_gather("'+fn+'.npz")'] } Sequencer functions are: - generate points (if not in the 'points' configuration) gen_rand_points(self,n=107,scale=1000) gen_grid_points(w=10,h=10,pitch=100,rnd=.2) - sorting points: sort_points(self) - generate/download/execute motion progran, upload trace of motors (gather data) gen_prog(self,prgId=2,file=None,host=None) if host=None nothing will be downloaded/executed and trace of motors will not be uploaded if file=None the program will not be saved and nothing will be executed gen_prog modes: -1 jog a 10mm square 0 linear motion 1 pvt motion 2 spline motion - plot gathered data plot_gather("'+fn+'.npz") this makes only sence, if motion has been executed and data can be gathered from the powerbrick Acquired time is:MaxSamples*Period*.2 ''' import os, sys, json import numpy as np import matplotlib as mpl import matplotlib.pyplot as plt import subprocess as sprc import telnetlib from utilities import * class ShapePath: def __init__(self,args): if args.cfg: fh=open(args.cfg,'r') s=fh.read() cfg=json.loads(s, object_hook=ConvUtf8) s=json.dumps(cfg, indent=2, separators=(',', ': '));print s else: fn='/tmp/shapepath' #fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/data/'+time.strftime('%y-%m-%d-%H_%M_%S') #cfg={"points": [[100,523],[635,632],[756,213]],"sequencer":['sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871")','plot_gather("'+fn+'.npz")']} #cfg={"sequencer":['gen_rand_points(n=107, scale=1000)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871")','plot_gather("'+fn+'.npz")']} #cfg={"sequencer":['gen_grid_points(w=10,h=10,pitch=100,rnd=.2)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871")','plot_gather("'+fn+'.npz")']} #cfg={"sequencer":['gen_grid_points(w=10,h=10,pitch=100,rnd=0.2)','sort_points()','gen_prog(file="'+fn+'.prg")','plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_rand_points(n=107, scale=1000)', 'sort_points()','plot_gather("'+fn+'.npz")']} #cfg={"sequencer":['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871")','plot_gather("'+fn+'.npz")']} #cfg={"sequencer":['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)','sort_points()','gen_prog(file="'+fn+'.prg")','plot_gather("'+fn+'.npz")']} #cfg={"sequencer":['gen_rand_points(n=400, scale=1000)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871")','plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=50,rnd=.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=1)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=100,rnd=0)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=100,rnd=0.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather("'+fn+'.npz")']} #cfg = {"points": [[0, 0],[100, 0],[200, 0],[300, 0],[400, 0],[400, 100],[300, 100],[200, 100],[100, 100],[0, 100],[10, 200],[100, 200],[200, 200],[300, 200],[400, 200],[410, 300],[300, 300],[200, 300],[100, 300],[0, 300],[0, 400],[100, 400],[200, 400],[300, 400],[400, 400]],"sequencer": ['gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=10000,rnd=0)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=-1)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=10000,rnd=0)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=1000)', 'plot_gather("'+fn+'.npz")']} cfg = {"sequencer": ['gen_grid_points(w=20,h=20,pitch=50,rnd=0.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=10,acq_per=10)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_rand_points(n=400, scale=1000)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=10)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=100,rnd=0.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=2,pt2pt_time=10)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer":['gen_rand_points(n=400, scale=1000)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=10,acq_per=1)','plot_gather("'+fn+'.npz")']} self.cfg=dotdict(cfg) self.args=args def run(self): print('args='+str(self.args)) print('cfg='+str(self.cfg)) try: self.points=np.array(self.cfg.points) except AttributeError: pass try: sequencer= self.cfg.pop('sequencer') except KeyError: print('no command sequence to execute') else: dryrun=self.args.dryrun for cmd in sequencer: print '>'*5+' '+cmd+' '+'<'*5 if not dryrun: eval('self.' + cmd) def gen_rand_points(self,n=107,scale=1000): np.random.seed(0) #data=np.random.randint(0,1000,(30,2)) pts=np.random.rand(n,2)*scale self.points=pts def gen_grid_points(self,w=10,h=10,pitch=100,rnd=.2): np.random.seed(0) xx,yy=np.meshgrid(range(w), range(h)) pts=np.array([xx.reshape(-1),yy.reshape(-1)],dtype=np.float).transpose()*pitch if rnd != 0: pts+=(np.random.rand(pts.shape[0],2)*(rnd*pitch)) self.points=pts def gen_prog(self,prgId=2,file=None,host=None,mode=0,**kwargs): ''' kwargs: acq_per : acquire period: acquire data all acq_per servo loops (default=1) pt2pt_time : time to move from one point to the next point ''' prg=[] acq_per=kwargs.get('acq_per',1) gather={"MaxSamples":1000000, "Period":acq_per} #Sys.ServoPeriod is dependent of !common() macro ServoPeriod= .2 #0.2ms #ServoPeriod = .05 self.meta = {'timebase': ServoPeriod*gather['Period']} #channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos"] channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos","Motor[1].DesPos","Motor[2].DesPos","Motor[3].DesPos"] prg.append('Gather.Enable=0') prg.append('Gather.Items=%d'%len(channels)) for k,v in gather.iteritems(): prg.append('Gather.%s=%d'%(k,v)) for i,c in enumerate(channels): prg.append('Gather.Addr[%d]=%s.a'%(i,c)) prg.append('open prog %d'%(prgId)) # this uses Coord[1].Tm and limits with MaxSpeed if mode==-1: #### jog a 10mm square pos=self.points prg.append(' linear abs') prg.append('X(%g) Y(%g)' % tuple(pos[0, :])) prg.append('dwell 10') prg.append('Gather.Enable=2') prg.append('jog2:10000') prg.append('dwell 100') prg.append('jog3:10000') prg.append('dwell 100') prg.append('jog2:-10000') prg.append('dwell 100') prg.append('jog3:-10000') prg.append('dwell 100') prg.append('Gather.Enable=0') elif mode==0: #### linear motion pos=self.points prg.append(' linear abs') prg.append('X(%g) Y(%g)' % tuple(pos[0, :])) prg.append('dwell 10') prg.append('Gather.Enable=2') prg.append(' linear abs') for idx in range(pos.shape[0]): prg.append('X%g Y%g'%tuple(pos[idx,:])) prg.append('dwell 100') prg.append('Gather.Enable=0') elif mode==1: #### pvt motion try: pt2pt_time=kwargs['pt2pt_time'] #how many ms to move to next point (pt2pt_time) except KeyError: print 'missing pt2pt_time, use default=100ms' pt2pt_time=100. 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) prg.append(' linear abs') prg.append('X%g Y%g' % tuple(pv[0, (0,1)])) prg.append('dwell 10') prg.append('Gather.Enable=2') prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position for idx in range(1,pv.shape[0]): prg.append('X%g:%g Y%g:%g'%tuple(pv[idx,(0,2,1,3)])) prg.append('X%g Y%g' % tuple(pv[-1, (0,1)])) prg.append('dwell 1000') prg.append('Gather.Enable=0') elif mode==2: #### spline motion try: pt2pt_time=kwargs['pt2pt_time'] #how many ms to move to next point (pt2pt_time) except KeyError: print 'missing pt2pt_time, use default=100ms' pt2pt_time=100. pos=self.points pcor=np.ndarray(pos.shape,dtype=pos.dtype);pcor[:]=np.NaN pcor[(0,-1),:]=pos[(0,-1),:] pcor[1:-1,:]=(-pos[0:-2,:]+8*pos[1:-1,:]-pos[2:,:])/6. #pcor=pos prg.append(' linear abs') prg.append('X(%g) Y(%g)' % tuple(pcor[0, :])) prg.append('dwell 10') prg.append('Gather.Enable=2') prg.append(' spline%g abs'%pt2pt_time) #100ms to next position for idx in range(pcor.shape[0]): prg.append('X%g Y%g'%tuple(pcor[idx,:])) prg.append('dwell 100') prg.append('Gather.Enable=0') prg.append('close') prg.append('&1\nb%dr\n'%prgId) if self.args.verbose & 4: for ln in prg: print(ln) if file is not None: fh=open(file,'w') fh.write('\n'.join(prg)) fh.close() if host is not None: cmd ='gpasciiCommander --host '+host+' '+ file print cmd p = sprc.Popen(cmd, shell=True)#, stdout=sprc.PIPE, stderr=sprc.STDOUT) #res=p.stdout.readlines(); print res retval = p.wait() #gather -u /var/ftp/gather/out.txt cmd ='PBGatherPlot -m24 -v7 --host '+host print cmd p = sprc.Popen(cmd, shell=True)#, stdout=sprc.PIPE, stderr=sprc.STDOUT) retval = p.wait() self.prg=prg def sort_points(self): pts=self.points verb=self.args.verbose #if verb&2: # self.plot_points(pts) cnt=pts.shape[0] idx=np.ndarray(cnt,dtype=np.int32) grp_cnt=int(np.sqrt(cnt)) grp_sz=int(np.ceil(float(cnt)/grp_cnt)) if self.args.yx==True: idxA=0;idxB=1 else: idxA=1;idxB=0 #sort points along idxA pts=pts[pts[:,idxA].argsort()] #group sorting along idxB for i in range(grp_cnt): a=i*grp_sz #print a,a+grp_sz if i%2: idx[a:a+grp_sz]=a+pts[a:a+grp_sz,idxB].argsort()[::-1] else: idx[a:a+grp_sz]=a+pts[a:a+grp_sz,idxB].argsort() #print(idx) pts=pts[idx] if verb&2: self.plot_points(pts) plt.show() self.points=pts @staticmethod def onclick(event): print 'button=%s, x=%d, y=%d, xdata=%f, ydata=%f'%( event.button, event.x, event.y, event.xdata, event.ydata) obj=event.canvas.figure.obj def plot_points(self,pts): fig=plt.figure() ax = fig.add_subplot(1,1,1) #hl=ax[0].plot(x, y, color=col) hl=ax.plot(pts[:,0],pts[:,1],'r.') hl=ax.plot(pts[:,0],pts[:,1],'y--') cid = fig.canvas.mpl_connect('button_press_event', self.onclick) fig.obj=self self.ax=ax self.hl=hl def plot_gather(self,fnOut='/tmp/shapepath.npz',fnLoc='/tmp/gather.txt'): meta=self.meta pts=self.points # X,Y array rec = np.genfromtxt(fnLoc, delimiter=' ') #rec=Motor[1].ActPos,Motor[2].ActPos,Motor[3].ActPos,Motor[1].DesPos,Motor[2].DesPos,Motor[3].DesPos #res=rot.ActPos,y.ActPos,x.ActPos,rot.DesPos,y.DesPos,x.DesPos #idx 0 1 2 3 4 5 if fnOut: # time base: # Gather.Period=10 # Sys.ServoPeriod=0.2 -> 5kHz # -> aquired data all 10*0.2ms= 2 ms -> 30000 points = 60 sec np.savez_compressed(fnOut, rec=rec, pts=pts, meta=meta) fig=plt.figure() ax = fig.add_subplot(1,1,1) #hl=ax[0].plot(x, y, color=col) hl=ax.plot(pts[:,0],pts[:,1],'r.') hl=ax.plot(pts[:,0],pts[:,1],'y--') hl = ax.plot(rec[:, 5], rec[:, 4], 'b-') # desired path hl=ax.plot(rec[:,2],rec[:,1],'g-') # actual path ax.xaxis.set_label_text('x-pos um') ax.yaxis.set_label_text('y-pos um') cid = fig.canvas.mpl_connect('button_press_event', self.onclick) fig.obj=self self.ax=ax self.hl=hl fig = plt.figure() ax = fig.add_subplot(1, 1, 1) errx=rec[:,2]-rec[:,5] erry=rec[:,1]-rec[:,4] err=np.sqrt(errx**2+erry**2) hl = [] hl += ax.plot(errx, 'b-',label='x-error') hl += ax.plot(erry, 'g-',label='y-error') hl += ax.plot(err, 'r-',label='error') ax.xaxis.set_label_text('datapoint (timebase: %g ms per data point)'%meta['timebase']) ax.yaxis.set_label_text('pos-error um') legend = ax.legend(loc='upper right', shadow=True) print('average error x %g um, y %g um, %g um'%(np.abs(errx).mean(),np.abs(erry).mean(),err.mean())) plt.show() if __name__=='__main__': from optparse import OptionParser, IndentedHelpFormatter class MyFormatter(IndentedHelpFormatter): 'helper class for formating the OptionParser' def __init__(self): IndentedHelpFormatter.__init__(self) def format_epilog(self, epilog): if epilog: return epilog else: return "" def parse_args(): 'main command line interpreter function' #usage: gpasciiCommunicator.py --host=PPMACZT84 myPowerBRICK.cfg (h, t)=os.path.split(sys.argv[0]);cmd='\n '+(t if len(h)>3 else sys.argv[0])+' ' exampleCmd=('--host=PPMAC1391 -m 63 --cfg gather.cfg', 'samplePowerBrick.cfg', '-n stackCheck1.cfg', '--host=PPMACZT84 stackCheck1.cfg', '--host=PPMACZT84 stackCheck1.cfg -v15', ) epilog=__doc__+''' Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' fmt=MyFormatter() parser=OptionParser(epilog=epilog, formatter=fmt) parser.add_option('-v', '--verbose', type="int", dest='verbose', help='verbosity bits (see below)', default=0) parser.add_option('-n', '--dryrun', action='store_true', help='dryrun to stdout') parser.add_option('--yx', action='store_true', help='sort y,x instead x,y') parser.add_option('--cfg', help='config file containing json configuration structure') (args, other)=parser.parse_args() args.other=other sp=ShapePath(args) sp.run() #------------------ Main Code ---------------------------------- #ssh_test() ret=parse_args() exit(ret)