Update compute error script

org/control-tracking.org

@@ -1714,120 +1714,6 @@ The obtained position error is shown in Figure [[fig:hybrid_control_Ex]].
 
 * Position Error computation
 <<sec:compute_pose_error>>
-Let's denote:
-- $\{W\}$ the initial fixed frame (base in which the 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
-
-Reference Position with respect to fixed frame {W}: ${}^WT_R$
-#+begin_src matlab
-  Dx = 0;
-  Dy = 0;
-  Dz = 0.1;
-  Rx = pi;
-  Ry = 0;
-  Rz = 0;
-
-  WTr = zeros(4,4);
-
-  R = [cos(Rz) -sin(Rz) 0;
-       sin(Rz)  cos(Rz) 0;
-       0        0         1] * ...
-      [cos(Ry)  0        sin(Ry);
-       0        1        0;
-       -sin(Ry)  0        cos(Ry)] * ...
-      [1        0        0;
-       0        cos(Rx) -sin(Rx);
-       0        sin(Rx)  cos(Rx)];
-
-
-  WTr(1:3, 1:3) = R;
-  WTr(1:4, 4) = [Dx ; Dy ; Dz; 1];
-#+end_src
-
-Measured Position with respect to fixed frame {W}: ${}^WT_M$
-#+begin_src matlab
-  Dx = 0;
-  Dy = 0;
-  Dz = 0;
-  Rx = pi;
-  Ry = 0;
-  Rz = 0;
-
-  WTm = zeros(4,4);
-
-  R = [cos(Rz) -sin(Rz) 0;
-       sin(Rz)  cos(Rz) 0;
-       0        0         1] * ...
-      [cos(Ry)  0        sin(Ry);
-       0        1        0;
-       -sin(Ry)  0        cos(Ry)] * ...
-      [1        0        0;
-       0        cos(Rx) -sin(Rx);
-       0        sin(Rx)  cos(Rx)];
-
-
-  WTm(1:3, 1:3) = R;
-  WTm(1:4, 4) = [Dx ; Dy ; Dz; 1];
-#+end_src
-
-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}
-
-#+begin_src matlab
-  % Error with respect to the top platform
-  MTr = [WTm(1:3,1:3)', -WTm(1:3,1:3)'*WTm(1:3,4) ; 0 0 0 1]*WTr;
-
-  T = MTr;
-  Edx = T(1, 4);
-  Edy = T(2, 4);
-  Edz = T(3, 4);
-
-  % The angles obtained are u-v-w Euler angles (rotations in the moving frame)
-  Ery = atan2( T(1, 3),          sqrt(T(1, 1)^2 + T(1, 2)^2));
-  Erx = atan2(-T(2, 3)/cos(Ery), T(3, 3)/cos(Ery));
-  Erz = atan2(-T(1, 2)/cos(Ery), T(1, 1)/cos(Ery));
-
-  [Edx, Edy, Edz, Erx, Ery, Erz]
-#+end_src
-
-* Tests
-#+begin_src matlab
-  Dxr = 0.1;
-  Dyr = 0.1;
-  Dzr = 0;
-  Rxr = 0;
-  Ryr = 0;
-  Rzr = 0.1;
-
-  Dxm = 0.1;
-  Dym = 0.1;
-  Dzm = 0.1;
-  Rxm = 0.1;
-  Rym = 0.1;
-  Rzm = 0.1;
-
-  [Edx, Edy, Edz, Erx, Ery, Erz] = computePosErrorTest(Dxr, Dyr, Dzr, Rxr, Ryr, Rzr, Dxm, Dym, Dzm, Rxm, Rym, Rzm)
-#+end_src
-
-* Function Name
-:PROPERTIES:
-:header-args:matlab+: :tangle ../src/computePosErrorTest.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-
-#+begin_src matlab
-  function [Edx, Edy, Edz, Erx, Ery, Erz] = computePosErrorTest(Dxr, Dyr, Dzr, Rxr, Ryr, Rzr, Dxm, Dym, Dzm, Rxm, Rym, Rzm)
-#+end_src
 
 Let's note:
 - $\{W\}$ the fixed measurement frame (corresponding to the metrology frame / the frame where the wanted displacement are expressed).
