wip

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   

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,57 +44,15 @@ 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
-    ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y')
-    ax.view_init(elev=14., azim=10)
-
-  def setCenter(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 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):
-  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:
@@ -109,7 +71,7 @@ class HelicalScan:
     self.cfg=dotdict(cfg)
     self.args=args
 
-    def run(self):
+  def sequencer(self):
     print('args='+str(self.args))
     print('cfg='+str(self.cfg))
     #try:
@@ -127,19 +89,13 @@ class HelicalScan:
         if not dryrun:
           eval('self.' + cmd)
 
-    def test_coord_trf(self):
+  def calcParam(self):
-      d2r=2*np.pi/360
+    n = 3.;
-      plt.ion()
+    per = 1.;
-      fig = plt.figure()
+    w = 2 * np.pi * per / n * np.arange(n)
-      #ax = fig.gca(projection='3d')
+    p = ((2.3, .71, 4.12, 10.6 * d2r), (6.2, .45, 3.2, 45.28 * d2r))  # (y, bias, ampl, phi)
-      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
@@ -149,43 +105,293 @@ class HelicalScan:
       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)
+    print param
-        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))
+
+  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")
-      ax.scatter(pt[:,2], pt[:,0], pt[:,1])
+        h[i]=obj
-      ax.plot(pt[2:,2], pt[2:,0], pt[2:,1], label='zs=0, zdir=z')
+      h[2].set_data(pt[:, 2], pt[:, 0])#, pt[:, 1]))
-      print param
+      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)
+
+    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)); 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],extAx)
+    m=self.fwd_transform(param[0][1],0,pt[2,0],pt[2,2])
-      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])
+    #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)
 
-      m=self.fwd_transform(param[0][1],20*d2r,pt[2][0],pt[2][2])
+    #a=anim.FuncAnimation(fig,self.my_anim_func3,100,fargs=(horig,pt),interval=20,blit=False)
-      extAx.pltOrig(m)
+    #plt.show()
-
-      #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):
+  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
-      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)
+
+    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]
@@ -199,21 +405,41 @@ class HelicalScan:
     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
+    dx=cx-v[0]
+    dz=cz-v[2]
+    res=(y,w,dx,dz)
+    print res
 
-      #v=v/abs(v)
+    m=Trf.trans(cx,y,cz)
-      print v
+    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):
-      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
+    param=self.param
-      #returns y,phi,cx,cz
+    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
 
 
@@ -238,7 +464,6 @@ class HelicalScan:
     # phi  phase
     # bias bias value
     # ampl amplitude
-      d2r=2*np.pi/360
 
     t = np.arange(n)
     w=2*np.pi*per/n*t
@@ -266,6 +491,19 @@ class HelicalScan:
     plt.show()
     pass
 
+  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 my_anim_func(idx,hs,horig):
+  print('anim')
+  a=idx*.01*2*np.pi
+  m=Trf.rotY(a)
+  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)
+    hs=HelicalScan(args)
-    sp.run()
+    #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()

python/helicalscan1.svg

@@ -133,15 +133,15 @@
      borderopacity="1.0"
      inkscape:pageopacity="0.0"
      inkscape:pageshadow="2"
-     inkscape:zoom="1.3382855"
+     inkscape:zoom="1.9573107"
-     inkscape:cx="183.81592"
+     inkscape:cx="306.39127"
-     inkscape:cy="755.54223"
+     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:start="1.2174288"
-       sodipodi:end="3.2616514"
+       sodipodi:end="0.78462883"
-       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"
+       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" />
     <path
@@ -294,9 +294,9 @@
        sodipodi:cy="192.17082"
        sodipodi:rx="18.581232"
        sodipodi:ry="4.645308"
-       sodipodi:start="6.0151975"
+       sodipodi:start="0.58303123"
-       sodipodi:end="1.0471386"
+       sodipodi:end="2.0657742"
-       d="m 439.44423,190.94078 a 18.581232,4.645308 0 0 1 -8.62642,5.25286"
+       d="m 437.03781,194.72833 a 18.581232,4.645308 0 0 1 -24.33788,1.53027"
        sodipodi:open="true" />
     <text
        xml:space="preserve"