nass-simscape/experiment_tomography/index.org

#+TITLE: Tomography Experiment
: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 matlab/modal_frf_coh.m
#+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:

* Introduction                                                       :ignore:
* Matlab Init                                               :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
  <<matlab-init>>
#+end_src

#+begin_src matlab :tangle no
  simulinkproject('../');
#+end_src

* Simscape Model
The simulink file to do tomography experiments is =sim_nano_station_tomo.slx=.
#+begin_src matlab
  open('experiment_tomography/matlab/sim_nano_station_tomo.slx')
#+end_src

We load the shared simulink configuration and we set the =StopTime=.
#+begin_src matlab
  load('mat/conf_simscape.mat');
  set_param(conf_simscape, 'StopTime', '5');
#+end_src

We first initialize all the stages.
#+begin_src matlab
  initializeGround();
  initializeGranite();
  initializeTy();
  initializeRy();
  initializeRz();
  initializeMicroHexapod();
  initializeAxisc();
  initializeMirror();
  initializeNanoHexapod(struct('actuator', 'piezo'));
  initializeSample(struct('mass', 1));
#+end_src

* Tomography Experiment with no disturbances
** Simulation Setup
We initialize the reference path for all the stages.
All stage is set to its zero position except the Spindle which is rotating at 60rpm.
#+begin_src matlab
  initializeReferences(struct('Rz_type', 'rotating', 'Rz_period', 1));
#+end_src

And we initialize the disturbances to be equal to zero.
#+begin_src matlab
  opts = struct(...
      'Dwx', false, ... % Ground Motion - X direction
      'Dwy', false, ... % Ground Motion - Y direction
      'Dwz', false, ... % Ground Motion - Z direction
      'Fty_x', false, ... % Translation Stage - X direction
      'Fty_z', false, ... % Translation Stage - Z direction
      'Frz_z', false  ... % Spindle - Z direction
  );
  initDisturbances(opts);
#+end_src

We simulate the model.
#+begin_src matlab
  sim('sim_nano_station_tomo');
#+end_src

And we save the obtained data.
#+begin_src matlab
  MTr_alig_no_dist = MTr;
  save('experiment_tomography/mat/experiment.mat', 'MTr_alig_no_dist', '-append');
#+end_src

