#!/usr/bin/env python # *-----------------------------------------------------------------------* # | | # | Copyright (c) 2017 by Paul Scherrer Institute (http://www.psi.ch) | # | | # | Author Thierry Zamofing (thierry.zamofing@psi.ch) | # *-----------------------------------------------------------------------* ''' tools to setup and execute a helical scan of a cristal motors CX CZ RY FY 7 8 1 2 Mot 1: Rotation stage LS Mecapion MDM-DC06DNC0H 32 poles = 1 rev = 16*2048=32768 phase_step Mot 2: Stage Y Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step Mot 3: Stage X Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step Mot/Enc 4: camera base plate X Mot/Enc 5: camera base plate Y Mot 6: Backlight 2.3A Mot 7: Stada Stepper: 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor) Mot 8: Stada Stepper: 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor) verbose bits: #1 basic info #2 plot sorting steps 4 list program #4 upload progress #8 plot gather path ''' import os, sys, json import numpy as np import matplotlib as mpl import matplotlib.pyplot as plt import mpl_toolkits.mplot3d as plt3d import matplotlib.animation as anim from matplotlib.widgets import Slider import subprocess as sprc from utilities import * import telnetlib d2r=2*np.pi/360 #ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y') #plot coordinates: X Y Z #data Z X Y class Trf: #https://stackoverflow.com/questions/6802577/python-rotation-of-3d-vector @staticmethod def rotZ(rad): """ Return matrix for rotating about the z-axis by 'radians' radians """ c = np.cos(rad) s = np.sin(rad) m=np.identity(4) m[0:2,0:2]=((c, -s),(s, c)) return m @staticmethod def rotY(rad): """ Return matrix for rotating about the z-axis by 'radians' radians """ c = np.cos(rad) s = np.sin(rad) m=np.identity(4) m[np.ix_((0,2),(0,2))]=((c, s),(-s, c)) return m @staticmethod def trans(x,y,z): m=np.identity(4) m[0:3, 3] =(x,y,z) return m class HelicalScan: 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/shapepath4' #fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/data/'+time.strftime('%y-%m-%d-%H_%M_%S') #cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=100,rnd=0.4)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,cnt=1)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['test_find_rot_ctr()']} #cfg = {"sequencer": ['test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)']} cfg = {"sequencer": ['test_coord_trf()']} self.cfg=dotdict(cfg) self.args=args def sequencer(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) @staticmethod def test_find_rot_ctr(n=3.,per=1.,bias=4.1,ampl=2.4,phi=37): # find the rotation center, amplitude out of n (niminum 3) measurements # n number of equidistant measurements # per number of periods (full rotation of all measurements nut be a interger value for precise measurements) # phi phase # bias bias value # ampl amplitude t = np.arange(n) w=2*np.pi*per/n*t y=ampl*np.cos(w+phi*d2r)+bias plt.figure(1) plt.subplot(311) plt.plot(t,y,'b.-') plt.subplot(312) f = np.fft.fft(y) plt.step(t, f.real,'b.-', t, f.imag,'r.-', where='mid') (bias,ampl,phase)=HelicalScan.meas_rot_ctr(y, per) print('bias: '+str(bias)) print('amplitude: '+str(ampl)) print('phase: '+str(phase*360./2/np.pi)) plt.subplot(313) t2 = np.linspace(0,2*np.pi,64) y2=ampl*np.cos(t2+phase)+bias plt.plot(t2,y2,'g-') plt.stem(w,y,'b-') plt.show() pass def test_coord_trf(self): param = self.param dx, dz, w, y, = (0.2,0.3,0.1,3.3) print 'input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r,y) (cx,cz,w,fy) = self.inv_transform(dx,dz,w,y) print 'inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r,fy) (dx,dz,w,y) = self.fwd_transform(cx,cz,w,fy) print 'fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r,y) # plt.ion() # fig = plt.figure() # a=anim.FuncAnimation(fig,self.my_anim_func3,100,fargs=(horig,pt),interval=20,blit=False) # plt.show() # def my_anim_func3(self,idx): # self.hCrist,pt=self.pltCrist(cx=0,ty=0,cz=0,w=10*idx*d2r,h=self.hCrist) def show_vel(self): rec=self.rec fig = plt.figure() #y = np.diff(rec[:, 2]) y = np.diff(rec,axis=0) mx=y.max(0) mn=y.min(0) y=y/(mx-mn) x = np.arange(0, y.shape[0]); x= x.reshape(-1,1).dot(np.ones((1,y.shape[1]))) plt.plot(x, y) def interactive_cx_cz_w_fy(self): fig = plt.figure() self.manip=False#True#False self.ax=ax=plt3d.Axes3D(fig,[0.02, 0.15, 0.96, 0.83]) ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y') ax.view_init(elev=14., azim=10) param=self.param # param[i]=(z_i, y_i, x_i, r_i,phi_i) ctr=param[:,0:3].mean(0)[::-1] self.axSetCenter(ctr,10) axCx=plt.axes([0.1, 0.01, 0.8, 0.02]) axCz=plt.axes([0.1, 0.04, 0.8, 0.02]) axW =plt.axes([0.1, 0.07, 0.8, 0.02]) axFy=plt.axes([0.1, 0.10, 0.8, 0.02]) if self.manip: lz=ax.get_xlim() lx=ax.get_ylim() ly=ax.get_zlim() else: lx = ly=lz=[-5,5] self.sldCx=sCx=Slider(axCx, 'cx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.) self.sldCz=sCz=Slider(axCz, 'cz', lz[0], lz[1], valinit=(lz[0]+lz[1])/2.) self.sldW =sW =Slider(axW, 'ang', -180., 180.0, valinit=0) self.sldFy=sFy=Slider(axFy, 'fy', ly[0], ly[1], valinit=(ly[0]+ly[1])/2.) sCx.on_changed(self.update_cx_cz_w_fy) sCz.on_changed(self.update_cx_cz_w_fy) sW.on_changed(self.update_cx_cz_w_fy) sFy.on_changed(self.update_cx_cz_w_fy) hCrist,pt=self.pltCrist() self.hCrist=hCrist;self.fig=fig # param[i]=(z_i, y_i, x_i, r_i,phi_i) p=np.ndarray((param.shape[0], 3)) for i in range(2): (z_i, y_i, x_i, r_i, phi_i)=param[i] p[i,0]=x_i+r_i*np.sin(phi_i) # x= x_i+r_i*cos(phi_i+w)+cx p[i,1]=y_i # y= y_i p[i,2]=z_i+r_i*np.cos(phi_i) # z= z_i+r_i*sin(phi_i*w) print p ofs=(p[1]+p[0])/2. # = center of the cristal m=Trf.trans(*ofs); self.hOrig=self.pltOrig(m) plt.show() def update_cx_cz_w_fy(self,val): cx = self.sldCx.val cz = self.sldCz.val w = self.sldW.val fy = self.sldFy.val if self.manip: param = self.param # param[i]=(z_i, y_i, x_i, r_i,phi_i) p = np.ndarray((param.shape[0], 3)) for i in range(2): (z_i, y_i, x_i, r_i, phi_i) = param[i] p[i, 0] = x_i + r_i * np.cos(phi_i) # x= x_i+r_i*cos(phi_i+w)+cx p[i, 1] = y_i # y= y_i p[i, 2] = z_i + r_i * np.sin(phi_i) # z= z_i+r_i*sin(phi_i*w) print p ofs = (p[1] + p[0]) / 2. # = center of the cristal m = Trf.trans(cx,fy,cz) m= m.dot(Trf.rotY(w*d2r)) self.hOrig = self.pltOrig(m,self.hOrig) else: self.hCrist,pt=self.pltCrist(cx,cz,w*d2r,fy,self.hCrist) #l.set_ydata(amp * np.sin(2 * np.pi * freq * t)) self.fig.canvas.draw_idle() def interactive_dx_dz_w_y(self): fig = plt.figure() self.ax=ax=plt3d.Axes3D(fig,[0.02, 0.15, 0.96, 0.83]) ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y') ax.view_init(elev=14., azim=10) param=self.param # param[i]=(z_i, y_i, x_i, r_i,phi_i) ctr=(0,0,0) self.axSetCenter(ctr,10) axDx=plt.axes([0.1, 0.01, 0.8, 0.02]) axDz=plt.axes([0.1, 0.04, 0.8, 0.02]) axW =plt.axes([0.1, 0.07, 0.8, 0.02]) axY =plt.axes([0.1, 0.10, 0.8, 0.02]) lx=[-1,1];ly=[0,1];lz=[-1,1] ly = param[:,1] self.sldDx=sDx=Slider(axDx, 'dx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.) self.sldDz=sDz=Slider(axDz, 'dz', lz[0], lz[1], valinit=(lz[0]+lz[1])/2.) self.sldW =sW =Slider(axW, 'ang', -180., 180.0, valinit=0) self.sldY =sY =Slider(axY, 'y', ly[0], ly[1], valinit=(ly[0]+ly[1])/2.) sDx.on_changed(self.update_dx_dz_w_y) sDz.on_changed(self.update_dx_dz_w_y) sW.on_changed(self.update_dx_dz_w_y) sY.on_changed(self.update_dx_dz_w_y) # param[i]=(z_i, y_i, x_i, r_i,phi_i) p=np.ndarray((param.shape[0], 3)) for i in range(2): (z_i, y_i, x_i, r_i, phi_i)=param[i] p[i,0]=x_i+r_i*np.sin(phi_i) # x= x_i+r_i*cos(phi_i+w)+cx p[i,1]=y_i # y= y_i p[i,2]=z_i+r_i*np.cos(phi_i) # z= z_i+r_i*sin(phi_i*w) ofs=(p[1]+p[0])/2. # = center of the cristal print 'p, ofs',p,ofs m=Trf.trans(0,0,0); self.hOrig=self.pltOrig(m) hCrist,pt=self.pltCrist(cx=-ofs[0],fy=-ofs[1],cz=-ofs[2]) self.hCrist=hCrist;self.fig=fig plt.show() def update_dx_dz_w_y(self,val): dx = self.sldDx.val dz = self.sldDz.val w = self.sldW.val y = self.sldY.val w=w*d2r (cx,cz,w,fy)=self.inv_transform(dx,dz,w,y) #print (cx,cz,w,fy) self.hCrist,pt=self.pltCrist(-cx,-cz,w,-fy,self.hCrist) self.fig.canvas.draw_idle() def interactive_anim(self): fig = plt.figure() self.ax=ax=plt3d.Axes3D(fig,[0.02, 0.15, 0.96, 0.83]) ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y') ax.view_init(elev=14., azim=10) param=self.param # param[i]=(z_i, y_i, x_i, r_i,phi_i) ctr=(0,0,0) self.axSetCenter(ctr,10) axFrm=plt.axes([0.1, 0.01, 0.8, 0.02]) lx=[-1,1];ly=[0,1];lz=[-1,1] ly = param[:,1] self.sldFrm=sFrm=Slider(axFrm, 'frm', 0, self.rec.shape[0]-1, valinit=0) sFrm.on_changed(self.update_anim) m=Trf.trans(0,0,0); self.hOrig=self.pltOrig(m) self.hCrist=None self.fig=fig animCnt=100 self.step=self.rec.shape[0]/animCnt #a = anim.FuncAnimation(fig, self.anim_gather_data, animCnt, fargs=(), interval=20, repeat=False, blit=False) self.update_anim(0) plt.show() pass def update_anim(self,frm): (cx, cz, w, fy)=self.rec[int(frm),:] #data/=. #scale from um to mm w*=d2r/1000 # scale from deg to rad print (cx,cz,w,fy) self.hCrist,pt=self.pltCrist(-cx,-cz,w,-fy,self.hCrist) self.fig.canvas.draw_idle() def anim_gather_data(self,idx): (cx, cz, w, fy)=self.rec[int(idx*self.step),:] w*=d2r/1000 # scale from deg to rad self.hCrist,pt=self.pltCrist(-cx,-cz,w,-fy,self.hCrist) #data=self.rec[int(idx*self.step),:] #self.hCrist,pt=self.pltCrist(*data,h=self.hCrist) def save_rec(self,fn_npz='/tmp/helicalscan.npz'): param= self.param meta= self.meta points = self.points # target points rec = self.rec np.savez_compressed(fn_npz, rec=rec, points=points, param=param, meta=meta) def load_rec(self,fn_npz='/tmp/helicalscan.npz'): try: fh=np.load(fn_npz) except IOError as e: sys.stderr.write('Unable to open File: '+fn_npz+'\n') else: pass s='content of numpy file: '+fn_npz+'\n' for k,v in fh.iteritems(): s+=' '+k+': '+str(v.dtype)+' '+str(v.shape)+'\n' setattr(self,k,v) print s def fwd_transform(self,cx,cz,w,fy): #cx,cy: coarse stage #input: cx,cz,w,fy #output: dx,dz,w,y # param[i]=(z_i, y_i, x_i, r_i,phi_i) param=self.param p=np.ndarray((param.shape[0], 3)) for i in range(2): (z_i, y_i, x_i, r_i, phi_i)=param[i] p[i,0]=x_i+r_i*np.sin(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx p[i,1]=y_i # y= y_i p[i,2]=z_i+r_i*np.cos(phi_i+w) # z= z_i+r_i*sin(phi_i*w) v=p[1]-p[0] #for y = 0..1: #v=v*y #for y = y_0..y_1: y_0=param[0,1];y_1=param[1,1];v=v*(fy-y_0)/(y_1-y_0) v=v+p[0] dx=cx-v[0] dz=cz-v[2] y=v[1] res=(dx,dz,w,y) return res def inv_transform(self,dx,dz,w,y): #input: dx,dz,w,y #output: cx,cz,w,fy #dx,dy: deviation from cristal center line param=self.param p=np.ndarray((param.shape[0], 3)) for i in range(2): (z_i, y_i, x_i, r_i, phi_i)=param[i] p[i,0]=x_i+r_i*np.sin(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx p[i,1]=y_i # y= y_i p[i,2]=z_i+r_i*np.cos(phi_i+w) # z= z_i+r_i*sin(phi_i*w) v=p[1]-p[0] #for y = 0..1: #v=v*y #for y = y_0..y_1: y_0=param[0,1];y_1=param[1,1];v=v*(y-y_0)/(y_1-y_0) v=v+p[0] cx=dx+v[0] cz=dz+v[2] fy=v[1] res=(cx,cz,w,fy) return res def calcParam(self): n = 3.; per = 1.; w = 2 * np.pi * per / n * np.arange(n) p = ((2.3, .71, 4.12, 10.6 * d2r),(6.2, .45, 3.2, 45.28 * d2r)) # (y, bias, ampl, phi) self.param = param = np.ndarray((len(p), 5)) z = 14.5 # fix z position for i in range(2): (y, bias, ampl, phi) = p[i] x = ampl * np.cos(w + phi) + bias print('yMeas_%d=' % i + str(y) + ' xMeas_%d=' % i + str(x)) # param[i]=(z_i, y_i, x_i, r_i,phi_i) param[i, 0] = z param[i, 1] = y param[i, 2:] = HelicalScan.meas_rot_ctr(x) # (bias,ampl,phase) (bias, ampl, phase) = param[i][2:] print param def pltOrig(self,m,h=None): ax=self.ax # m is a 4x4 matrix. the transformed matrix idx=(2,0,1) r=m[idx,0] #1st g=m[idx,1] #2nd b=m[idx,2] #3rd o=m[idx,3] #origin lines = np.ndarray((3, 2, 3)) # numlines, points, xyz lines[:, 0, :] = o lines[0, 1, :] = o + r lines[1, 1, :] = o + g lines[2, 1, :] = o + b if h is None: lseg = tuple(lines) col=('r','g','b') hlc = plt3d.art3d.Line3DCollection(lseg, colors=col) # , *args[argi:], **kwargs) ax.add_collection(hlc) return hlc else: h.set_segments(lines) return h def pltCrist(self,cx=0,cz=0,w=0,fy=0,h=None): #h are the handles ax = self.ax param = self.param pt = np.ndarray((4, 3)) if h is None: h=[] #handels for i in range(2): (z, y, x, r, phi) = param[i] x+=cx;y+=fy;z+=cz;phi+=w pt[i] = (z, x, y) pt[i + 2] = (z + r * np.cos(phi), x + r * np.sin(phi), y) obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('r', alpha=0.3)) h1=ax.add_patch(obj) h2=plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z") #print h1._segment3d h.append(obj) #hs=ax.scatter(pt[:, 2], pt[:, 0], pt[:, 1]) hs=ax.plot(pt[:, 0], pt[:, 1], pt[:, 2],'.') hp=ax.plot(pt[2:, 0], pt[2:, 1], pt[2:, 2]) h+=(hs[0],hp[0]) if hasattr(self,'points'): lines = self.get_meas_lines(pt,cx,cz,fy,w) p=tuple(lines[:,0,:].T) hl =plt3d.art3d.Line3D(*p,color='r',marker='.')#, *args[argi:], **kwargs) ax.add_artist(hl);h.append(hl) lseg=tuple(lines) col=(mpl.colors.colorConverter.to_rgba('r'),)*len(lseg) hlc=plt3d.art3d.Line3DCollection(lseg,colors=col)#, *args[argi:], **kwargs) ax.add_collection(hlc);h.append(hlc) else: for i in range(2): (z, y, x, r, phi) = param[i] x+=cx;y+=fy;z+=cz;phi+=w pt[i] = (z, x, y) pt[i + 2] = (z + r * np.cos(phi), x + r * np.sin(phi), y) h[i].remove() obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('r', alpha=0.3)) ax.add_patch(obj) plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z") h[i]=obj h[2].set_data(pt[:, 0], pt[:, 1])#, pt[:, 1])) h[2].set_3d_properties(pt[:, 2]) h[3].set_data(pt[2:, 0], pt[2:, 1])#, pt[:, 1])) h[3].set_3d_properties(pt[2:, 2]) if hasattr(self,'points'): lines = self.get_meas_lines(pt,cx,cz,fy,w) hl=h[4] #hl.set_data(lines[:,0,0], lines[:,0,1]) # , pt[:, 1])) #hl.set_3d_properties(lines[:,0,2]) p=tuple(lines[:,0,:].T) hl._verts3d=p;hl.stale=True hlc=h[5] hlc.set_segments(lines) return (h,pt) def get_meas_lines(self,pt,cx,cz,fy,w): param = self.param pts = self.points dx_ = pts[:, 0] # add 0.2 to test dz_ = pts[:, 1] # add 0.2 to test w_ = pts[:, 2] * (d2r / 1000.) y_ = pts[:, 3] # self.inv_transform(self, dx, dz, w, y): f = (pts[:, 3] - param[0, 1]) / (param[1, 1] - param[0, 1]) lines = np.ndarray((pts.shape[0], 2, 3)) ofx=dx_*-np.sin(w-w_)+dz_*np.cos(w-w_) # 0.2=dx ofy=dx_*np.cos(w-w_)+dz_*np.sin(w-w_) lines[:, 0, 0] = pt[2, 0] + (pt[3, 0] - pt[2, 0]) * f +ofx # z data lines[:, 0, 1] = pt[2, 1] + (pt[3, 1] - pt[2, 1]) * f +ofy # x data lines[:, 0, 2] = pts[:, 3] + fy # y data lines[:, 1, 0] = lines[:, 0, 0] + np.cos(w-w_)*.1 lines[:, 1, 1] = lines[:, 0, 1] + np.sin(w-w_)*.1 lines[:, 1, 2] = lines[:, 0, 2] return lines @staticmethod def meas_rot_ctr(y,per=1): # find the amplitude bias and phase of an equidistant sampled sinus # it needs at least 3 measurements e.g. at 0,120 240 deg or 0 90 180 270 deg # per is the number of persiods, default is 1 period =360 deg n=len(y) f = np.fft.fft(y) idx=int(per) bias=np.absolute(f[0]/n) phase=np.angle(f[idx]) ampl=np.absolute(f[idx])*2/n return (bias,ampl,phase) def axSetCenter(self,v,l): ax=self.ax #v=center vector, l= length of each axis l2=l/2. ax.set_xlim(v[2]-l2, v[2]+l2); ax.set_ylim(v[0]-l2, v[0]+l2); ax.set_zlim(v[1]-l2, v[1]+l2) def gen_coord_trf_code(self,file=None,host=None): param=self.param prg = [] prg.append(''' // Set the motors as inverse kinematic axes in CS 1 //motors CX CZ RY FY // 7 8 1 2 &1 #7->I #8->I #1->I #2->I open forward define(qCX='L7', qCZ='L8', qW='L1', qFY='L2') define(DX='C6', DZ='C8', W='C1', Y='C7') //coord X Z B Y define(p0_x='L10', p0_y='L11', p0_z='L12') define(p1_x='L13', p1_y='L14', p1_z='L15') define(scale='L16') //send 1"forward kinematic %f %f %f %f\\n",qCX,qCZ,qW,qFY''') for i in range(2): #https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list l=[j for i in zip((i,) * param.shape[1], list(param[i])) for j in i] prg.append(" define(z_%i=%g, y_%i=%g, x_%i=%g, r_%i=%g, phi_%i=%g)"%tuple(l)) prg.append(" W=qW") prg.append(" qW=qW*%g"%(d2r/1000.)) #scale from 1000*deg to rad prg.append(''' p0_x=x_0+r_0*sin(phi_0+qW) p1_x=x_1+r_1*sin(phi_1+qW) p0_y=y_0 p1_y=y_1 p0_z=z_0+r_0*cos(phi_0+qW) p1_z=z_1+r_1*cos(phi_1+qW) scale=(qFY-y_0)/(y_1-y_0) p0_x=p0_x+scale*(p1_x-p0_x) p0_y=p0_y+scale*(p1_y-p0_y) p0_z=p0_z+scale*(p1_z-p0_z) DX=qCX-p0_x DZ=qCZ-p0_z Y=qFY //send 1"forward result %f %f %f %f\\n",DX,DZ,W,Y //P1001+=1 D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2 close ''') prg.append(''' open inverse define(DX='C6', DZ='C8', W='C1', Y='C7') //coord X Z B Y //D0 is set to $000001c2 define(qCX='L7', qCZ='L8', qW='L1', qFY='L2') define(p0_x='L10', p0_y='L11', p0_z='L12') define(p1_x='L13', p1_y='L14', p1_z='L15') define(scale='L16') //send 1"inverse kinematic %f %f %f %f\\n",DX,DZ,W,Y''') for i in range(2): # https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list l = [j for i in zip((i,) * param.shape[1], list(param[i])) for j in i] prg.append(" define(z_%i=%g, y_%i=%g, x_%i=%g, r_%i=%g, phi_%i=%g)" % tuple(l)) prg.append(" qW=W") prg.append(" W=W*%g"%(d2r/1000.)) #scale from 1000*deg to rad prg.append(''' p0_x=x_0+r_0*sin(phi_0+W) p1_x=x_1+r_1*sin(phi_1+W) p0_y=y_0 p1_y=y_1 p0_z=z_0+r_0*cos(phi_0+W) p1_z=z_1+r_1*cos(phi_1+W) scale=(qFY-y_0)/(y_1-y_0) p0_x=p0_x+scale*(p1_x-p0_x) p0_y=p0_y+scale*(p1_y-p0_y) p0_z=p0_z+scale*(p1_z-p0_z) qCX=DX+p0_x qCZ=DZ+p0_z qFY=Y //send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY //P1002+=1 close ''') 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 def gen_prog(self,prgId=2,fnPrg=None,host=None,mode=0,**kwargs): ''' kwargs: acq_per : acquire period: acquire data all acq_per servo loops (default=1) mode=-1: test motion mode=0: #linear motion with 100 ms break at each measurement cnt : move path multiple times cntVert : number of vertical measurements cntHor : number of horizontal measurements hRng : min, max horizontal boundaries wRng : starting and ending angle yRng : starting and ending height mode=1: #PVT motion pt2pt_time : time to move from one point to the next point cnt : move path multiple times cntVert : number of vertical measurements cntHor : number of horizontal measurements hRng : min, max horizontal boundaries wRng : starting and ending angle yRng : starting and ending height ''' 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"] # CX CZ W FY channels=["Motor[7].ActPos","Motor[8].ActPos","Motor[1].ActPos","Motor[2].ActPos"] 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)) prg.append(' P1000=0') # this uses Coord[1].Tm and limits with MaxSpeed #******** mode -1 ******** if mode==-1: #### jog all motors 10000um (or 10000 mdeg) pos=np.array([[0,0,0,0],]) prg.append(' linear abs') #prg.append('X(%g) Z(%g) B(%g) Y(%g)' % tuple(pos[0, :])) prg.append(' jog1,2,7,8=0') prg.append(' dwell 10') prg.append(' Gather.Enable=2') prg.append(' jog7:10000') prg.append(' dwell 100') prg.append(' jog8:10000') prg.append(' dwell 100') prg.append(' jog1:10000') prg.append(' dwell 100') prg.append(' jog2:10000') prg.append(' dwell 100') prg.append(' jog1,2,7,8=0') prg.append(' dwell 100') prg.append(' Gather.Enable=0') # ******** mode 0 ******** elif mode==0: #### linear motion #y=2.3 6.2 dx=0, dz=0 w=0..3600000 # 10 rev cnt= kwargs.get('cnt', 1) #move path multiple times cntVert = kwargs.get('cntVert', 12) cntHor = kwargs.get('cntHor', 4) hRng = kwargs.get('hRng', (-.2,.2)) wRng = kwargs.get('wRng', (0,360000)) yRng = kwargs.get('yRng', (2.3,6.2)) numPt=cntVert*cntHor pt=np.zeros((numPt,4)) if cntHor>1: a=np.linspace(hRng[0],hRng[1],cntHor) a=np.append(a,a[::-1]) a=np.tile(a,(cntVert+1)//2) pt[:,0]= a[0:pt.shape[0]] #pt[:,1]= np.linspace(0,.2,numPt) #dz pt[:,2]= np.linspace(wRng[0],wRng[1],numPt) #w pt[:,3]= np.repeat(np.linspace(yRng[0],yRng[1],cntVert),cntHor) #y self.points=pt prg.append(' Coord[1].SegMoveTime=.05') #to calculate every 0.05 ms the inverse kinematics prg.append(' linear abs') prg.append(' X%g Z%g B%g Y%g' % tuple(pt[0, :])) prg.append(' dwell 100') prg.append(' Gather.Enable=2') for i in range(pt.shape[0]): prg.append(' X%g Z%g B%g Y%g' % tuple(pt[i, :])) prg.append(' dwell 100') prg.append(' Gather.Enable=0') # ******** mode 1 ******** elif mode==1: #### pvt motion #y=2.3 6.2 dx=0, dz=0 w=0..3600000 # 10 rev cnt= kwargs.get('cnt', 1) #move path multiple times cntVert = kwargs.get('cntVert', 12) cntHor = kwargs.get('cntHor', 4) hRng = kwargs.get('hRng', (-.2,.2)) wRng = kwargs.get('wRng', (0,360000)) yRng = kwargs.get('yRng', (2.3,6.2)) pt2pt_time = kwargs.get('pt2pt_time ', 100) numPt=cntVert*cntHor pt=np.zeros((numPt,4)) if cntHor>1: a=np.linspace(hRng[0],hRng[1],cntHor) a=np.append(a,a[::-1]) a=np.tile(a,(cntVert+1)//2) pt[:,0]= a[0:pt.shape[0]] #pt[:,1]= np.linspace(0,.2,numPt) #dz pt[:,2]= np.linspace(wRng[0],wRng[1],numPt) #w pt[:,3]= np.repeat(np.linspace(yRng[0],yRng[1],cntVert),cntHor) #y self.points=pt #pv is an array of dx,dz,w,y,vel_dx,vel_dz,vel_w,vel_y pv=np.ndarray(shape=(pt.shape[0]+2,8),dtype=pt.dtype) pv[:]=np.NaN pv[ 0,(0,1,2,3)]=pt[0,:] pv[ 1:-1,(0,1,2,3)]=pt pv[ -1,(0,1,2,3)]=pt[-1,:] pv[(0,0,0,0,-1,-1,-1,-1),(4,5,6,7,4,5,6,7)]=0 dist=pv[2:,(0,1,2,3)] - pv[:-2,(0,1,2,3)] pv[ 1:-1,(4,5,6,7)] = 1000.*dist/(2.*pt2pt_time) prg.append(' linear abs') prg.append(' X%g Z%g B%g Y%g' % tuple(pv[0, (0,1,2,3)])) prg.append(' dwell 10') prg.append(' Gather.Enable=2') if cnt>1: prg.append(' P100=%d'%cnt) prg.append(' N100:') prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position for idx in range(1,pv.shape[0]): prg.append(' P2000=%d'%idx) prg.append(' X%g:%g Z%g:%g B%g:%g Y%g:%g' % tuple(pv[idx, (0,4,1,5,2,6,3,7)])) if cnt>1: prg.append(' dwell 10') prg.append(' P100=P100-1') prg.append(' if(P100>0)') prg.append(' {') prg.append(' linear abs') prg.append(' X%g Z%g B%g Y%g' % tuple(pv[0, (0, 1, 2, 3)])) prg.append(' dwell 100') prg.append(' goto 100') prg.append(' }') else: prg.append(' dwell 1000') prg.append(' Gather.Enable=0') prg.append(' P1000=1') prg.append('close') prg.append('&1\nb%dr\n'%prgId) if self.args.verbose & 4: for ln in prg: print(ln) if fnPrg is not None: fh=open(fnPrg,'w') fh.write('\n'.join(prg)) fh.close() if host is not None: # ***download and start the program*** cmd ='gpasciiCommander --host '+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() # ***wait program finished P1000=1*** com=GpasciiCommunicator().connect(host,prompt='# ') ack=GpasciiCommunicator.gpascii_ack sys.stdout.write('wait execution...');sys.stdout.flush() while(True): com.write('P1000\n') val=com.read_until(ack) #print val val=int(val[val.find('=')+1:].rstrip(ack)) if val==1:break #com.write('Gather.Index\n') #val=com.read_until(ack) #print val time.sleep(.2) sys.stdout.write('.');sys.stdout.flush() fnRmt = '/var/ftp/gather/out.txt' fnLoc = '/tmp/gather.txt' print('\ngather data to %s...' % fnRmt) p = sprc.Popen(('ssh', 'root@' + host, 'gather ', '-u', fnRmt), shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE, stderr=sprc.PIPE) res = p.wait() if res: print('ssh failed. ssh root@%s to open a session' % host) return print('transfer data to %s...' % fnLoc) p = sprc.Popen(('scp', 'root@' + host + ':' + fnRmt, fnLoc), shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE, stderr=sprc.PIPE) res = p.wait() self.rec = np.genfromtxt(fnLoc, delimiter=' ') self.save_rec() class GpasciiCommunicator(): '''Communicates with the Delta Tau gpascii programm ''' gpascii_ack="\x06\r\n" gpascii_inp='Input\r\n' def connect(self, host, username='root', password='deltatau',prompt='ppmac# ',verbose=0): p=telnetlib.Telnet(host) s=p.read_until('login: ') if verbose: print(s) p.write(username+'\n') s =p.read_until('Password: ') if verbose: print(s) p.write(password+'\n') s =p.read_until(prompt) # command prompt if verbose: print(s) p.write('gpascii -2\n') # execute gpascii command s=p.read_until(self.gpascii_inp) if verbose: print(s) return p 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=('-n', '-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('--xy', action='store_true', help='sort x,y instead y,x') parser.add_option('--cfg', help='config file containing json configuration structure') (args, other)=parser.parse_args() args.other=other hs=HelicalScan(args) hs.args.verbose = 255 #hs.sequencer() #hs.test_find_rot_ctr() #hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6) hs.calcParam() #hs.test_coord_trf() #hs.interactive_cx_cz_w_fy() #hs.interactive_dx_dz_w_y() hs.gen_coord_trf_code('/tmp/helicalscan.cfg','MOTTEST-CPPM-CRM0485') #hs.gen_prog(file='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=-1) #hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0) #hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0,cntHor=1) #hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1) hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1, pt2pt_time=100,cnt=1,cntVert=35,cntHor=7,hRng=(-.3,.3),wRng=(0,360000*3),yRng=(6.2,2.3)) hs.load_rec() hs.interactive_anim() hs.show_vel(); plt.show() return #------------------ Main Code ---------------------------------- ret=parse_args() exit(ret)