diff --git a/kinematics/index.org b/kinematics/index.org index cedb38a..c1b0467 100644 --- a/kinematics/index.org +++ b/kinematics/index.org @@ -203,3 +203,245 @@ And we verify that we indeed succeed to go to the wanted position. | -1.2659e-10 | 6.5603e-11 | 6.2183e-10 | | 1.0354e-10 | -5.2439e-11 | -5.2425e-10 | | -5.9816e-10 | 5.532e-10 | -1.7737e-10 | +* TODO Tests on the transformation from reference to wanted position :noexport: +:PROPERTIES: +:header-args:matlab+: :eval no +:END: +** 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