Update compute error script

This commit is contained in:
Thomas Dehaeze 2020-03-12 14:49:15 +01:00
parent 89dfd6417c
commit 6fee1dcf75
2 changed files with 20 additions and 165 deletions

View File

@ -1714,120 +1714,6 @@ The obtained position error is shown in Figure [[fig:hybrid_control_Ex]].
* Position Error computation * Position Error computation
<<sec:compute_pose_error>> <<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: Let's note:
- $\{W\}$ the fixed measurement frame (corresponding to the metrology frame / the frame where the wanted displacement are expressed). - $\{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. $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$ - $\{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\}$. 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$. Thus we can compute the Homogeneous transformation matrix ${}^WT_M$.
#+begin_src matlab #+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)); 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)); Erx = atan2(-T(2, 3)/cos(Ery), T(3, 3)/cos(Ery));
Erz = atan2(-T(1, 2)/cos(Ery), T(1, 1)/cos(Ery)); Erz = atan2(-T(1, 2)/cos(Ery), T(1, 1)/cos(Ery));
end
#+end_src #+end_src

View File

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