diff --git a/python/FKIKfaststage.m b/python/FKIKfaststage.m new file mode 100644 index 0000000..8a37e34 --- /dev/null +++ b/python/FKIKfaststage.m @@ -0,0 +1,142 @@ +% FK/IK using homogenous matrices %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% This script demonstrates how homogenous matrices can be used to create +% both a forward kinematics (FK)and an inverse kinematics (IK) model of +% a serial kinematics manipulator, with 5 % motors (q1,q2,q3,q4,q5) +% and 4 user coordinates (X,Y,Z,theta). +% +% The script uses a chain of Homogenous Transformation Matrices to create +% the FK model. And the Moore-Penrose Pseudo-Inverse of the Jacobian +% to iteratively solve the IK model (Newton-Raphson). +% +% 5.10.2017 Wayne Glettig + +clear all; +syms offsetX offsetY offsetZ ... + alpha beta gamma... + q1 q2 q3 q4 q5; + +%% Here the Transformation Matrix HBA is constructed from a translation +% matrix and 3 rotation matrices, using symbolic variables. +%Translation: +HXYZ = [1,0,0,offsetX;... + 0,1,0,offsetY;... + 0,0,1,offsetZ;... + 0,0,0,1]; +%Rot around X: +HRX = [1,0 ,0 ,0;... + 0,cos(alpha),-sin(alpha),0;... + 0,sin(alpha), cos(alpha),0;... + 0,0 ,0 ,1]; +%Rot around Y: +HRY = [ cos(beta),0,sin(beta),0;... + 0 ,1,0 ,0;... + -sin(beta),0,cos(beta),0;... + 0 ,0,0 ,1]; +%Rot around Z: +HRZ = [cos(gamma),-sin(gamma),0,0;... + sin(gamma), cos(gamma),0,0;... + 0 ,0 ,1,0;... + 0 ,0 ,0,1]; +%Create HBA (first rotation then translation) +HBA=HXYZ*HRZ*HRY*HRX; +%Create HAB (inverse) +HAB=inv(HBA); + + +%% Here the Kinematic Chain is created (q1,q2,q3,q4,q5 are motor coordinates) +%Essentially the HBA matrix above is filled with the offsetXYZ and angle +%values to assemble the chain. The chain consists here of the reference +%frames 0-1-2-3-4-5-6-7: +%Frame 0: MATLAB darstellungsframe (for plot3) +%Frame 1: Beamline UCS +%Frame 2: on Z Stage +%Frame 3: on X Stage +%Frame 4: on RX Stage +%Frame 5: on Fast X Stage +%Frame 6: on Fast Y Stage +%Frame 7: Sample pos + +H10 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [0,0,0,90/180*pi,0/180*pi,90/180*pi]); +H21 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [0,0,q1,0,0,0]); %Z Stage +H32 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [q2,0,0,0,0,0]); %X Stage +H43 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [0,0,0,0,q3/180*pi,0]); %Rotation RY +H54 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [q4,0,0,0,0,0]); %Fast Stage X +H65 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [0,q5,0,0,0,0]); %Fast Stage Y +H76 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [0,0,0,0,0,0]); %sample Offset +H70 = H10*H21*H32*H43*H54*H65*H76; +H07 = inv(H70); + +%% The FK model (forward kinematics) can be defined as +% [X;Y;Z;theta] = f(q1,q2,q3,q4,q5) +H71 = H21*H32*H43*H54*H65*H76; %UCS: reference frame 1 +f= H71*[0;0;0;1]; %XYZ position of orgin of reference frame 7 +f(4)=q3; %theta angle directly connected to q3 + +%% The IK model is more difficult to find. Different IK solution methods +% exist, this is one of the basic ones, using the Pseudo-inverse of the +% Jacobian matrix to converge a solution towards a solution +% (Newton-Raphson) + +% Jacobian of FK f function derived over motor coordinates +J= jacobian(f,[q1,q2,q3,q4,q5]); +Jinv = pinv(J); + +tic +% Algorithm for IK (q are motor coords, x are user coords) +xt=[2;1;0;32]; %SET TARGET USER COORDS +q0=[0;0;0;0;0]; %motor start values +qc = q0; %set current q values for loop +loopcond = 1; +loopcounter=0; +while loopcond + xc = vpa(subs(f, [q1,q2,q3,q4,q5], qc')); %get current x values based on q + deltax=xt-xc; %get x error (target - current) + if abs(deltax)<1e-9 %if abs error small enough, get out of loop + loopcond=0; + end + Jinvc=vpa(subs(Jinv, [q1,q2,q3,q4,q5], qc')); %inv Jacobian with current q + deltaq=Jinvc*deltax; %By multiplying the x error with Jinv, a q correction can be deduced + qc = qc+deltaq;%update current motor values + loopcounter=loopcounter+1; +end +q = qc %output q as the motor coordinates +toc + +%% Plot the results (construct sample position using motors) +Oin1 = [0;0;0;1]; +e1in1 = [1;0;0;1]; +e2in1 = [0;1;0;1]; +e3in1 = [0;0;1;1]; + +clf; +hold on; +grid on; +axis vis3d; +axis([-10 10 -10 10 -10 10]); +title('Sample Position, constructed with (q1,q2,q3,q4,q5)'); +xlabel('Beamline Z-Axis'); +ylabel('Beamline X-Axis'); +zlabel('Beamline Y-Axis'); + + +for i= 0:1 +H60c = subs(H70, [q1 q2 q3, q4, q5], q'); + +Oin0 = H60c*Oin1; +e1in0 = H60c*e1in1; +e2in0 = H60c*e2in1; +e3in0 = H60c*e3in1; + +plot3([Oin0(1),e1in0(1)],[Oin0(2),e1in0(2)],[Oin0(3),e1in0(3)],'r-'); +plot3([Oin0(1),e2in0(1)],[Oin0(2),e2in0(2)],[Oin0(3),e2in0(3)],'g-'); +plot3([Oin0(1),e3in0(1)],[Oin0(2),e3in0(2)],[Oin0(3),e3in0(3)],'b-'); +plot3([-10,0],[0,0],[0,0],'r-') +end \ No newline at end of file diff --git a/python/helicalscan.py b/python/helicalscan.py index add2ea3..40813f7 100755 --- a/python/helicalscan.py +++ b/python/helicalscan.py @@ -16,9 +16,13 @@ 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 from utilities import * +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 @@ -40,22 +44,454 @@ class Trf: c = np.cos(rad) s = np.sin(rad) m=np.identity(4) - m[np.ix_((0,2),(0,2))]=((c, -s),(s, c)) + m[np.ix_((0,2),(0,2))]=((c, s),(-s, c)) return m @staticmethod - def trans(v): + def trans(x,y,z): m=np.identity(4) - m[0:3, 3] = v + m[0:3, 3] =(x,y,z) return m -class ExtAxes():#mpl.axes._subplots.Axes3DSubplot): - def __init__(self,ax): - self.ax=ax +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) + + 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 + r=m[:3,0] #1st + g=m[:3,1] #2nd + b=m[:3,2] #3rd + o=m[:3,3] #origin + 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 + 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])) + return h + + def pltCrist(self,fy=0,cx=0,cz=0,w=0,h=None): + #h are the handles + 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)) + 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") + 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]) + h+=(hs[0],hp[0]) + 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)) + 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[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 interactive_fy_cx_cz_w(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) + self.calcParam() + 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) - def setCenter(self,v,l): + axFy = plt.axes([0.1, 0.01, 0.8, 0.02]) + axCx = plt.axes([0.1, 0.04, 0.8, 0.02]) + axCz = plt.axes([0.1, 0.07, 0.8, 0.02]) + axW = 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.sldFy = sFy = Slider(axFy, 'fy', ly[0], ly[1], valinit=(ly[0]+ly[1])/2.) + 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) + sFy.on_changed(self.update_fy_cx_cz_w) + sCx.on_changed(self.update_fy_cx_cz_w) + sCz.on_changed(self.update_fy_cx_cz_w) + sW.on_changed(self.update_fy_cx_cz_w) + + 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.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(*ofs); self.hOrig=self.pltOrig(m) + + + + plt.show() + + def update_fy_cx_cz_w(self,val): + fy = self.sldFy.val + cx = self.sldCx.val + cz = self.sldCz.val + w = self.sldW.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(fy,cx,cz,w*d2r,self.hCrist) + #l.set_ydata(amp * np.sin(2 * np.pi * freq * t)) + self.fig.canvas.draw_idle() + + def interactive_y_dx_dz_w(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) + self.calcParam() + 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) + + axFy = plt.axes([0.1, 0.01, 0.8, 0.02]) + axCx = plt.axes([0.1, 0.04, 0.8, 0.02]) + axCz = plt.axes([0.1, 0.07, 0.8, 0.02]) + axW = 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=[-1,1];ly=[0,1];lz=[-1,1] + self.sldFy = sFy = Slider(axFy, 'y', ly[0], ly[1], valinit=(ly[0]+ly[1])/2.) + self.sldCx = sCx = Slider(axCx, 'dx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.) + self.sldCz = sCz = Slider(axCz, 'dz', lz[0], lz[1], valinit=(lz[0]+lz[1])/2.) + self.sldW =sW = Slider(axW, 'ang', -180., 180.0, valinit=0) + sFy.on_changed(self.update_y_dx_dz_w) + sCx.on_changed(self.update_y_dx_dz_w) + sCz.on_changed(self.update_y_dx_dz_w) + sW.on_changed(self.update_y_dx_dz_w) + + 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.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(*ofs); self.hOrig=self.pltOrig(m) + + + + plt.show() + + def update_y_dx_dz_w(self,val): + fy = self.sldFy.val + cx = self.sldCx.val + cz = self.sldCz.val + w = self.sldW.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(fy,cx,cz,w*d2r,self.hCrist) + #l.set_ydata(amp * np.sin(2 * np.pi * freq * t)) + self.fig.canvas.draw_idle() + + + def test_coord_trf(self): + plt.ion() + fig = plt.figure() + #self.ax=ax=fig.add_subplot(1,1,1,projection='3d') + self.ax=ax=plt3d.Axes3D(fig,[0.02, 0.02, 0.96, 0.96]) + #fig.Axes3DSubplot() + ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y') + ax.view_init(elev=14., azim=10) + self.axSetCenter((0,5,15),10) + self.calcParam() + param=self.param + + hCrist,pt=self.pltCrist() + z = 14.5 # fix z position + #self.hCrist=hCrist + #a=anim.FuncAnimation(fig,self.my_anim_func3,100,fargs=(),interval=20,blit=False) + #plt.show() + + #plt.show() + #m=np.identity(4); horig=extAx.pltOrig(m) + m=Trf.trans(0,0,z); self.hOrig=self.pltOrig(m) + + #self.fwd_transform(y ,w ,cx ,cz) + # y_0 ,0deg ,x_0 ,z_0) + m=self.fwd_transform(param[0][1],0,pt[2,0],pt[2,2]) + m=self.fwd_transform(param[0][1],20*d2r,pt[2,0],pt[2,2]) + #m=self.fwd_transform(param[0][1],0,pt[2][0],pt[2][2]) + pass + #m=self.fwd_transform(param[0][1],20*d2r,pt[2][0],pt[2][2]) + #self.pltOrig(m) + + #a=anim.FuncAnimation(fig,self.my_anim_func3,100,fargs=(horig,pt),interval=20,blit=False) + #plt.show() + # y_0 ,120deg ,x_0 ,z_0) + self.fwd_transform(param[0][1],2*np.pi/3.,param[0][2],param[0][0]) + #self.fwd_transform(param[1][1],0.,param[1][2],param[1][3]) + + 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 my_anim_func2(self,idx, horig,pt): + #print('my_anim_func2%d'%idx) + pIdx, aIdx= divmod(idx, 25) + a = aIdx/25. * 2. * np.pi + #print(aIdx,a) + m = Trf.trans(*pt[pIdx]) + m = m=m.dot(Trf.rotY(a)) + self.pltOrig(m, horig) + + def my_anim_func(self,idx, horig): + print('my_anim_func%d'%idx) + a = idx * .01 * 2 * np.pi + m = Trf.rotY(a) + self.pltOrig(m, horig) + + def fwd_transform(self,y,w,cx,cz): + #cx,cy: coarse stage + #input: y,w,cx,cz + #output: y,w,dx,dz + # 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.cos(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.sin(phi_i+w) # z= z_i+r_i*sin(phi_i*w) + print p + v=p[1]-p[0] + v=v/np.sqrt(v.dot(v)) # v/|v| + y_0=param[0][1] + y_1=param[1][1] + v=v*(y-y_0)/(y_1-y_0) # v(y)=v*(v-y_0)/(y_1-y_0) + v=p[0]+v + dx=cx-v[0] + dz=cz-v[2] + res=(y,w,dx,dz) + print res + + m=Trf.trans(cx,y,cz) + m=m.dot(Trf.rotY(w)) + self.pltOrig(m,self.hOrig) + self.axSetCenter(m[0:3,3],10) + return res + + def inv_transform(self,y,w,dx=0,dz=0): + #input: y,w,dx,dz + #output: y,w,cx,cz + #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.cos(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.sin(phi_i+w) # z= z_i+r_i*sin(phi_i*w) + print p + v=p[1]-p[0] + v=v/np.sqrt(v.dot(v)) # v/|v| + y_0=param[0][1] + y_1=param[1][1] + v=v*(y-y_0)/(y_1-y_0) # v(y)=v*(v-y_0)/(y_1-y_0) + v=p[0]+v + dx=cx-v[0] + dz=cz-v[2] + res=(y,w,dx,dz) + print res + + + pass + + + @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) + + @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 axSetCenter(self,v,l): ax=self.ax #v=center vector, l= length of each axis l2=l/2. @@ -63,209 +499,11 @@ class ExtAxes():#mpl.axes._subplots.Axes3DSubplot): ax.set_ylim(v[0]-l2, v[0]+l2); ax.set_zlim(v[1]-l2, v[1]+l2) - def pltOrig(self,m): - 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 - 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 - -def my_anim_func(idx,horig): +def my_anim_func(idx,hs,horig): print('anim') a=idx*.01*2*np.pi m=Trf.rotY(a) - r = m[:3, 0] # 1st - g = m[:3, 1] # 2nd - b = m[:3, 2] # 3rd - o = m[:3, 3] # origin - hr,hg,hb=horig - 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])) - -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 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 test_coord_trf(self): - d2r=2*np.pi/360 - plt.ion() - fig = plt.figure() - #ax = fig.gca(projection='3d') - ax = fig.add_subplot(1,1,1,projection='3d') - extAx=ExtAxes(ax) - extAx.setCenter((0,5,15),10) - n = 3.; per = 1.; w = 2*np.pi*per/n*np.arange(n) - p=((2.3,0.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 - pt=np.ndarray((4,3)) - 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:] - pt[i]=(bias, y, z) - pt[i+2]=(bias+ampl*np.cos(phase),y, z+ampl*np.sin(phase)) - obj = mpl.patches.Circle((z,bias), ampl, 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") - ax.scatter(pt[:,2], pt[:,0], pt[:,1]) - ax.plot(pt[2:,2], pt[2:,0], pt[2:,1], label='zs=0, zdir=z') - print param - #plt.show() - #m=np.identity(4); horig=extAx.pltOrig(m) - m=Trf.trans((0,0,z)); horig=extAx.pltOrig(m) - #self.fwd_transform(y ,w ,cx ,cz) - # y_0 ,0deg ,x_0 ,z_0) - m=self.fwd_transform(param[0][1],0,pt[2][0],pt[2][2],extAx) - m=self.fwd_transform(param[0][1],0,pt[2][0],pt[2][2],extAx) - - m=self.fwd_transform(param[0][1],20*d2r,pt[2][0],pt[2][2]) - extAx.pltOrig(m) - - #my_anim_func(0,horig) - a=anim.FuncAnimation(fig,my_anim_func,25,fargs=(horig,),interval=50,blit=False) - plt.show() - # y_0 ,120deg ,x_0 ,z_0) - self.fwd_transform(param[0][1],2*np.pi/3.,param[0][2],param[0][0]) - #self.fwd_transform(param[1][1],0.,param[1][2],param[1][3]) - - def fwd_transform(self,y,w,cx,cz,extAx): - #cx,cy: coarse stage - #input: y,w,cx,cz - #output: y,w,dx,dz - m=Trf.trans((cx,y,cz)) - m=m.dot(Trf.rotY(w)) - extAx.pltOrig(m) - extAx.setCenter(m[0:3,3],1) - #TODO: NOT WORKING AT ALL NOW... - 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+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.sin(phi_i+w) # z= z_i+r_i*sin(phi_i*w) - print p - v=p[1]-p[0] - v=v/np.sqrt(v.dot(v)) # v/|v| - y_0=param[0][1] - y_1=param[1][1] - v=v*(y-y_0)/(y_1-y_0) # v(y)=v*(v-y_0)/(y_1-y_0) - v=p[0]+v - cz + cx - - #v=v/abs(v) - print v - - - return m - - def inv_transform(y,phi,dx=0,dz=0): - #input: y,w,dx,dz - #output: y,w,cx,cz - m=np.identity(4) - #dx,dy: deviation from cristal center line - #ps= #x,y,z - #returns y,phi,cx,cz - pass - - - @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) - - @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 - d2r=2*np.pi/360 - - 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 - + hs.pltOrig(m,horig) if __name__=='__main__': from optparse import OptionParser, IndentedHelpFormatter @@ -302,8 +540,11 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' (args, other)=parser.parse_args() args.other=other - sp=HelicalScan(args) - sp.run() + hs=HelicalScan(args) + #hs.sequencer() + hs.interactive_fy_cx_cz_w() + hs.interactive_y_dx_dz_w() + #hs.test_coord_trf() #------------------ Main Code ---------------------------------- #ssh_test() ret=parse_args() diff --git a/python/helicalscan1.svg b/python/helicalscan1.svg index d266f78..d26c52f 100644 --- a/python/helicalscan1.svg +++ b/python/helicalscan1.svg @@ -133,15 +133,15 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="1.3382855" - inkscape:cx="183.81592" - inkscape:cy="755.54223" + inkscape:zoom="1.9573107" + inkscape:cx="306.39127" + inkscape:cy="814.96274" inkscape:document-units="px" inkscape:current-layer="layer1" showgrid="false" - inkscape:window-width="1871" + inkscape:window-width="1920" inkscape:window-height="1176" - inkscape:window-x="49" + inkscape:window-x="1920" inkscape:window-y="24" inkscape:window-maximized="1" inkscape:snap-bbox="false" @@ -281,9 +281,9 @@ sodipodi:cy="373.53488" sodipodi:rx="20.900398" sodipodi:ry="5.2250996" - sodipodi:start="0.47984631" - sodipodi:end="3.2616514" - d="m -404.18498,375.94701 a 20.900398,5.2250996 0 0 1 -24.71557,2.57967 20.900398,5.2250996 0 0 1 -14.5744,-5.61761" + sodipodi:start="1.2174288" + sodipodi:end="0.78462883" + d="m -415.49223,378.43714 a 20.900398,5.2250996 0 0 1 -25.94606,-2.57525 20.900398,5.2250996 0 0 1 7.43885,-6.72668 20.900398,5.2250996 0 0 1 27.55269,1.12243 20.900398,5.2250996 0 0 1 -1.48808,6.9691" transform="scale(-1,1)" sodipodi:open="true" />