nass-simscape/org/positioning_error.org

#+TITLE: Computation of the Positioning Error with respect to the nano-hexapod
#+SETUPFILE: ./setup/org-setup-file.org

* Introduction                                                       :ignore:
The global measurement and control schematic is shown in figure [[fig:control-schematic-nass]].

#+name: fig:control-schematic-nass
#+caption: Global Control Schematic for the Station
[[file:figs/control-schematic-nass.png]]

In this document, are interesting by the "compute Sample Position w.r.t. Granite" bloc and we develop and verify that the two green blocs are working.

We suppose that we are able to measure perfectly the position of the sample with respect to the granite.
This means that we do not care about the bloc "Compute Sample Position w.r.t. Granite" that makes the transformation from the interferometer measurements to the position of the sample.
We suppose that we can directly measure perfectly the position of the sample with respect to the granite.
This is actually done with a "transform sensor block" that outputs the x-y-z translation of the sample with respect to the granite as well as the rotation matrix that maps the granite frame to the sample frame.

Also, all the stages can be perfectly positioned.

First, in section [[sec:measurement_principle]], is explained how the measurement of the position of the sample with respect to the granite is performed (using Simscape blocs).

In section [[sec:compute_reference]], we verify that the function developed to compute the wanted pose (translation and orientation) of the sample with respect to the granite can be determined from the wanted position of each stage (translation stage, tilt stage, spindle and micro-hexapod). This corresponds to the bloc "Compute Wanted Sample Position w.r.t. Granite" in figure [[fig:control-schematic-nass]].
To do so, we impose a perfect displacement and all the stage, we perfectly measure the position of the sample with respect to the granite, and we verify that this measured position corresponds to the computed wanted pose of the sample.

Then, in section [[sec:compute_pos_error]], we introduce some positioning error in the micro-station's stages.
The positioning error of the sample expressed with respect to the granite frame (the one measured) is expressed in a frame connected to the NASS top platform (corresponding to the green bloc "Compute Sample Position Error w.r.t. NASS" in figure [[fig:control-schematic-nass]]).
Then, we move the NASS such that it compensate for the positioning error that are expressed in the frame of the NASS, and we verify that the positioning error of the sample is well compensated.

* How do we measure the position of the sample with respect to the granite
<<sec:measurement_principle>>
A transform sensor block gives the translation and orientation of the follower frame with respect to the base frame.

The base frame is fixed to the granite and located at the initial sample location that defines the zero position.

The follower frame is attached to the sample (or more precisely to the reflector).

The outputs of the transform sensor are:
- the 3 translations x, y and z in meter
- the *rotation matrix* $\bm{R}$ that permits to rotate the base frame into the follower frame.

We can then determine extract other orientation conventions such that Euler angles or screw axis.

* Verify that the function to compute the reference pose is correct
<<sec:compute_reference>>
** Introduction                                                    :ignore:
The goal here is to perfectly move the station and verify that there is no mismatch between the metrology measurement and the computation of the reference pose.

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

#+begin_src matlab
  open('nass_model.slx')
#+end_src

** Prepare the Simulation
We set a small =StopTime=.
#+begin_src matlab
  load('mat/conf_simulink.mat');
  set_param(conf_simulink, 'StopTime', '0.5');
#+end_src

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

No disturbance and no gravity.
#+begin_src matlab
  initializeSimscapeConfiguration('gravity', false);
  initializeDisturbances('enable', false);
#+end_src

We setup the reference path to be constant.
#+begin_src matlab
  initializeReferences(...
      'Ts',           1e-3, ...       % Sampling Frequency [s]
      'Dy_type',      'constant', ... % Either "constant" / "triangular" / "sinusoidal"
      'Dy_amplitude', 5e-3, ...          % Amplitude of the displacement [m]
      'Dy_period',    1, ...          % Period of the displacement [s]
      'Ry_type',      'constant', ... % Either "constant" / "triangular" / "sinusoidal"
      'Ry_amplitude', -1*pi/180, ...          % Amplitude [rad]
      'Ry_period',    10, ...         % Period of the displacement [s]
      'Rz_type',      'constant', ... % Either "constant" / "rotating"
      'Rz_amplitude', -135*pi/180, ...          % Initial angle [rad]
      'Rz_period',    1, ...          % Period of the rotating [s]
      'Dh_type',      'constant', ... % For now, only constant is implemented
      'Dh_pos',       [0.01; 0.02; -0.03; -3*pi/180; 1*pi/180; 3*pi/180], ...  % Initial position [m,m,m,rad,rad,rad] of the top platform
      'Rm_type',      'constant', ... % For now, only constant is implemented
      'Rm_pos',       [0, pi]', ...   % Initial position of the two masses
      'Dn_type',      'constant', ... % For now, only constant is implemented
      'Dn_pos',       [1e-3; 2e-3; 3e-3; 1*pi/180; 0; 1*pi/180] ...  % Initial position [m,m,m,rad,rad,rad] of the top platform
      );
