#+TITLE: Matlab Functions used for the NASS Project
:DRAWER:
#+STARTUP: overview

#+LANGUAGE: en
#+EMAIL: dehaeze.thomas@gmail.com
#+AUTHOR: Dehaeze Thomas

#+HTML_LINK_HOME: ../index.html
#+HTML_LINK_UP: ../index.html

#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>

#+HTML_MATHJAX: align: center tagside: right font: TeX

#+PROPERTY: header-args:matlab  :session *MATLAB*
#+PROPERTY: header-args:matlab+ :comments org
#+PROPERTY: header-args:matlab+ :results none
#+PROPERTY: header-args:matlab+ :exports both
#+PROPERTY: header-args:matlab+ :eval no-export
#+PROPERTY: header-args:matlab+ :output-dir figs
#+PROPERTY: header-args:matlab+ :tangle no
#+PROPERTY: header-args:matlab+ :mkdirp yes

#+PROPERTY: header-args:shell  :eval no-export

#+PROPERTY: header-args:latex  :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
#+PROPERTY: header-args:latex+ :eval no-export
#+PROPERTY: header-args:latex+ :exports both
#+PROPERTY: header-args:latex+ :mkdirp yes
#+PROPERTY: header-args:latex+ :output-dir figs
:END:

* TODO computePsdDispl
:PROPERTIES:
:header-args:matlab+: :tangle ../src/computePsdDispl.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:computePsdDispl>>

This Matlab function is accessible [[file:../src/computePsdDispl.m][here]].

#+begin_src matlab
  function [psd_object] = computePsdDispl(sys_data, t_init, n_av)
      i_init = find(sys_data.time > t_init, 1);

      han_win = hanning(ceil(length(sys_data.Dx(i_init:end, :))/n_av));
      Fs = 1/sys_data.time(2);

      [pdx, f] = pwelch(sys_data.Dx(i_init:end, :), han_win, [], [], Fs);
      [pdy, ~] = pwelch(sys_data.Dy(i_init:end, :), han_win, [], [], Fs);
      [pdz, ~] = pwelch(sys_data.Dz(i_init:end, :), han_win, [], [], Fs);

      [prx, ~] = pwelch(sys_data.Rx(i_init:end, :), han_win, [], [], Fs);
      [pry, ~] = pwelch(sys_data.Ry(i_init:end, :), han_win, [], [], Fs);
      [prz, ~] = pwelch(sys_data.Rz(i_init:end, :), han_win, [], [], Fs);

      psd_object = struct(...
          'f',  f,   ...
          'dx', pdx, ...
          'dy', pdy, ...
          'dz', pdz, ...
          'rx', prx, ...
          'ry', pry, ...
          'rz', prz);
  end
#+end_src

* TODO computeSetpoint
:PROPERTIES:
:header-args:matlab+: :tangle ../src/computeSetpoint.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:computeSetpoint>>

This Matlab function is accessible [[file:../src/computeSetpoint.m][here]].

#+begin_src matlab
  function setpoint = computeSetpoint(ty, ry, rz)
  %%
  setpoint = zeros(6, 1);

  %% Ty
  Ty = [1 0 0 0  ;
        0 1 0 ty ;
        0 0 1 0  ;
        0 0 0 1 ];

  % Tyinv = [1 0 0  0  ;
  %          0 1 0 -ty ;
  %          0 0 1  0  ;
  %          0 0 0  1 ];

  %% Ry
  Ry = [ cos(ry) 0 sin(ry) 0 ;
        0       1 0       0 ;
        -sin(ry) 0 cos(ry) 0 ;
        0        0 0       1 ];

  % TMry = Ty*Ry*Tyinv;

  %% Rz
  Rz = [cos(rz) -sin(rz) 0 0 ;
        sin(rz)  cos(rz) 0 0 ;
        0        0       1 0 ;
        0        0       0 1 ];

  % TMrz = Ty*TMry*Rz*TMry'*Tyinv;

  %% All stages
  % TM = TMrz*TMry*Ty;

  TM = Ty*Ry*Rz;

  [thetax, thetay, thetaz] = RM2angle(TM(1:3, 1:3));

  setpoint(1:3) = TM(1:3, 4);
  setpoint(4:6) = [thetax, thetay, thetaz];

  %% Custom Functions
  function [thetax, thetay, thetaz] = RM2angle(R)
      if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
          thetay = -asin(R(3, 1));
          thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
          thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
      else
          thetaz = 0;
          if abs(R(3, 1)+1) < 1e-6 % R31 = -1
              thetay = pi/2;
              thetax = thetaz + atan2(R(1, 2), R(1, 3));
          else
              thetay = -pi/2;
              thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
          end
      end
  end
  end
#+end_src

* TODO converErrorBasis
:PROPERTIES:
:header-args:matlab+: :tangle ../src/converErrorBasis.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:converErrorBasis>>

This Matlab function is accessible [[file:../src/converErrorBasis.m][here]].

