Add flexible hexapod initialized in the wanted configuration

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

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