** Analysis
#+begin_src matlab
  Edx = squeeze(MTr(1, 4, :));
  Edy = squeeze(MTr(2, 4, :));
  Edz = squeeze(MTr(3, 4, :));
  % The angles obtained are u-v-w Euler angles (rotations in the moving frame)
  Ery = atan2( squeeze(MTr(1, 3, :)),           squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
  Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
  Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
  plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
  plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
  hold off;
  xlabel('Time [s]'); ylabel('Displacement [m]');
  xlim([2, inf]);
  legend('location', 'northeast');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_tomo_without_dist_trans.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:exp_tomo_without_dist_trans
#+CAPTION: X-Y-Z translation of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_tomo_without_dist_trans.png][png]], [[./figs/exp_tomo_without_dist_trans.pdf][pdf]])
[[file:figs/exp_tomo_without_dist_trans.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
  plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
  plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
  hold off;
  xlabel('Time [s]'); ylabel('Rotation [rad]');
  xlim([2, inf]);
  legend('location', 'northeast');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_tomo_without_dist_rot.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:exp_tomo_without_dist_rot
#+CAPTION: X-Y-Z rotations of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_tomo_without_dist_rot.png][png]], [[./figs/exp_tomo_without_dist_rot.pdf][pdf]])
[[file:figs/exp_tomo_without_dist_rot.png]]

* With Perturbations
** Simulation Setup
We now activate the disturbances.
#+begin_src matlab
  opts = struct(...
      'Dwx', true, ... % Ground Motion - X direction
      'Dwy', true, ... % Ground Motion - Y direction
      'Dwz', true, ... % Ground Motion - Z direction
      'Fty_x', true, ... % Translation Stage - X direction
      'Fty_z', true, ... % Translation Stage - Z direction
      'Frz_z', true  ... % Spindle - Z direction
  );
  initDisturbances(opts);
#+end_src

We simulate the model.
#+begin_src matlab
  sim('sim_nano_station_tomo');
#+end_src

And we save the obtained data.
#+begin_src matlab
  MTr_alig_dist = MTr;
  save('experiment_tomography/mat/experiment.mat', 'MTr_alig_dist', '-append');
#+end_src

** Analysis
#+begin_src matlab
  Edx = squeeze(MTr(1, 4, :));
  Edy = squeeze(MTr(2, 4, :));
  Edz = squeeze(MTr(3, 4, :));
  % The angles obtained are u-v-w Euler angles (rotations in the moving frame)
  Ery = atan2( squeeze(MTr(1, 3, :)),           squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
  Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
  Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
  plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
  plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
  hold off;
  xlabel('Time [s]'); ylabel('Displacement [m]');
  xlim([2, inf]);
  legend('location', 'northeast');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_tomo_dist_trans.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:exp_tomo_dist_trans
#+CAPTION: X-Y-Z translation of the sample w.r.t. the granite when performing tomography experiment with disturbances ([[./figs/exp_tomo_dist_trans.png][png]], [[./figs/exp_tomo_dist_trans.pdf][pdf]])
[[file:figs/exp_tomo_dist_trans.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
  plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
  plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
  hold off;
  xlabel('Time [s]'); ylabel('Rotation [rad]');
  xlim([2, inf]);
  legend('location', 'northeast');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_tomo_dist_rot.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:exp_tomo_dist_rot
#+CAPTION: X-Y-Z rotations of the sample w.r.t. the granite when performing tomography experiment with disturbances ([[./figs/exp_tomo_dist_rot.png][png]], [[./figs/exp_tomo_dist_rot.pdf][pdf]])
[[file:figs/exp_tomo_dist_rot.png]]

* Tomography
** Simulation Setup
We first set the wanted translation of the Micro Hexapod.
#+begin_src matlab
  P_micro_hexapod = [0.01; 0; 0]; % [m]
#+end_src

We initialize the reference path.
#+begin_src matlab
  initializeReferences(struct('Dh_pos', [P_micro_hexapod; 0; 0; 0], 'Rz_type', 'rotating', 'Rz_period', 1));
#+end_src

We initialize the stages.
#+begin_src matlab
  initializeMicroHexapod(struct('AP', P_micro_hexapod));
#+end_src

And we initialize the disturbances to zero.
#+begin_src matlab
  opts = struct(...
      'Dwx', false, ... % Ground Motion - X direction
      'Dwy', false, ... % Ground Motion - Y direction
      'Dwz', false, ... % Ground Motion - Z direction
      'Fty_x', false, ... % Translation Stage - X direction
      'Fty_z', false, ... % Translation Stage - Z direction
      'Frz_z', false  ... % Spindle - Z direction
  );
  initDisturbances(opts);
#+end_src

We simulate the model.
#+begin_src matlab
  sim('sim_nano_station_tomo');
#+end_src

And we save the obtained data.
#+begin_src matlab
  MTr_not_alig = MTr;
  save('experiment_tomography/mat/experiment.mat', 'MTr_not_alig', '-append');
#+end_src

** Analysis
#+begin_src matlab
  Edx = squeeze(MTr(1, 4, :));
  Edy = squeeze(MTr(2, 4, :));
  Edz = squeeze(MTr(3, 4, :));
  % The angles obtained are u-v-w Euler angles (rotations in the moving frame)
  Ery = atan2( squeeze(MTr(1, 3, :)),           squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
  Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
  Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
  plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
  plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
  hold off;
  xlabel('Time [s]'); ylabel('Displacement [m]');
  xlim([2, inf]);
  legend('location', 'northeast');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_tomo_offset_trans.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:exp_tomo_offset_trans
#+CAPTION: X-Y-Z translation of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_tomo_offset_trans.png][png]], [[./figs/exp_tomo_offset_trans.pdf][pdf]])
[[file:figs/exp_tomo_offset_trans.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
  plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
  plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
  hold off;
  xlabel('Time [s]'); ylabel('Rotation [rad]');
  xlim([2, inf]);
  legend('location', 'northeast');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_tomo_offset_rot.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:exp_tomo_offset_rot
#+CAPTION: X-Y-Z rotations of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_tomo_offset_rot.png][png]], [[./figs/exp_tomo_offset_rot.pdf][pdf]])
[[file:figs/exp_tomo_offset_rot.png]]

* TODO Tests on the transformation from reference to wanted position :noexport:
:PROPERTIES:
:header-args:matlab+: :eval no
:END:
** Introduction                                                     :ignore:
#+begin_quote
Rx = [1 0       0;
      0 cos(t) -sin(t);
      0 sin(t)  cos(t)];

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

Rz = [cos(t) -sin(t) 0;
      sin(t)  cos(t) 0;
      0       0      1];
#+end_quote

Let's define the following frames:
- $\{W\}$ the frame that is *fixed to the granite* and its origin at the theoretical meeting point between the X-ray and the spindle axis.
- $\{S\}$ the frame *attached to the sample* (in reality attached to the top platform of the nano-hexapod) with its origin at 175mm above the top platform of the nano-hexapod.
  Its origin is $O_S$.
- $\{T\}$ the theoretical wanted frame that correspond to the wanted pose of the frame $\{S\}$.
  $\{T\}$ is computed from the wanted position of each stage. It is thus theoretical and does not correspond to a real position.
  The origin of $T$ is $O_T$ and is the wanted position of the sample.

Thus:
- the *measurement* of the position of the sample corresponds to ${}^W O_S = \begin{bmatrix} {}^WP_{x,m} & {}^WP_{y,m} & {}^WP_{z,m} \end{bmatrix}^T$ in translation and to $\theta_m {}^W\bm{s}_m = \theta_m \cdot \begin{bmatrix} {}^Ws_{x,m} & {}^Ws_{y,m} & {}^Ws_{z,m} \end{bmatrix}^T$ in rotations
- the *wanted position* of the sample expressed w.r.t. the granite is ${}^W O_T = \begin{bmatrix} {}^WP_{x,r} & {}^WP_{y,r} & {}^WP_{z,r} \end{bmatrix}^T$ in translation and to $\theta_r {}^W\bm{s}_r = \theta_r \cdot \begin{bmatrix} {}^Ws_{x,r} & {}^Ws_{y,r} & {}^Ws_{z,r} \end{bmatrix}^T$ in rotations

** Wanted Position of the Sample with respect to the Granite
Let's define the wanted position of each stage.
#+begin_src matlab
  Ty = 0; % [m]
  Ry = 3*pi/180; % [rad]
  Rz = 180*pi/180; % [rad]

  % Hexapod (first consider only translations)
  Thx = 0; % [m]
  Thy = 0; % [m]
  Thz = 0; % [m]
#+end_src

Now, we compute the corresponding wanted translation and rotation of the sample with respect to the granite frame $\{W\}$.
This corresponds to ${}^WO_T$ and $\theta_m {}^Ws_m$.

To do so, we have to define the homogeneous transformation for each stage.
#+begin_src matlab
  % Translation Stage
  Rty = [1 0 0 0;
         0 1 0 Ty;
         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 (only rotations first)
  Rh = [1 0 0 Thx ;
        0 1 0 Thy ;
        0 0 1 Thz ;
        0 0 0 1 ];
#+end_src

We combine the individual homogeneous transformations into one homogeneous transformation for all the station.
#+begin_src matlab
  Ttot = Rty*Rry*Rrz*Rh;
#+end_src

Using this homogeneous transformation, we can compute the wanted position and orientation of the sample with respect to the granite.

Translation.
#+begin_src matlab
  WOr = Ttot*[0;0;0;1];
  WOr = WOr(1:3);
#+end_src

Rotation.
#+begin_src matlab
  thetar = acos((trace(Ttot(1:3, 1:3))-1)/2)
  if thetar == 0
    WSr = [0; 0; 0];
  else
    [V, D] = eig(Ttot(1:3, 1:3));
    WSr = thetar*V(:, abs(diag(D) - 1) < eps(1));
  end
#+end_src

#+begin_src matlab
  WPr = [WOr ; WSr];
#+end_src

** Measured Position of the Sample with respect to the Granite
The measurement of the position of the sample using the metrology system gives the position and orientation of the sample with respect to the granite.
#+begin_src matlab
  % Measurements: Xm, Ym, Zm, Rx, Ry, Rz
  Dxm = 0; % [m]
  Dym = 0; % [m]
  Dzm = 0; % [m]

  Rxm = 0*pi/180; % [rad]
  Rym = 0*pi/180; % [rad]
  Rzm = 180*pi/180; % [rad]
#+end_src

Let's compute the corresponding orientation using screw axis.
#+begin_src matlab
  Trxm = [1 0         0;
          0 cos(Rxm) -sin(Rxm);
          0 sin(Rxm)  cos(Rxm)];
  Trym = [ cos(Rym) 0 sin(Rym);
           0        1 0;
          -sin(Rym) 0 cos(Rym)];
  Trzm = [cos(Rzm) -sin(Rzm) 0;
          sin(Rzm)  cos(Rzm) 0;
          0         0        1];

  STw = [[ Trym*Trxm*Trzm , [Dxm; Dym; Dzm]]; 0 0 0 1];
#+end_src

We then obtain the orientation measurement in the form of screw coordinate $\theta_m ({}^Ws_{x,m},\ {}^Ws_{y,m},\ {}^Ws_{z,m})^T$ where:
- $\theta_m = \cos^{-1} \frac{\text{Tr}(R) - 1}{2}$
- ${}^W\bm{s}_m$ is the eigen vector of the rotation matrix $R$ corresponding to the eigen value $\lambda = 1$

#+begin_src matlab
  thetam = acos((trace(STw(1:3, 1:3))-1)/2); % [rad]
  if thetam == 0
    WSm = [0; 0; 0];
  else
    [V, D] = eig(STw(1:3, 1:3));
    WSm = thetam*V(:, abs(diag(D) - 1) < eps(1));
  end
#+end_src

#+begin_src matlab
  WPm = [Dxm ; Dym ; Dzm ; WSm];
#+end_src

** Positioning Error with respect to the Granite
The wanted position expressed with respect to the granite is ${}^WO_T$ and the measured position with respect to the granite is ${}^WO_S$, thus the *position error* expressed in $\{W\}$ is
\[ {}^W E = {}^W O_T - {}^W O_S \]
The same is true for rotations:
\[ \theta_\epsilon {}^W\bm{s}_\epsilon = \theta_r {}^W\bm{s}_r - \theta_m {}^W\bm{s}_m \]

#+begin_src matlab
  WPe = WPr - WPm;
#+end_src

#+begin_quote
Now we want to express this error in a frame attached to the *base of the nano-hexapod* with its origin at the same point where the Jacobian of the nano-hexapod is computed (175mm above the top platform + 90mm of total height of the nano-hexapod).

Or maybe should we want to express this error with respect to the *top platform of the nano-hexapod*?
We are measuring the position of the top-platform, and we don't know exactly the position of the bottom platform.
We could compute the position of the bottom platform in two ways:
- from the encoders of each stage
- from the measurement of the nano-hexapod top platform + the internal metrology in the nano-hexapod (capacitive sensors e.g)

A third option is to say that the maximum stroke of the nano-hexapod is so small that the error should no change to much by the change of base.
#+end_quote

** Position Error Expressed in the Nano-Hexapod Frame
We now want the position error to be expressed in $\{S\}$ (the frame attach to the sample) for control:
\[ {}^S E = {}^S T_W \cdot {}^W E \]

Thus we need to compute the homogeneous transformation ${}^ST_W$.
Fortunately, this homogeneous transformation can be computed from the measurement of the sample position and orientation with respect to the granite.
#+begin_src matlab
  Trxm = [1 0         0;
          0 cos(Rxm) -sin(Rxm);
          0 sin(Rxm)  cos(Rxm)];
  Trym = [ cos(Rym) 0 sin(Rym);
           0        1 0;
          -sin(Rym) 0 cos(Rym)];
  Trzm = [cos(Rzm) -sin(Rzm) 0;
          sin(Rzm)  cos(Rzm) 0;
          0         0        1];

  STw = [[ Trym*Trxm*Trzm , [Dxm; Dym; Dzm]]; 0 0 0 1];
#+end_src

Translation Error.
#+begin_src matlab
  SEm = STw * [WPe(1:3); 0];
  SEm = SEm(1:3);
#+end_src

Rotation Error.
#+begin_src matlab
  SEr = STw * [WPe(4:6); 0];
  SEr = SEr(1:3);
#+end_src

#+begin_src matlab
  Etot = [SEm ; SEr]
#+end_src
** Another try
Let's denote:
- $\{W\}$ the initial fixed frame
- $\{R\}$ the reference frame corresponding to the wanted pose of the sample
- $\{M\}$ the frame corresponding to the measured pose of the sample

We have then computed:
- ${}^WT_R$
- ${}^WT_M$

We have:
\begin{align}
  {}^MT_R &= {}^MT_W {}^WT_R \\
          &= {}^WT_M^t {}^WT_R
\end{align}

#+begin_src matlab
  MTr = STw'*Ttot;
#+end_src

Position error:
#+begin_src matlab
  MTr(1:3, 1:4)*[0; 0; 0; 1]
#+end_src

Orientation error:
#+begin_src matlab
  MTr(1:3, 1:3)
#+end_src

** Verification
How can we verify that the computation is correct?
Options:
- Test with simscape multi-body
  - Impose motion on each stage
  - Measure the position error w.r.t. the NASS
  - Compare with the computation