From 2a564881c5ff96ae0c0552f685a134f6ce049048 Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Thu, 5 Dec 2019 12:49:14 +0100 Subject: [PATCH] Add file to test the metrology --- metrology/index.org | 317 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 317 insertions(+) create mode 100644 metrology/index.org diff --git a/metrology/index.org b/metrology/index.org new file mode 100644 index 0000000..e23affe --- /dev/null +++ b/metrology/index.org @@ -0,0 +1,317 @@ +#+TITLE: Metrology +:DRAWER: +#+STARTUP: overview + +#+LANGUAGE: en +#+EMAIL: dehaeze.thomas@gmail.com +#+AUTHOR: Dehaeze Thomas + +#+HTML_LINK_HOME: ../index.html +#+HTML_LINK_UP: ../index.html + +# #+HTML_HEAD: +# #+HTML_HEAD: +# #+HTML_HEAD: +# #+HTML_HEAD: +# #+HTML_HEAD: +# #+HTML_HEAD: +# #+HTML_HEAD: + +#+HTML_MATHJAX: align: center tagside: right font: TeX + +#+PROPERTY: header-args:matlab :session *MATLAB* +#+PROPERTY: header-args:matlab+ :comments org +#+PROPERTY: header-args:matlab+ :results none +#+PROPERTY: header-args:matlab+ :exports both +#+PROPERTY: header-args:matlab+ :eval no-export +#+PROPERTY: header-args:matlab+ :output-dir figs +#+PROPERTY: header-args:matlab+ :tangle matlab/modal_frf_coh.m +#+PROPERTY: header-args:matlab+ :mkdirp yes + +#+PROPERTY: header-args:shell :eval no-export + +#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}") +#+PROPERTY: header-args:latex+ :imagemagick t :fit yes +#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150 +#+PROPERTY: header-args:latex+ :imoutoptions -quality 100 +#+PROPERTY: header-args:latex+ :results raw replace :buffer no +#+PROPERTY: header-args:latex+ :eval no-export +#+PROPERTY: header-args:latex+ :exports both +#+PROPERTY: header-args:latex+ :mkdirp yes +#+PROPERTY: header-args:latex+ :output-dir figs +:END: + +* 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 'simscape/sim_nano_station_tomo.slx' +#+end_src + +* Test with Simulink +#+begin_src matlab + load('simscape/conf_simscape.mat'); +#+end_src + +#+begin_src matlab + initializeReferences(struct('Dy_type', 'triangular', 'Dy_amplitude', 10e-3, 'Dy_period', 1)); + % initializeReferences(); +#+end_src + +* TODO Tests on the transformation from reference to wanted position +- [X] Are the rotation matrix commutable? => no +- [X] How to express the measured rotation errors? => screw axis coordinate seems nice (used in Taghirad's book) +- [ ] Should ask Veijo how he specifies the position of the Symetrie Hexapod +- [ ] Create functions for all distinct part and then include that in Simulink +- [ ] How the express the orientation error? +- [ ] If we use screw coordinate, can we add/subtract them? +- [ ] Do some simple tests to verify that the algorithm is working fine + +** 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 + +** 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 +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 \\ + &= {}^WT_M^t {}^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