@@ -1838,6 +1724,26 @@ Let's note:
   $O_R$ is the origin of this frame where the we want to position the point $O_M$ of the element.
 - $\{V\}$ is a frame which its axes are aligned with $\{W\}$ and its origin $O_V$ is coincident with the $O_M$
 
+Reference Position with respect to fixed frame {W}: ${}^WT_R$
+#+begin_src matlab
+  Dxr = 0;
+  Dyr = 0;
+  Dzr = 0.1;
+  Rxr = pi;
+  Ryr = 0;
+  Rzr = 0;
+#+end_src
+
+Measured Position with respect to fixed frame {W}: ${}^WT_M$
+#+begin_src matlab
+  Dxm = 0;
+  Dym = 0;
+  Dzm = 0;
+  Rxm = pi;
+  Rym = 0;
+  Rzm = 0;
+#+end_src
+
 We measure the position and orientation (pose) of the element represented by the frame $\{M\}$ with respect to frame $\{W\}$.
 Thus we can compute the Homogeneous transformation matrix ${}^WT_M$.
 #+begin_src matlab
@@ -1906,5 +1812,4 @@ Now we want to express ${}^VT_R$:
     Ery = atan2( T(1, 3),          sqrt(T(1, 1)^2 + T(1, 2)^2));
     Erx = atan2(-T(2, 3)/cos(Ery), T(3, 3)/cos(Ery));
     Erz = atan2(-T(1, 2)/cos(Ery), T(1, 1)/cos(Ery));
-  end
 #+end_src

src/computePosErrorTest.m

@@ -1,50 +0,0 @@
-function [Edx, Edy, Edz, Erx, Ery, Erz] = computePosErrorTest(Dxr, Dyr, Dzr, Rxr, Ryr, Rzr, Dxm, Dym, Dzm, Rxm, Rym, Rzm)
-
-%% Measured Pose
-WTm = zeros(4,4);
-
-WTm(1:3, 1:3) = [cos(Rzm) -sin(Rzm) 0;
-     sin(Rzm)  cos(Rzm) 0;
-     0        0         1] * ...
-    [cos(Rym)  0        sin(Rym);
-     0        1        0;
-     -sin(Rym)  0        cos(Rym)] * ...
-    [1        0        0;
-     0        cos(Rxm) -sin(Rxm);
-     0        sin(Rxm)  cos(Rxm)];
-WTm(1:4, 4) = [Dxm ; Dym ; Dzm; 1];
-
-%% Reference Pose
-WTr = zeros(4,4);
-
-WTr(1:3, 1:3) = [cos(Rzr) -sin(Rzr) 0;
-     sin(Rzr)  cos(Rzr) 0;
-     0        0         1] * ...
-    [cos(Ryr)  0        sin(Ryr);
-     0        1        0;
-     -sin(Ryr)  0        cos(Ryr)] * ...
-    [1        0        0;
-     0        cos(Rxr) -sin(Rxr);
-     0        sin(Rxr)  cos(Rxr)];
-WTr(1:4, 4) = [Dxr ; Dyr ; Dzr; 1];
-
-WTv = eye(4);
-WTv(1:3, 4) = WTm(1:3, 4);
-
-%% Wanted pose expressed in a frame corresponding to the actual measured pose
-MTr = [WTm(1:3,1:3)', -WTm(1:3,1:3)'*WTm(1:3,4) ; 0 0 0 1]*WTr;
-
-%% Wanted pose expressed in a frame coincident with the actual position but with no rotation
-VTr = [WTv(1:3,1:3)', -WTv(1:3,1:3)'*WTv(1:3,4) ; 0 0 0 1] * WTr;
-
-%% Extract Translations and Rotations from the Homogeneous Matrix
-  T = MTr;
-  Edx = T(1, 4);
-  Edy = T(2, 4);
-  Edz = T(3, 4);
-
-  % The angles obtained are u-v-w Euler angles (rotations in the moving frame)
-  Ery = atan2( T(1, 3),          sqrt(T(1, 1)^2 + T(1, 2)^2));
-  Erx = atan2(-T(2, 3)/cos(Ery), T(3, 3)/cos(Ery));
-  Erz = atan2(-T(1, 2)/cos(Ery), T(1, 1)/cos(Ery));
-end