diff --git a/python/helicalscan.py b/python/helicalscan.py index 06ea081..4eae577 100755 --- a/python/helicalscan.py +++ b/python/helicalscan.py @@ -313,13 +313,6 @@ class HelicalScan: ax.view_init(elev=14., azim=10) param=self.param - fnLoc='/tmp/gather.txt' - #meta = self.meta - #pts = self.points # X,Y array - self.rec = np.genfromtxt(fnLoc, delimiter=' ') - #if fnOut: - # np.savez_compressed(fnOut, rec=rec, pts=pts, meta=meta) - # param[i]=(z_i, y_i, x_i, r_i,phi_i) ctr=(0,0,0) self.axSetCenter(ctr,10) @@ -330,30 +323,19 @@ class HelicalScan: ly = param[:,1] self.sldFrm=sFrm=Slider(axFrm, 'frm', 0, self.rec.shape[0]-1, valinit=0) sFrm.on_changed(self.update_anim) - # 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 + 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.show_vel() + #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,val): - frm = self.sldFrm.val + 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 @@ -369,7 +351,25 @@ class HelicalScan: #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 @@ -445,74 +445,109 @@ class HelicalScan: def pltOrig(self,m,h=None): ax=self.ax # m is a 4x4 matrix. the transformed matrix - r=m[:3,0] #1st - g=m[:3,1] #2nd - b=m[:3,2] #3rd - o=m[:3,3] #origin + 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: - hr=ax.plot((o[2],o[2]+r[2]), (o[0],o[0]+r[0]), (o[1],o[1]+r[1]), 'r') - hg=ax.plot((o[2],o[2]+g[2]), (o[0],o[0]+g[0]), (o[1],o[1]+g[1]), 'g') - hb=ax.plot((o[2],o[2]+b[2]), (o[0],o[0]+b[0]), (o[1],o[1]+b[1]), 'b') - return hr + hg + hb # this is a list + lseg = tuple(lines) + col=('r','g','b') + hlc = plt3d.art3d.Line3DCollection(lseg, colors=col) # , *args[argi:], **kwargs) + ax.add_collection(hlc) + return hlc else: - hr, hg, hb = h - hr.set_data((o[2], o[2] + r[2]), (o[0], o[0] + r[0])) - hr.set_3d_properties((o[1], o[1] + r[1])) - hg.set_data((o[2], o[2] + g[2]), (o[0], o[0] + g[0])) - hg.set_3d_properties((o[1], o[1] + g[1])) - hb.set_data((o[2], o[2] + b[2]), (o[0], o[0] + b[0])) - hb.set_3d_properties((o[1], o[1] + b[1])) + 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 - ax=self.ax - param=self.param - pt = np.ndarray((4, 3)) for i in range(2): (z, y, x, r, phi) = param[i] x+=cx;y+=fy;z+=cz;phi+=w - pt[i] = (x, y, z) - pt[i + 2] = (x + r * np.sin(phi), y, z + r * np.cos(phi)) + 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[:, 2], pt[:, 0], pt[:, 1],'.') - hp=ax.plot(pt[2:, 2], pt[2:, 0], pt[2:, 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: - ax=self.ax - param=self.param - pt = np.ndarray((4, 3)) for i in range(2): (z, y, x, r, phi) = param[i] x+=cx;y+=fy;z+=cz;phi+=w - pt[i] = (x, y, z) - pt[i + 2] = (x + r * np.sin(phi), y, z + r * np.cos(phi)) + 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[:, 2], pt[:, 0])#, pt[:, 1])) - h[2].set_3d_properties(pt[:, 1]) + 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) - h[3].set_data(pt[2:, 2], pt[2:, 0])#, pt[:, 1])) - h[3].set_3d_properties(pt[2:, 1]) - #hr.set_data((o[2], o[2] + r[2]), (o[0], o[0] + r[0])) - #hr.set_3d_properties((o[1], o[1] + r[1])) - #h[3].set_3d_properties(zs=pt[:, 1])) - #h[4].set_data((pt[2:, 2], pt[2:, 0]))#, pt[2:, 1])) - #hp=ax.plot(pt[2:, 2], pt[2:, 0], pt[2:, 1], label='zs=0, zdir=z') - #h+=(hs,hp[0]) 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): @@ -558,7 +593,7 @@ open forward 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''') + //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] @@ -580,7 +615,7 @@ open forward DX=qCX-p0_x DZ=qCZ-p0_z Y=qFY - send 1"forward result %f %f %f %f\\n",DX,DZ,W,Y + //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 @@ -595,7 +630,7 @@ open inverse 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''') + //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] @@ -617,7 +652,7 @@ open inverse qCX=DX+p0_x qCZ=DZ+p0_z qFY=Y - send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY + //send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY //P1002+=1 close @@ -638,11 +673,26 @@ close retval = p.wait() # gather -u /var/ftp/gather/out.txt - def gen_prog(self,prgId=2,file=None,host=None,mode=0,**kwargs): + 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) - pt2pt_time : time to move from one point to the next point + 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) @@ -665,6 +715,8 @@ close 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') @@ -683,49 +735,64 @@ close 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 - pos=np.array([[0, 0, 0, 2.300], - [0, 0, 360000, 6.200],]) - numPt=13 - pos=np.zeros((numPt,4)) - pos[:,2]= np.linspace(0,3600000,numPt) - pos[:,3]= np.linspace(2.3,6.2,numPt) - #[0, 0, 3600000, 6.200],]) - #prg.append('Coord[1].SegMoveTime=1') #to calculate every 1 ms the inverse kinematics - prg.append(' Coord[1].SegMoveTime=.05') + #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(pos[0, :])) + 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(pos.shape[0]): - prg.append(' X%g Z%g B%g Y%g' % tuple(pos[i, :])) + 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 - 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. - try: - cnt=kwargs['cnt'] #move path multiple times - except KeyError: - cnt=1 - numPt=13 + #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)) - pt[:,2]= np.linspace(0,3600000,numPt) - pt[:,3]= np.linspace(2.3,6.2,numPt) - vel=pt[2:,:]-pt[:-2,:] - #pv is an array of posx posy velx vely + 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*pt[0,:]-pt[1,:] pv[ 0,(0,1,2,3)]=pt[0,:] pv[ 1:-1,(0,1,2,3)]=pt - #pv[ -1,(0,1)]=2*pt[-1,:]-pt[-2,:] 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)] @@ -740,6 +807,7 @@ close 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') @@ -754,29 +822,6 @@ close else: 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. - cnt=13 - pos=np.zeros((cnt,4)) - pos[:,2]= np.linspace(0,3600000,cnt) - pos[:,3]= np.linspace(2.3,6.2,cnt) - 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(' P1000=1') prg.append('close') @@ -785,13 +830,13 @@ close for ln in prg: print(ln) - if file is not None: - fh=open(file,'w') + 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+' '+ file + 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 @@ -804,8 +849,12 @@ close 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' @@ -814,10 +863,16 @@ close 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(): @@ -883,15 +938,21 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' #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.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_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(file='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0) - #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 ----------------------------------