Add flexible hexapod initialized in the wanted configuration

Change the tomography experiment simulation
Add simulink "matlab function" to compute the position error

experiment_tomography/index.org

@@ -61,10 +61,10 @@ The simulink file to do tomography experiments is =sim_nano_station_tomo.slx=.
   open('experiment_tomography/matlab/sim_nano_station_tomo.slx')
 #+end_src
 
-We load the shared simulink configuration and we set a small =StopTime=.
+We load the shared simulink configuration and we set the =StopTime=.
 #+begin_src matlab
   load('mat/conf_simscape.mat');
-  set_param(conf_simscape, 'StopTime', '10');
+  set_param(conf_simscape, 'StopTime', '5');
 #+end_src
 
 We first initialize all the stages.
@@ -81,14 +81,15 @@ We first initialize all the stages.
   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
 
-* Tomography Experiment with no disturbances
-And we initialize the disturbances to zero.
+And we initialize the disturbances to be equal to zero.
 #+begin_src matlab
   opts = struct(...
       'Dwx', false, ... % Ground Motion - X direction
@@ -101,21 +102,36 @@ And we initialize the disturbances to zero.
   initDisturbances(opts);
 #+end_src
 
+We simulate the model.
 #+begin_src matlab
-  sim('sim_nano_station_tomo')
+  sim('sim_nano_station_tomo');
 #+end_src
 
+And we save the obtained data.
 #+begin_src matlab
-  Dsm_without_dist = Dsm;
+  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(Dsm_without_dist.x.Time, Dsm_without_dist.x.Data, 'DisplayName', 'x')
-  plot(Dsm_without_dist.y.Time, Dsm_without_dist.y.Data, 'DisplayName', 'y')
-  plot(Dsm_without_dist.z.Time, Dsm_without_dist.z.Data, 'DisplayName', 'z')
+  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
@@ -129,11 +145,30 @@ And we initialize the disturbances to zero.
 #+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
 
-Rotations.
-Think of the good way to plot these rotations with respect to time.
+#+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
@@ -146,23 +181,42 @@ Think of the good way to plot these rotations with respect to time.
   initDisturbances(opts);
 #+end_src
 
+We simulate the model.
 #+begin_src matlab
-  sim('sim_nano_station_tomo')
+  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(Dsm.x.Time, Dsm.x.Data, 'DisplayName', 'x')
-  plot(Dsm.y.Time, Dsm.y.Data, 'DisplayName', 'y')
-  plot(Dsm.z.Time, Dsm.z.Data, 'DisplayName', 'z')
+  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-tall" :post pdf2svg(file=*this*, ext="png")
+#+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
 
@@ -170,6 +224,121 @@ Think of the good way to plot these rotations with respect to time.
 #+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

functions/index.org

@@ -42,6 +42,9 @@
 :END:
 
 * Functions
+:PROPERTIES:
+:ID:       8b6ae566-b1a0-448c-8cb1-9d5b3b846ed4
+:END:
 <<sec:functions>>
 ** TODO computePsdDispl
 :PROPERTIES:
@@ -405,3 +408,34 @@ This Matlab function is accessible [[file:src/computeReferencePose.m][here]].
     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