#+begin_src matlab
  function error_nass = convertErrorBasis(pos, setpoint, ty, ry, rz)
  % convertErrorBasis -
  %
  % Syntax: convertErrorBasis(p_error, ty, ry, rz)
  %
  % Inputs:
  %    - p_error - Position error of the sample w.r.t. the granite [m, rad]
  %    - ty - Measured translation of the Ty stage [m]
  %    - ry - Measured rotation of the Ry stage [rad]
  %    - rz - Measured rotation of the Rz stage [rad]
  %
  % Outputs:
  %    - P_nass - Position error of the sample w.r.t. the NASS base [m]
  %    - R_nass - Rotation error of the sample w.r.t. the NASS base [rad]
  %
  % Example:
  %

  %% If line vector => column vector
  if size(pos, 2) == 6
      pos = pos';
  end

  if size(setpoint, 2) == 6
      setpoint = setpoint';
  end

  %% Position of the sample in the frame fixed to the Granite
  P_granite = [pos(1:3); 1]; % Position [m]
  R_granite = [setpoint(1:3); 1]; % Reference [m]

  %% Transformation matrices of the stages
  % T-y
  TMty = [1 0 0 0  ;
          0 1 0 ty ;
          0 0 1 0  ;
          0 0 0 1 ];

  % R-y
  TMry = [ cos(ry) 0 sin(ry) 0 ;
          0       1 0       0 ;
          -sin(ry) 0 cos(ry) 0 ;
          0        0 0       1 ];

  % R-z
  TMrz = [cos(rz) -sin(rz) 0 0 ;
          sin(rz)  cos(rz) 0 0 ;
          0        0       1 0 ;
          0        0       0 1 ];

  %% Compute Point coordinates in the new reference fixed to the NASS base
  % P_nass = TMrz*TMry*TMty*P_granite;
  P_nass = TMrz\TMry\TMty\P_granite;
  R_nass = TMrz\TMry\TMty\R_granite;

  dx = R_nass(1)-P_nass(1);
  dy = R_nass(2)-P_nass(2);
  dz = R_nass(3)-P_nass(3);

  %% Compute new basis vectors linked to the NASS base
  % ux_nass = TMrz*TMry*TMty*[1; 0; 0; 0];
  % ux_nass = ux_nass(1:3);
  % uy_nass = TMrz*TMry*TMty*[0; 1; 0; 0];
  % uy_nass = uy_nass(1:3);
  % uz_nass = TMrz*TMry*TMty*[0; 0; 1; 0];
  % uz_nass = uz_nass(1:3);

  ux_nass = TMrz\TMry\TMty\[1; 0; 0; 0];
  ux_nass = ux_nass(1:3);
  uy_nass = TMrz\TMry\TMty\[0; 1; 0; 0];
  uy_nass = uy_nass(1:3);
  uz_nass = TMrz\TMry\TMty\[0; 0; 1; 0];
  uz_nass = uz_nass(1:3);

  %% Rotations error w.r.t. granite Frame
  % Rotations error w.r.t. granite Frame
  rx_nass = pos(4);
  ry_nass = pos(5);
  rz_nass = pos(6);

  % Rotation matrices of the Sample w.r.t. the Granite
  Mrx_error = [1 0              0 ;
              0 cos(-rx_nass) -sin(-rx_nass) ;
              0 sin(-rx_nass)  cos(-rx_nass)];

  Mry_error = [ cos(-ry_nass) 0 sin(-ry_nass) ;
                0             1 0 ;
              -sin(-ry_nass) 0 cos(-ry_nass)];

  Mrz_error = [cos(-rz_nass) -sin(-rz_nass) 0 ;
              sin(-rz_nass)  cos(-rz_nass) 0 ;
              0              0             1];

  % Rotation matrix of the Sample w.r.t. the Granite
  Mr_error = Mrz_error*Mry_error*Mrx_error;

  %% Use matrix to solve
  R = Mr_error/[ux_nass, uy_nass, uz_nass]; % Rotation matrix from NASS base to Sample

  [thetax, thetay, thetaz] = RM2angle(R);

  error_nass = [dx; dy; dz; thetax; thetay; thetaz];

  %% Custom Functions
  function [thetax, thetay, thetaz] = RM2angle(R)
      if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
          thetay = -asin(R(3, 1));
          % thetaybis = pi-thetay;
          thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
          % thetaxbis = atan2(R(3, 2)/cos(thetaybis), R(3, 3)/cos(thetaybis));
          thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
          % thetazbis = atan2(R(2, 1)/cos(thetaybis), R(1, 1)/cos(thetaybis));
      else
          thetaz = 0;
          if abs(R(3, 1)+1) < 1e-6 % R31 = -1
              thetay = pi/2;
              thetax = thetaz + atan2(R(1, 2), R(1, 3));
          else
              thetay = -pi/2;
              thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
          end
      end
  end

  end
