diff --git a/org/control-tracking.org b/org/control-tracking.org index 5e6eed2..180278b 100644 --- a/org/control-tracking.org +++ b/org/control-tracking.org @@ -1714,120 +1714,6 @@ The obtained position error is shown in Figure [[fig:hybrid_control_Ex]]. * Position Error computation <> -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 diff --git a/src/computePosErrorTest.m b/src/computePosErrorTest.m deleted file mode 100644 index 40fc4f1..0000000 --- a/src/computePosErrorTest.m +++ /dev/null @@ -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