347 lines
11 KiB
Org Mode
347 lines
11 KiB
Org Mode
#+TITLE: Tomography Experiment
|
|
: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: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
|
# #+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
|
# #+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
|
# #+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
|
# #+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
|
# #+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
|
# #+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
|
|
|
#+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)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
simulinkproject('../');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
open 'simscape/sim_nano_station_tomo.slx'
|
|
#+end_src
|
|
|
|
* Initialize Experiment
|
|
We first initialize all the stages.
|
|
#+begin_src matlab
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
initializeAxisc();
|
|
initializeMirror();
|
|
initializeNanoHexapod(struct('actuator', 'piezo'));
|
|
initializeSample(struct('mass', 1));
|
|
#+end_src
|
|
|
|
We initialize the reference path for all the stages.
|
|
#+begin_src matlab
|
|
initializeReferences(struct('Rz_type', 'rotating', 'Rz_period', 1));
|
|
#+end_src
|
|
|
|
And we initialize the disturbances.
|
|
#+begin_src matlab
|
|
initDisturbances();
|
|
#+end_src
|
|
|
|
* Run the Tomography Experiment
|
|
We first load the simulation configuration
|
|
#+begin_src matlab
|
|
load('mat/conf_simscape.mat');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
set_param(conf_simscape, 'StopTime', '1');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
set_param('sim_nano_station_tomo', 'SimulationCommand', 'start');
|
|
#+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
|