Now that we have introduced some positioning error, the computed wanted pose and the measured pose will not be the same.
@@ -763,8 +761,8 @@ Rz = [cos
-2.3 Verify that be imposing the error motion on the nano-hexapod, we indeed have zero error at the end
+
+
2.3 Verify that be imposing the error motion on the nano-hexapod, we indeed have zero error at the end
We now keep the wanted pose but we impose a displacement of the nano hexapod corresponding to the measured position error.
@@ -858,8 +856,8 @@ Verify that the pose error is small.
-
2.4 Conclusion
+
+
2.4 Conclusion
@@ -870,14 +868,10 @@ Indeed, we are able to convert the position error in the frame of the NASS and t
-
-
-
3 Verify that we are able to compensate the errors using the nano-hexapod
-
Author: Dehaeze Thomas
-
Created: 2019-12-12 jeu. 13:17
+
Created: 2019-12-12 jeu. 13:46
Validate
diff --git a/positioning_error/index.org b/positioning_error/index.org
index 1ed1225..4696a1a 100644
--- a/positioning_error/index.org
+++ b/positioning_error/index.org
@@ -289,7 +289,6 @@ Now we introduce some positioning error.
Rye = 2e-4; % [rad]
Rze = 1e-5; % [rad]
Dhe = zeros(6,1);
- % Dne = [1e-3 ; 0 ; 2e-3 ; 0 ; 3e-3 ; 1e-3]; % [m,rad]
Dhle = [1e-6 ; 2e-6 ; 3e-6 ; -2e-6 ; 1e-6 ; 2e-6]; % [m]
Dne = zeros(6,1);
#+end_src
@@ -441,261 +440,3 @@ Verify that the pose error is small.
#+begin_important
Indeed, we are able to convert the position error in the frame of the NASS and then compensate these errors with the NASS.
#+end_important
-
-* Verify that we are able to compensate the errors using the nano-hexapod
-* Tests on the transformation from reference to wanted position :noexport:
-** Introduction :ignore:
-#+begin_quote
-Rx = [1 0 0;
- 0 cos(t) -sin(t);
- 0 sin(t) cos(t)];
-
-Ry = [ cos(t) 0 sin(t);
- 0 1 0;
- -sin(t) 0 cos(t)];
-
-Rz = [cos(t) -sin(t) 0;
- sin(t) cos(t) 0;
- 0 0 1];
-#+end_quote
-
-Let's define the following frames:
-- $\{W\}$ the frame that is *fixed to the granite* and its origin at the theoretical meeting point between the X-ray and the spindle axis.
-- $\{S\}$ the frame *attached to the sample* (in reality attached to the top platform of the nano-hexapod) with its origin at 175mm above the top platform of the nano-hexapod.
- Its origin is $O_S$.
-- $\{T\}$ the theoretical wanted frame that correspond to the wanted pose of the frame $\{S\}$.
- $\{T\}$ is computed from the wanted position of each stage. It is thus theoretical and does not correspond to a real position.
- The origin of $T$ is $O_T$ and is the wanted position of the sample.
-
-Thus:
-- the *measurement* of the position of the sample corresponds to ${}^W O_S = \begin{bmatrix} {}^WP_{x,m} & {}^WP_{y,m} & {}^WP_{z,m} \end{bmatrix}^T$ in translation and to $\theta_m {}^W\bm{s}_m = \theta_m \cdot \begin{bmatrix} {}^Ws_{x,m} & {}^Ws_{y,m} & {}^Ws_{z,m} \end{bmatrix}^T$ in rotations
-- the *wanted position* of the sample expressed w.r.t. the granite is ${}^W O_T = \begin{bmatrix} {}^WP_{x,r} & {}^WP_{y,r} & {}^WP_{z,r} \end{bmatrix}^T$ in translation and to $\theta_r {}^W\bm{s}_r = \theta_r \cdot \begin{bmatrix} {}^Ws_{x,r} & {}^Ws_{y,r} & {}^Ws_{z,r} \end{bmatrix}^T$ in rotations
-
-** Matlab Init :noexport:ignore:
-#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
- <>
-#+end_src
-
-#+begin_src matlab :exports none :results silent :noweb yes
- <>
-#+end_src
-
-#+begin_src matlab :tangle no
- simulinkproject('../');
-#+end_src
-
-#+begin_src matlab
- open('positioning_error/matlab/sim_nano_station_metrology.slx')
-#+end_src
-
-** Wanted Position of the Sample with respect to the Granite
-Let's define the wanted position of each stage.
-#+begin_src matlab
- Ty = 0; % [m]
- Ry = 3*pi/180; % [rad]
- Rz = 180*pi/180; % [rad]
-
- % Hexapod (first consider only translations)
- Thx = 0; % [m]
- Thy = 0; % [m]
- Thz = 0; % [m]
-#+end_src
-
-Now, we compute the corresponding wanted translation and rotation of the sample with respect to the granite frame $\{W\}$.
-This corresponds to ${}^WO_T$ and $\theta_m {}^Ws_m$.
-
-To do so, we have to define the homogeneous transformation for each stage.
-#+begin_src matlab
- % Translation Stage
- Rty = [1 0 0 0;
- 0 1 0 Ty;
- 0 0 1 0;
- 0 0 0 1];
-
- % Tilt Stage - Pure rotating aligned with Ob
- Rry = [ cos(Ry) 0 sin(Ry) 0;
- 0 1 0 0;
- -sin(Ry) 0 cos(Ry) 0;
- 0 0 0 1];
-
- % Spindle - Rotation along the Z axis
- Rrz = [cos(Rz) -sin(Rz) 0 0 ;
- sin(Rz) cos(Rz) 0 0 ;
- 0 0 1 0 ;
- 0 0 0 1 ];
-
- % Micro-Hexapod (only rotations first)
- Rh = [1 0 0 Thx ;
- 0 1 0 Thy ;
- 0 0 1 Thz ;
- 0 0 0 1 ];
-#+end_src
-
-We combine the individual homogeneous transformations into one homogeneous transformation for all the station.
-#+begin_src matlab
- Ttot = Rty*Rry*Rrz*Rh;
-#+end_src
-
-Using this homogeneous transformation, we can compute the wanted position and orientation of the sample with respect to the granite.
-
-Translation.
-#+begin_src matlab
- WOr = Ttot*[0;0;0;1];
- WOr = WOr(1:3);
-#+end_src
-
-Rotation.
-#+begin_src matlab
- thetar = acos((trace(Ttot(1:3, 1:3))-1)/2)
- if thetar == 0
- WSr = [0; 0; 0];
- else
- [V, D] = eig(Ttot(1:3, 1:3));
- WSr = thetar*V(:, abs(diag(D) - 1) < eps(1));
- end
-#+end_src
-
-#+begin_src matlab
- WPr = [WOr ; WSr];
-#+end_src
-
-** Measured Position of the Sample with respect to the Granite
-The measurement of the position of the sample using the metrology system gives the position and orientation of the sample with respect to the granite.
-#+begin_src matlab
- % Measurements: Xm, Ym, Zm, Rx, Ry, Rz
- Dxm = 0; % [m]
- Dym = 0; % [m]
- Dzm = 0; % [m]
-
- Rxm = 0*pi/180; % [rad]
- Rym = 0*pi/180; % [rad]
- Rzm = 180*pi/180; % [rad]
-#+end_src
-
-Let's compute the corresponding orientation using screw axis.
-#+begin_src matlab
- Trxm = [1 0 0;
- 0 cos(Rxm) -sin(Rxm);
- 0 sin(Rxm) cos(Rxm)];
- Trym = [ cos(Rym) 0 sin(Rym);
- 0 1 0;
- -sin(Rym) 0 cos(Rym)];
- Trzm = [cos(Rzm) -sin(Rzm) 0;
- sin(Rzm) cos(Rzm) 0;
- 0 0 1];
-
- STw = [[ Trym*Trxm*Trzm , [Dxm; Dym; Dzm]]; 0 0 0 1];
-#+end_src
-
-We then obtain the orientation measurement in the form of screw coordinate $\theta_m ({}^Ws_{x,m},\ {}^Ws_{y,m},\ {}^Ws_{z,m})^T$ where:
-- $\theta_m = \cos^{-1} \frac{\text{Tr}(R) - 1}{2}$
-- ${}^W\bm{s}_m$ is the eigen vector of the rotation matrix $R$ corresponding to the eigen value $\lambda = 1$
-
-#+begin_src matlab
- thetam = acos((trace(STw(1:3, 1:3))-1)/2); % [rad]
- if thetam == 0
- WSm = [0; 0; 0];
- else
- [V, D] = eig(STw(1:3, 1:3));
- WSm = thetam*V(:, abs(diag(D) - 1) < eps(1));
- end
-#+end_src
-
-#+begin_src matlab
- WPm = [Dxm ; Dym ; Dzm ; WSm];
-#+end_src
-
-** Positioning Error with respect to the Granite
-The wanted position expressed with respect to the granite is ${}^WO_T$ and the measured position with respect to the granite is ${}^WO_S$, thus the *position error* expressed in $\{W\}$ is
-\[ {}^W E = {}^W O_T - {}^W O_S \]
-The same is true for rotations:
-\[ \theta_\epsilon {}^W\bm{s}_\epsilon = \theta_r {}^W\bm{s}_r - \theta_m {}^W\bm{s}_m \]
-
-#+begin_src matlab
- WPe = WPr - WPm;
-#+end_src
-
-#+begin_quote
-Now we want to express this error in a frame attached to the *base of the nano-hexapod* with its origin at the same point where the Jacobian of the nano-hexapod is computed (175mm above the top platform + 90mm of total height of the nano-hexapod).
-
-Or maybe should we want to express this error with respect to the *top platform of the nano-hexapod*?
-We are measuring the position of the top-platform, and we don't know exactly the position of the bottom platform.
-We could compute the position of the bottom platform in two ways:
-- from the encoders of each stage
-- from the measurement of the nano-hexapod top platform + the internal metrology in the nano-hexapod (capacitive sensors e.g)
-
-A third option is to say that the maximum stroke of the nano-hexapod is so small that the error should no change to much by the change of base.
-#+end_quote
-
-** Position Error Expressed in the Nano-Hexapod Frame
-We now want the position error to be expressed in $\{S\}$ (the frame attach to the sample) for control:
-\[ {}^S E = {}^S T_W \cdot {}^W E \]
-
-Thus we need to compute the homogeneous transformation ${}^ST_W$.
-Fortunately, this homogeneous transformation can be computed from the measurement of the sample position and orientation with respect to the granite.
-#+begin_src matlab
- Trxm = [1 0 0;
- 0 cos(Rxm) -sin(Rxm);
- 0 sin(Rxm) cos(Rxm)];
- Trym = [ cos(Rym) 0 sin(Rym);
- 0 1 0;
- -sin(Rym) 0 cos(Rym)];
- Trzm = [cos(Rzm) -sin(Rzm) 0;
- sin(Rzm) cos(Rzm) 0;
- 0 0 1];
-
- STw = [[ Trym*Trxm*Trzm , [Dxm; Dym; Dzm]]; 0 0 0 1];
-#+end_src
-
-Translation Error.
-#+begin_src matlab
- SEm = STw * [WPe(1:3); 0];
- SEm = SEm(1:3);
-#+end_src
-
-Rotation Error.
-#+begin_src matlab
- SEr = STw * [WPe(4:6); 0];
- SEr = SEr(1:3);
-#+end_src
-
-#+begin_src matlab
- Etot = [SEm ; SEr]
-#+end_src
-** Another try => seems better
-Let's denote:
-- $\{W\}$ the initial fixed frame
-- $\{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:
-- ${}^WT_R$
-- ${}^WT_M$
-
-We have:
-\begin{align}
- {}^MT_R &= {}^MT_W {}^WT_R \\
- &= {}^W{T_M}^{-1} {}^WT_R
-\end{align}
-
-#+begin_src matlab
- MTr = STw'*Ttot;
-#+end_src
-
-Position error:
-#+begin_src matlab
- MTr(1:3, 1:4)*[0; 0; 0; 1]
-#+end_src
-
-Orientation error:
-#+begin_src matlab
- MTr(1:3, 1:3)
-#+end_src
-
-** Verification
-How can we verify that the computation is correct?
-Options:
-- Test with simscape multi-body
- - Impose motion on each stage
- - Measure the position error w.r.t. the NASS
- - Compare with the computation
diff --git a/positioning_error/matlab/sim_nano_station_metrology.slx b/positioning_error/matlab/sim_nano_station_metrology.slx
index 3b0ca33..9ae293a 100644
Binary files a/positioning_error/matlab/sim_nano_station_metrology.slx and b/positioning_error/matlab/sim_nano_station_metrology.slx differ
diff --git a/simscape_subsystems/metrology_6dof_rotation_matrix.slx b/simscape_subsystems/metrology_6dof_rotation_matrix.slx
index 387bcdf..b10bc8d 100644
Binary files a/simscape_subsystems/metrology_6dof_rotation_matrix.slx and b/simscape_subsystems/metrology_6dof_rotation_matrix.slx differ
diff --git a/simscape_subsystems/reference_mirror.slx b/simscape_subsystems/reference_mirror.slx
index 4596951..87d8894 100644
Binary files a/simscape_subsystems/reference_mirror.slx and b/simscape_subsystems/reference_mirror.slx differ
