#+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/org/}{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:

* 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) = Rnz*Rny*Rnx;

    %% 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