#+end_src

No position error for now (perfect positioning).
#+begin_src matlab
  Dye = 0; % [m]
  Rye = 0; % [rad]
  Rze = 0; % [rad]
  Dhe = zeros(6,1); % [m,m,m,rad,rad,rad]
  Dhle = zeros(6,1); % [m,m,m,m,m,m]
  Dne = zeros(6,1); % [m,m,m,rad,rad,rad]
#+end_src

We want to log the signals
#+begin_src matlab
  initializeLoggingConfiguration('log', 'all');
#+end_src

And we run the simulation.
#+begin_src matlab
  sim('nass_model');
#+end_src

** Verify that the pose of the sample is the same as the computed one
Let's denote:
- $\{W\}$ the initial fixed frame (base in which the interferometric measurement is done)
- $\{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:
- ${}^W\bm{T}_R$ which corresponds to the wanted pose of the sample with respect to the granite
- ${}^W\bm{T}_M$ which corresponds to the measured pose of the sample with respect to the granite

We load the reference and we compute the desired trajectory of the sample in the form of an homogeneous transformation matrix ${}^W\bm{T}_R$.
#+begin_src matlab
  n = length(simout.r.Dy.Time);
  WTr = zeros(4, 4, n);
  for i = 1:n
    WTr(:, :, i) = computeReferencePose(simout.r.Dy.Data(i), simout.r.Ry.Data(i), simout.r.Rz.Data(i), simout.r.Dh.Data(i,:), simout.r.Dn.Data(i,:));
  end
#+end_src

As the displacement is perfect, we also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix ${}^W\bm{T}_M$.
#+begin_src matlab
  n = length(simout.y.R.Time);
  WTm = zeros(4, 4, n);
  WTm(1:3, 1:3, :) = simout.y.R.Data;
  WTm(1:3, 4, :) = [simout.y.x.Data' ; simout.y.y.Data' ; simout.y.z.Data'];
  WTm(4, 4, :) = 1;
#+end_src

As the simulation is perfect (no measurement error and no motion error), we should have that
\[ {}^W\bm{T}_R = {}^W\bm{T}_M \]

Or are least:
\[ {}^W\bm{T}_R(1:3, 4) = {}^W\bm{T}_M(1:3, 4) \]
\[ {}^W\bm{R}_R^t \cdot {}^W\bm{R}_M = \bm{I}_3 \]

#+begin_src matlab :results output replace
  WTr(1:3, 4, end)-WTm(1:3, 4, end)
  WTr(1:3, 1:3, end)'*WTm(1:3, 1:3, end)-eye(3)
#+end_src

#+RESULTS:
#+begin_example
WTr(1:3, 4, end)-WTm(1:3, 4, end)
ans =
      8.53830894875784e-15
     -5.58580959264532e-15
     -5.89805981832114e-17
WTr(1:3, 1:3, end)'*WTm(1:3, 1:3, end)-eye(3)
ans =
      1.62092561595273e-14     -1.59832000065641e-14      1.11022302462516e-16
      1.61295672998496e-14      1.62092561595273e-14     -1.72431513512095e-15
     -1.65492619608187e-15      -9.8879238130678e-16      9.41469124882133e-14
#+end_example

** Conclusion
#+begin_important
We are able to compute the wanted position and orientation of the sample.
Both the measurement and the theory gives the same result.
#+end_important

* Verify that the function to convert the position error in the frame fixed to the nano-hexapod is working
<<sec:compute_pos_error>>
** Introduction                                                     :ignore:
We now introduce some positioning error in the stage.
This will induce a global positioning error of the sample with respect to the desired pose that we can compute.

We want to verify that we are able to measure this positioning error and convert it in the frame attached to the Nano-hexapod.

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

#+begin_src matlab
  open('nass_model.slx')
#+end_src

** Prepare the Simulation
We set a small =StopTime=.
#+begin_src matlab
  load('mat/conf_simulink.mat');
  set_param(conf_simulink, 'StopTime', '0.5');
#+end_src

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

No disturbance and no gravity.
#+begin_src matlab
  initializeSimscapeConfiguration('gravity', false);
  initializeDisturbances('enable', false);
#+end_src

We setup the reference path to be constant.
#+begin_src matlab
  initializeReferences(...
      'Ts',           1e-3, ...       % Sampling Frequency [s]
      'Dy_type',      'constant', ... % Either "constant" / "triangular" / "sinusoidal"
      'Dy_amplitude', 0, ...          % Amplitude of the displacement [m]
      'Ry_type',      'constant', ... % Either "constant" / "triangular" / "sinusoidal"
      'Ry_amplitude', 0, ...          % Amplitude [rad]
      'Rz_type',      'constant', ... % Either "constant" / "rotating"
      'Rz_amplitude', 0*pi/180, ...          % Initial angle [rad]
      'Dh_type',      'constant', ... % For now, only constant is implemented
      'Dh_pos',       [0; 0; 0; 0; 0; 0],  ...  % Initial position [m,m,m,rad,rad,rad] of the top platform
      'Rm_type',      'constant', ... % For now, only constant is implemented
      'Rm_pos',       [0, pi]', ...   % Initial position of the two masses
      'Dn_type',      'constant', ... % For now, only constant is implemented
      'Dn_pos',       [0; 0; 0; 0; 0; 0]  ...  % Initial position [m,m,m,rad,rad,rad] of the top platform
      );
#+end_src

Now we introduce some positioning error.
#+begin_src matlab
  Dye = 1e-6; % [m]
  Rye = 2e-4; % [rad]
  Rze = 1e-5; % [rad]
  initializePosError('error', true, 'Dy', Dye, 'Ry', Rye, 'Rz', Rze);
#+end_src

And we run the simulation.
#+begin_src matlab
  sim('nass_model');
#+end_src

** Compute the wanted pose of the sample in the NASS Base from the metrology and the reference
Now that we have introduced some positioning error, the computed wanted pose and the measured pose will not be the same.

We would like to compute ${}^M\bm{T}_R$ which corresponds to the wanted pose of the sample expressed in a frame attached to the top platform of the nano-hexapod (frame $\{M\}$).

We have:
\begin{align}
  {}^M\bm{T}_R &= {}^M\bm{T}_W \cdot {}^W\bm{T}_R \\
          &= {}^W{\bm{T}_M}^{-1} \cdot {}^W\bm{T}_R
\end{align}

The top platform of the nano-hexapod is considered to be rigidly connected to the sample, thus, ${}^M\bm{T}_R$ corresponds to the pose error of the sample with respect to the nano-hexapod platform.

We load the reference and we compute the desired trajectory of the sample in the form of an homogeneous transformation matrix ${}^W\bm{T}_R$.
#+begin_src matlab
  n = length(simout.r.Dy.Time);
  WTr = zeros(4, 4, n);
  for i = 1:n
    WTr(:, :, i) = computeReferencePose(simout.r.Dy.Data(i), simout.r.Ry.Data(i), simout.r.Rz.Data(i), simout.r.Dh.Data(i,:), simout.r.Dn.Data(i,:));
  end
#+end_src

We also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix ${}^W\bm{T}_M$.
#+begin_src matlab
  n = length(simout.y.R.Time);
  WTm = zeros(4, 4, n);
  WTm(1:3, 1:3, :) = simout.y.R.Data;
  WTm(1:3, 4, :) = [simout.y.x.Data' ; simout.y.y.Data' ; simout.y.z.Data'];
  WTm(4, 4, :) = 1;
#+end_src

The *inverse of the transformation matrix* can be obtain by (it is less computation intensive than doing a full inverse)
\begin{equation}
  {}^B\bm{T}_A = {}^A\bm{T}_B^{-1} =
  \left[ \begin{array}{ccc|c}
      &   &   & \\
      & {}^A\bm{R}_B^T  &   & -{}^A \bm{R}_B^T {}^A\bm{P}_{O_B} \\
      &   &   & \\
    \hline
    0 & 0 & 0 & 1 \\
  \end{array} \right]
\end{equation}

Finally, we compute ${}^M\bm{T}_R$.
#+begin_src matlab
  MTr = zeros(4, 4, n);
  for i = 1:n
    MTr(:, :, i) = [WTm(1:3,1:3,i)', -WTm(1:3,1:3,i)'*WTm(1:3,4,i) ; 0 0 0 1]*WTr(:,:,i);
  end
#+end_src

Verify that the pose error corresponds to the positioning error of the stages.
#+begin_src matlab :exports none
  Edx = MTr(1, 4, end);
  Edy = MTr(2, 4, end);
  Edz = MTr(3, 4, end);
  % The angles obtained are u-v-w Euler angles (rotations in the moving frame)
  Ery = atan2( MTr(1, 3, end),          sqrt(MTr(1, 1, end)^2 + MTr(1, 2, end)^2));
  Erx = atan2(-MTr(2, 3, end)/cos(Ery), MTr(3, 3, end)/cos(Ery));
  Erz = atan2(-MTr(1, 2, end)/cos(Ery), MTr(1, 1, end)/cos(Ery));
#+end_src

#+begin_src matlab
  MTr(1:3, 1:3, end)
  Rx = [1 0       0;
        0 cos(Erx) -sin(Erx);
        0 sin(Erx)  cos(Erx)];

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

  Rz = [cos(Erz) -sin(Erz) 0;
        sin(Erz)  cos(Erz) 0;
        0       0      1];
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable([Edx, Edy, Edz, Erx, Ery, Erz], {'Error'}, {'Edx [m]', 'Edy [m]', 'Edz [m]', 'Erx [rad]', 'Ery [rad]', 'Erz [rad]'}, ' %.1e ');
#+end_src

#+RESULTS:
|       |  Edx [m] |  Edy [m] |  Edz [m] | Erx [rad] | Ery [rad] | Erz [rad] |
|-------+----------+----------+----------+-----------+-----------+-----------|
| Error | -1.0e-11 | -1.0e-06 | -6.2e-16 |  -2.0e-09 |  -2.0e-04 |  -1.0e-05 |

** Verify that be imposing the error motion on the nano-hexapod, we indeed have zero error at the end
We now keep the wanted pose but we impose a displacement of the nano hexapod corresponding to the measured position error.
#+begin_src matlab
  initializeReferences(...
      'Ts',           1e-3, ...       % Sampling Frequency [s]
      'Dy_type',      'constant', ... % Either "constant" / "triangular" / "sinusoidal"
      'Dy_amplitude', 0, ...          % Amplitude of the displacement [m]
      'Ry_type',      'constant', ... % Either "constant" / "triangular" / "sinusoidal"
      'Ry_amplitude', 0, ...          % Amplitude [rad]
      'Rz_type',      'constant', ... % Either "constant" / "rotating"
      'Rz_amplitude', 0*pi/180, ...          % Initial angle [rad]
      'Dh_type',      'constant', ... % For now, only constant is implemented
      'Dh_pos',       [0; 0; 0; 0; 0; 0],  ...  % Initial position [m,m,m,rad,rad,rad] of the top platform
      'Rm_type',      'constant', ... % For now, only constant is implemented
      'Rm_pos',       [0, pi]', ...   % Initial position of the two masses
      'Dn_type',      'constant', ... % For now, only constant is implemented
      'Dn_pos',       [Edx; Edy; Edz; Erx; Ery; Erz] ...  % Initial position [m,m,m,rad,rad,rad] of the top platform
      );
#+end_src

And we run the simulation.
#+begin_src matlab
  sim('nass_model');
#+end_src

We keep the old computed computed reference pose ${}^W\bm{T}_r$ even though we have change the nano hexapod reference, but this is not a real wanted reference but rather a adaptation to reject the positioning errors.

As the displacement is perfect, we also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix ${}^W\bm{T}_M$.
#+begin_src matlab
  n = length(simout.y.R.Time);
  WTm = zeros(4, 4, n);
  WTm(1:3, 1:3, :) = simout.y.R.Data;
  WTm(1:3, 4, :) = [simout.y.x.Data' ; simout.y.y.Data' ; simout.y.z.Data'];
  WTm(4, 4, :) = 1;
#+end_src

Finally, we compute ${}^M\bm{T}_R$.
#+begin_src matlab
  MTr = zeros(4, 4, n);
  for i = 1:n
    MTr(:, :, i) = [WTm(1:3,1:3,i)', -WTm(1:3,1:3,i)'*WTm(1:3,4,i) ; 0 0 0 1]*WTr(:,:,i);
  end
#+end_src

Verify that the pose error is small.
#+begin_src matlab :exports none
  Edx = MTr(1, 4, end);
  Edy = MTr(2, 4, end);
  Edz = MTr(3, 4, end);
  Ery = atan2(MTr(1, 3, end), sqrt(MTr(1, 1, end)^2 + MTr(1, 2, end)^2));
  Erx = atan2(-MTr(2, 3, end)/cos(Ery), MTr(3, 3, end)/cos(Ery));
  Erz = atan2(-MTr(1, 2, end)/cos(Ery), MTr(1, 1, end)/cos(Ery));
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable([Edx, Edy, Edz, Erx, Ery, Erz], {'Error'}, {'Edx [m]', 'Edy [m]', 'Edz [m]', 'Erx [rad]', 'Ery [rad]', 'Erz [rad]'}, ' %.1e ');
#+end_src

#+RESULTS:
|       | Edx [m] |  Edy [m] | Edz [m] | Erx [rad] | Ery [rad] | Erz [rad] |
|-------+---------+----------+---------+-----------+-----------+-----------|
| Error | 2.4e-16 | -9.9e-17 | 4.4e-16 |   2.0e-09 |  -8.9e-15 |   2.0e-13 |

** Conclusion
#+begin_important
  Indeed, we are able to convert the position error in the frame of the NASS and then compensate these errors with the NASS.
#+end_important