#+end_src

* Inverse Kinematics of the Hexapod
:PROPERTIES:
:header-args:matlab+: :tangle ../src/inverseKinematicsHexapod.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:inverseKinematicsHexapod>>

This Matlab function is accessible [[file:src/inverseKinematicsHexapod.m][here]].

#+begin_src matlab
  function [L] = inverseKinematicsHexapod(hexapod, AP, ARB)
  % inverseKinematicsHexapod - Compute the initial position of each leg to have the wanted Hexapod's position
  %
  % Syntax: inverseKinematicsHexapod(hexapod, AP, ARB)
  %
  % Inputs:
  %    - hexapod - Hexapod object containing the geometry of the hexapod
  %    - AP - Position vector of point OB expressed in frame {A} in [m]
  %    - ARB - Rotation Matrix expressed in frame {A}

    % Wanted Length of the hexapod's legs [m]
    L = zeros(6, 1);

    for i = 1:length(L)
      Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m]
      Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-hexapod.h-hexapod.jacobian]; % [m]

      L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
    end
  end
#+end_src

* computeReferencePose
:PROPERTIES:
:header-args:matlab+: :tangle ../src/computeReferencePose.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:computeReferencePose>>

This Matlab function is accessible [[file:src/computeReferencePose.m][here]].

#+begin_src matlab
  function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
  % computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
  %
  % Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
  %
  % Inputs:
  %    - Dy - Reference of the Translation Stage [m]
  %    - Ry - Reference of the Tilt Stage [rad]
  %    - Rz - Reference of the Spindle [rad]
  %    - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad]
  %    - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad]
  %
  % Outputs:
  %    - WTr -

    %% Translation Stage
    Rty = [1 0 0 0;
           0 1 0 Dy;
           0 0 1 0;
           0 0 0 1];

    %% Tilt Stage - Pure rotating aligned with Ob
    Rry = [ cos(Ry) 0 sin(Ry) 0;
            0       1 0       0;
           -sin(Ry) 0 cos(Ry) 0;
            0       0 0       1];

    %% Spindle - Rotation along the Z axis
    Rrz = [cos(Rz) -sin(Rz) 0 0 ;
           sin(Rz)  cos(Rz) 0 0 ;
           0        0       1 0 ;
           0        0       0 1 ];


    %% Micro-Hexapod
    Rhx = [1 0           0;
           0 cos(Dh(4)) -sin(Dh(4));
           0 sin(Dh(4))  cos(Dh(4))];

    Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
           0           1 0;
           -sin(Dh(5)) 0 cos(Dh(5))];

    Rhz = [cos(Dh(6)) -sin(Dh(6)) 0;
           sin(Dh(6))  cos(Dh(6)) 0;
           0           0          1];

    Rh = [1 0 0 Dh(1) ;
          0 1 0 Dh(2) ;
          0 0 1 Dh(3) ;
          0 0 0 1 ];

    Rh(1:3, 1:3) = Rhz*Rhy*Rhx;

    %% Nano-Hexapod
    Rnx = [1 0           0;
           0 cos(Dn(4)) -sin(Dn(4));
           0 sin(Dn(4))  cos(Dn(4))];

    Rny = [ cos(Dn(5)) 0 sin(Dn(5));
           0           1 0;
           -sin(Dn(5)) 0 cos(Dn(5))];

    Rnz = [cos(Dn(6)) -sin(Dn(6)) 0;
           sin(Dn(6))  cos(Dn(6)) 0;
           0           0          1];

    Rn = [1 0 0 Dn(1) ;
          0 1 0 Dn(2) ;
          0 0 1 Dn(3) ;
          0 0 0 1 ];

    Rn(1:3, 1:3) = Rnx*Rny*Rnz;

    %% Total Homogeneous transformation
    WTr = Rty*Rry*Rrz*Rh*Rn;
  end
#+end_src
* Compute the Sample Position Error w.r.t. the NASS
:PROPERTIES:
:header-args:matlab+: :tangle ../src/computeSampleError.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:computeSampleError>>

This Matlab function is accessible [[file:src/computeSampleError.m][here]].

#+begin_src matlab
  function [MTr] = computeSampleError(WTm, WTr)
  % computeSampleError -
  %
  % Syntax: [MTr] = computeSampleError(WTm, WTr)
  %
  % Inputs:
  %    - WTm - Homoegeneous transformation that represent the
  %            wanted pose of the sample with respect to the granite
  %    - WTr - Homoegeneous transformation that represent the
  %            measured pose of the sample with respect to the granite
  %
  % Outputs:
  %    - MTr - Homoegeneous transformation that represent the
  %            wanted pose of the sample expressed in a frame
  %            attached to the top platform of the nano-hexapod

  MTr = zeros(4,4);

  MTr = [WTm(1:3,1:3)', -WTm(1:3,1:3)'*WTm(1:3,4) ; 0 0 0 1]*WTr;
  end
#+end_src