Add unused matlab tests in one file
This commit is contained in:
parent
75b39626e3
commit
8d8ebde512
@ -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.2659e-10 | 6.5603e-11 | 6.2183e-10 |
|
||||||
| 1.0354e-10 | -5.2439e-11 | -5.2425e-10 |
|
| 1.0354e-10 | -5.2439e-11 | -5.2425e-10 |
|
||||||
| -5.9816e-10 | 5.532e-10 | -1.7737e-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
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user