Update the metrology study.
This commit is contained in:
parent
2a564881c5
commit
7032f05ef5
@ -9,13 +9,13 @@
|
||||
#+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_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
|
||||
|
||||
@ -41,7 +41,23 @@
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Matlab Init :noexport:ignore:
|
||||
* Introduction :ignore:
|
||||
In this document, we suppose that we are able to measure perfectly the position of the sample with respect to the granite.
|
||||
Also, all the stages can be perfectly positioned.
|
||||
|
||||
In section [[sec:compute_reference]], we verify that the function developed to compute the wanted pose (translation and orientation) of the sample with respect to the granite can be determined from the wanted position of each stage (translation stage, tilt stage, spindle and micro-hexapod).
|
||||
To do so, we impose a perfect displacement and all the stage, we perfectly measure the position of the sample with respect to the granite, and we verify that this measured position corresponds to the computed wanted pose of the sample.
|
||||
|
||||
Then, in section [[sec:compute_pos_error]], we introduce some positioning error in the position stages.
|
||||
The positioning error of the sample expressed with respect to the granite frame (the one measured) is expressed in a frame connected to the NASS top platform.
|
||||
Finally, we move the NASS such that it compensate for the positioning error that are expressed in the frame of the NASS, and we verify that the positioning error of the sample is well compensated.
|
||||
|
||||
* Verify that the function to compute the reference pose is correct
|
||||
<<sec:compute_reference>>
|
||||
** Introduction :ignore:
|
||||
The goal here is to perfectly move the station and verify that there is no mismatch between the metrology measurement and the computation of the reference pose.
|
||||
|
||||
** 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
|
||||
@ -55,28 +71,395 @@
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
% open 'simscape/sim_nano_station_tomo.slx'
|
||||
open 'simscape/sim_nano_station_metrology.slx'
|
||||
#+end_src
|
||||
|
||||
* Test with Simulink
|
||||
** Prepare the Simulation
|
||||
We load the configuration.
|
||||
#+begin_src matlab
|
||||
load('simscape/conf_simscape.mat');
|
||||
#+end_src
|
||||
|
||||
We set a small =StopTime=.
|
||||
#+begin_src matlab
|
||||
initializeReferences(struct('Dy_type', 'triangular', 'Dy_amplitude', 10e-3, 'Dy_period', 1));
|
||||
% initializeReferences();
|
||||
set_param(conf_simscape, 'StopTime', '0.5');
|
||||
#+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
|
||||
We setup the reference path to be constant.
|
||||
#+begin_src matlab
|
||||
opts = struct( ...
|
||||
'Ts', 1e-3, ... % Sampling Frequency [s]
|
||||
'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
|
||||
'Dy_amplitude', 5e-3, ... % Amplitude of the displacement [m]
|
||||
'Dy_period', 1, ... % Period of the displacement [s]
|
||||
'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
|
||||
'Ry_amplitude', -1, ... % Amplitude [deg]
|
||||
'Ry_period', 10, ... % Period of the displacement [s]
|
||||
'Rz_type', 'constant', ... % Either "constant" / "rotating"
|
||||
'Rz_amplitude', -135, ... % Initial angle [deg]
|
||||
'Rz_period', 1, ... % Period of the rotating [s]
|
||||
'Dh_type', 'constant', ... % For now, only constant is implemented
|
||||
'Dh_pos', [0; 0; 0; -3; 1; 3], ... % Initial position [m,m,m,deg,deg,deg] of the top platform
|
||||
'Rm_type', 'constant', ... % For now, only constant is implemented
|
||||
'Rm_pos', [0, pi]', ... % Initial position of the two masses
|
||||
'Dn_type', 'constant', ... % For now, only constant is implemented
|
||||
'Dn_pos', [0; 0; 0; 0; 0; 0] ... % Initial position [m,m,m,deg,deg,deg] of the top platform
|
||||
);
|
||||
|
||||
initializeReferences(opts);
|
||||
#+end_src
|
||||
|
||||
No position error for now (perfect positioning).
|
||||
#+begin_src matlab
|
||||
Dye = 0; % [m]
|
||||
Rye = 0; % [rad]
|
||||
Rze = 0; % [rad]
|
||||
Dhe = zeros(6,1); % [m,rad]
|
||||
#+end_src
|
||||
|
||||
And we run the simulation.
|
||||
#+begin_src matlab
|
||||
sim('simscape/sim_nano_station_metrology.slx');
|
||||
#+end_src
|
||||
|
||||
** Verify that the pose of the sample is the same as the computed one
|
||||
Let's denote:
|
||||
- $\{W\}$ the initial fixed frame (base in which the interferometric measurement is done)
|
||||
- $\{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:
|
||||
- ${}^W\boldsymbol{T}_R$ which corresponds to the wanted pose of the sample with respect to the granite
|
||||
- ${}^W\boldsymbol{T}_M$ which corresponds to the measured pose of the sample with respect to the granite
|
||||
|
||||
We load the reference and we compute the desired trajectory of the sample in the form of an homogeneous transformation matrix ${}^WT_R$.
|
||||
#+begin_src matlab
|
||||
n = length(Dref.Dy.Time);
|
||||
|
||||
WTr = zeros(4, 4, n);
|
||||
for i = 1:n
|
||||
WTr(:, :, i) = computeReferencePose(Dref.Dy.Data(i), Dref.Ry.Data(i), Dref.Rz.Data(i), Dref.Dh.Data(i,:));
|
||||
end
|
||||
#+end_src
|
||||
|
||||
As the displacement is perfect, we also measure in simulation the pose of the sample with respect to the granite.
|
||||
From that we can compute the homogeneous transformation matrix ${}^WT_M$.
|
||||
#+begin_src matlab
|
||||
n = length(Dsm.R.Time);
|
||||
WTm = zeros(4, 4, n);
|
||||
WTm(1:3, 1:3, :) = Dsm.R.Data;
|
||||
WTm(1:3, 4, :) = [Dsm.x.Data' ; Dsm.y.Data' ; Dsm.z.Data'];
|
||||
WTm(4, 4, :) = 1;
|
||||
#+end_src
|
||||
|
||||
As the simulation is perfect (no measurement error and no motion error), we should have that
|
||||
\[ {}^W\boldsymbol{T}_R = {}^W\boldsymbol{T}_M \]
|
||||
|
||||
Or are least:
|
||||
\[ {}^W\boldsymbol{T}_R(1:3, 4) = {}^W\boldsymbol{T}_M(1:3, 4) \]
|
||||
\[ {}^W\boldsymbol{R}_R^t \cdot {}^W\boldsymbol{R}_M = \boldsymbol{I}_3 \]
|
||||
|
||||
#+begin_src matlab :results output replace
|
||||
WTr(1:3, 4, end)-WTm(1:3, 4, end)
|
||||
WTr(1:3, 1:3, end)'*WTm(1:3, 1:3, end)-eye(3)
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
#+begin_example
|
||||
WTr(1:3, 4, end)-WTm(1:3, 4, end)
|
||||
ans =
|
||||
5.38287405101034e-15
|
||||
9.42822209193395e-15
|
||||
-7.25141518012618e-16
|
||||
WTr(1:3, 1:3, end)'*WTm(1:3, 1:3, end)-eye(3)
|
||||
ans =
|
||||
1.53210777398272e-14 -1.60173523749974e-14 -7.42461647718073e-16
|
||||
1.60683098771042e-14 1.53210777398272e-14 -2.33146835171283e-15
|
||||
-3.95516952522712e-16 -1.72084568816899e-15 9.2370555648813e-14
|
||||
#+end_example
|
||||
|
||||
** Conclusion
|
||||
#+begin_important
|
||||
We are able to compute the wanted position and orientation of the sample.
|
||||
Both the measurement and the theory gives the same result.
|
||||
#+end_important
|
||||
|
||||
* Verify that the function to convert the position error in the frame fixed to the nano-hexapod is working
|
||||
<<sec:compute_pos_error>>
|
||||
** Introduction :ignore:
|
||||
We now introduce some positioning error in the stage.
|
||||
This will induce a global positioning error of the sample with respect to the desired pose that we can compute.
|
||||
|
||||
We want to verify that we are able to measure this positioning error and convert it in the frame attached to the Nano-hexapod.
|
||||
|
||||
** 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_metrology.slx'
|
||||
#+end_src
|
||||
|
||||
** Prepare the Simulation
|
||||
We load the configuration.
|
||||
#+begin_src matlab
|
||||
load('simscape/conf_simscape.mat');
|
||||
#+end_src
|
||||
|
||||
We set a small =StopTime=.
|
||||
#+begin_src matlab
|
||||
set_param(conf_simscape, 'StopTime', '0.5');
|
||||
#+end_src
|
||||
|
||||
We setup the reference path to be constant.
|
||||
#+begin_src matlab
|
||||
opts = struct( ...
|
||||
'Ts', 1e-3, ... % Sampling Frequency [s]
|
||||
'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
|
||||
'Dy_amplitude', 0, ... % Amplitude of the displacement [m]
|
||||
'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
|
||||
'Ry_amplitude', 0, ... % Amplitude [deg]
|
||||
'Rz_type', 'constant', ... % Either "constant" / "rotating"
|
||||
'Rz_amplitude', 180, ... % Initial angle [deg]
|
||||
'Dh_type', 'constant', ... % For now, only constant is implemented
|
||||
'Dh_pos', [0; 0; 0; 0; 0; 0], ... % Initial position [m,m,m,deg,deg,deg] of the top platform
|
||||
'Rm_type', 'constant', ... % For now, only constant is implemented
|
||||
'Rm_pos', [0, pi]', ... % Initial position of the two masses
|
||||
'Dn_type', 'constant', ... % For now, only constant is implemented
|
||||
'Dn_pos', [0; 0; 0; 0; 0; 0] ... % Initial position [m,m,m,deg,deg,deg] of the top platform
|
||||
);
|
||||
|
||||
initializeReferences(opts);
|
||||
#+end_src
|
||||
|
||||
Now we introduce some positioning error.
|
||||
#+begin_src matlab
|
||||
Dye = 0; % [m]
|
||||
Rye = 0; % [rad]
|
||||
Rze = 0; % [rad]
|
||||
Dhe = [1e-3 ; 3e-3 ; 2e-3 ; 1e-3 ; 2e-3 ; 3e-3]; % [m,rad]
|
||||
#+end_src
|
||||
|
||||
And we run the simulation.
|
||||
#+begin_src matlab
|
||||
sim('simscape/sim_nano_station_metrology.slx');
|
||||
#+end_src
|
||||
|
||||
** Compute the wanted pose of the sample in the NASS Base from the metrology and the reference
|
||||
Now that we have introduced some positioning error, the computed wanted pose and the measured pose will not be the same.
|
||||
|
||||
We would like to compute ${}^M\boldsymbol{T}_R$ which corresponds to the wanted pose of the sample expressed in a frame attached to the top platform of the nano-hexapod (frame $\{M\}$).
|
||||
|
||||
We have:
|
||||
\begin{align}
|
||||
{}^M\boldsymbol{T}_R &= {}^M\boldsymbol{T}_W \cdot {}^W\boldsymbol{T}_R \\
|
||||
&= {}^W{\boldsymbol{T}_M}^{-1} \cdot {}^W\boldsymbol{T}_R
|
||||
\end{align}
|
||||
|
||||
The top platform of the nano-hexapod is considered to be rigidly connected to the sample, thus, ${}^M\boldsymbol{T}_R$ corresponds to the pose error of the sample with respect to the nano-hexapod platform.
|
||||
|
||||
We load the reference and we compute the desired trajectory of the sample in the form of an homogeneous transformation matrix ${}^W\boldsymbol{T}_R$.
|
||||
#+begin_src matlab
|
||||
n = length(Dref.Dy.Time);
|
||||
|
||||
WTr = zeros(4, 4, n);
|
||||
for i = 1:n
|
||||
WTr(:, :, i) = computeReferencePose(Dref.Dy.Data(i), Dref.Ry.Data(i), Dref.Rz.Data(i), Dref.Dh.Data(i,:));
|
||||
end
|
||||
#+end_src
|
||||
|
||||
We also measure in simulation the pose of the sample with respect to the granite.
|
||||
From that we can compute the homogeneous transformation matrix ${}^W\boldsymbol{T}_M$.
|
||||
#+begin_src matlab
|
||||
n = length(Dsm.R.Time);
|
||||
WTm = zeros(4, 4, n);
|
||||
WTm(1:3, 1:3, :) = Dsm.R.Data;
|
||||
WTm(1:3, 4, :) = [Dsm.x.Data' ; Dsm.y.Data' ; Dsm.z.Data'];
|
||||
WTm(4, 4, :) = 1;
|
||||
#+end_src
|
||||
|
||||
The *inverse of the transformation matrix* can be obtain by (it is less computation intensive than doing a full inverse)
|
||||
\begin{equation}
|
||||
{}^B\boldsymbol{T}_A = {}^A\boldsymbol{T}_B^{-1} =
|
||||
\left[ \begin{array}{ccc|c}
|
||||
& & & \\
|
||||
& {}^A\boldsymbol{R}_B^T & & -{}^A \boldsymbol{R}_B^T {}^A\boldsymbol{P}_{O_B} \\
|
||||
& & & \\
|
||||
\hline
|
||||
0 & 0 & 0 & 1 \\
|
||||
\end{array} \right]
|
||||
\end{equation}
|
||||
Finally, we compute ${}^M\boldsymbol{T}_R$.
|
||||
#+begin_src matlab
|
||||
MTr = zeros(4, 4, n);
|
||||
for i = 1:n
|
||||
MTr(:, :, i) = [WTm(1:3,1:3,i)', -WTm(1:3,1:3,i)'*WTm(1:3,4,i) ; 0 0 0 1]*WTr(:,:,i);
|
||||
end
|
||||
#+end_src
|
||||
|
||||
Verify that the pose error corresponds to the positioning error of the stages.
|
||||
#+begin_src matlab :exports none
|
||||
Edx = MTr(1, 4, end);
|
||||
Edy = MTr(2, 4, end);
|
||||
Edz = MTr(3, 4, end);
|
||||
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
|
||||
Ery = atan2(MTr(1, 3, end), sqrt(MTr(1, 1, end)^2 + MTr(1, 2, end)^2));
|
||||
Erx = atan2(-MTr(2, 3, end)/cos(Ery), MTr(3, 3, end)/cos(Ery));
|
||||
Erz = atan2(-MTr(1, 2, end)/cos(Ery), MTr(1, 1, end)/cos(Ery));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||||
data2orgtable([Edx, Edy, Edz, Erx, Ery, Erz], {'Error'}, {'Edx [m]', 'Edy [m]', 'Edz [m]', 'Erx [rad]', 'Ery [rad]', 'Erz [rad]'}, ' %.1e ');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
| | Edx [m] | Edy [m] | Edz [m] | Erx [rad] | Ery [rad] | Erz [rad] |
|
||||
|-------+----------+----------+----------+-----------+-----------+-----------|
|
||||
| Error | -1.0e-03 | -3.0e-03 | -2.0e-03 | -1.0e-03 | -2.0e-03 | -3.0e-03 |
|
||||
|
||||
** Verify that be imposing the error motion on the nano-hexapod, we indeed have zero error at the end
|
||||
We now impose a displacement of the nano hexapod corresponding to the measured position error.
|
||||
#+begin_src matlab
|
||||
opts.Dn_pos = [Edx, Edy, Edz, 180/pi*Erx, 180/pi*Ery, 180/pi*Erz]';
|
||||
initializeReferences(opts);
|
||||
#+end_src
|
||||
|
||||
And we run the simulation.
|
||||
#+begin_src matlab
|
||||
sim('simscape/sim_nano_station_metrology.slx');
|
||||
#+end_src
|
||||
|
||||
We load the reference and we compute the desired trajectory of the sample in the form of an homogeneous transformation matrix ${}^WT_R$.
|
||||
#+begin_src matlab
|
||||
n = length(Dref.Dy.Time);
|
||||
|
||||
WTr = zeros(4, 4, n);
|
||||
for i = 1:n
|
||||
WTr(:, :, i) = computeReferencePose(Dref.Dy.Data(i), Dref.Ry.Data(i), Dref.Rz.Data(i), Dref.Dh.Data(i,:));
|
||||
end
|
||||
#+end_src
|
||||
|
||||
As the displacement is perfect, we also measure in simulation the pose of the sample with respect to the granite.
|
||||
From that we can compute the homogeneous transformation matrix ${}^WT_M$.
|
||||
#+begin_src matlab
|
||||
n = length(Dsm.R.Time);
|
||||
WTm = zeros(4, 4, n);
|
||||
WTm(1:3, 1:3, :) = Dsm.R.Data;
|
||||
WTm(1:3, 4, :) = [Dsm.x.Data' ; Dsm.y.Data' ; Dsm.z.Data'];
|
||||
WTm(4, 4, :) = 1;
|
||||
#+end_src
|
||||
|
||||
Finally, we compute ${}^MT_R$.
|
||||
#+begin_src matlab
|
||||
MTr = zeros(4, 4, n);
|
||||
for i = 1:n
|
||||
MTr(:, :, i) = [WTm(1:3,1:3,i)', -WTm(1:3,1:3,i)'*WTm(1:3,4,i) ; 0 0 0 1]*WTr(:,:,i);
|
||||
end
|
||||
#+end_src
|
||||
|
||||
Verify that the pose error is small.
|
||||
#+begin_src matlab :exports none
|
||||
Edx = MTr(1, 4, end);
|
||||
Edy = MTr(2, 4, end);
|
||||
Edz = MTr(3, 4, end);
|
||||
Ery = atan2(MTr(1, 3, end), sqrt(MTr(1, 1, end)^2 + MTr(1, 2, end)^2));
|
||||
Erx = atan2(-MTr(2, 3, end)/cos(Ery), MTr(3, 3, end)/cos(Ery));
|
||||
Erz = atan2(-MTr(1, 2, end)/cos(Ery), MTr(1, 1, end)/cos(Ery));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||||
data2orgtable([Edx, Edy, Edz, Erx, Ery, Erz], {'Error'}, {'Edx [m]', 'Edy [m]', 'Edz [m]', 'Erx [rad]', 'Ery [rad]', 'Erz [rad]'}, ' %.1e ');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
| | Edx [m] | Edy [m] | Edz [m] | Erx [rad] | Ery [rad] | Erz [rad] |
|
||||
|-------+---------+---------+---------+-----------+-----------+-----------|
|
||||
| Error | 1.2e-16 | 3.3e-16 | 2.3e-16 | -6.2e-17 | 1.1e-16 | 2.2e-16 |
|
||||
|
||||
** Conclusion
|
||||
#+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
|
||||
|
||||
* Functions
|
||||
** computeReferencePose
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/computeReferencePose.m
|
||||
:header-args:matlab+: :comments org :mkdirp yes
|
||||
:header-args:matlab+: :eval no :results none
|
||||
:END:
|
||||
<<sec:computeReferencePose>>
|
||||
|
||||
This Matlab function is accessible [[file:src/computeReferencePose.m][here]].
|
||||
|
||||
#+begin_src matlab
|
||||
function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh)
|
||||
% computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
|
||||
%
|
||||
% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh)
|
||||
%
|
||||
% Inputs:
|
||||
% - Dy, Ry, Rz, Dh -
|
||||
%
|
||||
% Outputs:
|
||||
% - WTr -
|
||||
|
||||
%% Translation Stage
|
||||
Rty = [1 0 0 0;
|
||||
0 1 0 Dy;
|
||||
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
|
||||
Rhx = [1 0 0;
|
||||
0 cos(Dh(4)) -sin(Dh(4));
|
||||
0 sin(Dh(4)) cos(Dh(4))];
|
||||
|
||||
Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
|
||||
0 1 0;
|
||||
-sin(Dh(5)) 0 cos(Dh(5))];
|
||||
|
||||
Rhz = [cos(Dh(6)) -sin(Dh(6)) 0;
|
||||
sin(Dh(6)) cos(Dh(6)) 0;
|
||||
0 0 1];
|
||||
|
||||
Rh = [1 0 0 Dh(1) ;
|
||||
0 1 0 Dh(2) ;
|
||||
0 0 1 Dh(3) ;
|
||||
0 0 0 1 ];
|
||||
|
||||
Rh(1:3, 1:3) = Rhx*Rhy*Rhz;
|
||||
|
||||
%% Total Homogeneous transformation
|
||||
WTr = Rty*Rry*Rrz*Rh;
|
||||
end
|
||||
#+end_src
|
||||
|
||||
|
||||
|
||||
* Tests on the transformation from reference to wanted position :noexport:
|
||||
** Introduction :ignore:
|
||||
#+begin_quote
|
||||
Rx = [1 0 0;
|
||||
@ -101,8 +484,25 @@ Let's define the following frames:
|
||||
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
|
||||
- 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\boldsymbol{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\boldsymbol{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)
|
||||
<<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_metrology.slx'
|
||||
#+end_src
|
||||
|
||||
** Wanted Position of the Sample with respect to the Granite
|
||||
Let's define the wanted position of each stage.
|
||||
@ -205,7 +605,7 @@ Let's compute the corresponding orientation using screw axis.
|
||||
|
||||
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$
|
||||
- ${}^W\boldsymbol{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]
|
||||
@ -225,7 +625,7 @@ We then obtain the orientation measurement in the form of screw coordinate $\the
|
||||
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 \]
|
||||
\[ \theta_\epsilon {}^W\boldsymbol{s}_\epsilon = \theta_r {}^W\boldsymbol{s}_r - \theta_m {}^W\boldsymbol{s}_m \]
|
||||
|
||||
#+begin_src matlab
|
||||
WPe = WPr - WPm;
|
||||
@ -278,7 +678,7 @@ Rotation Error.
|
||||
#+begin_src matlab
|
||||
Etot = [SEm ; SEr]
|
||||
#+end_src
|
||||
** Another try
|
||||
** Another try => seems better
|
||||
Let's denote:
|
||||
- $\{W\}$ the initial fixed frame
|
||||
- $\{R\}$ the reference frame corresponding to the wanted pose of the sample
|
||||
@ -291,7 +691,7 @@ We have then computed:
|
||||
We have:
|
||||
\begin{align}
|
||||
{}^MT_R &= {}^MT_W {}^WT_R \\
|
||||
&= {}^WT_M^t {}^WT_R
|
||||
&= {}^W{T_M}^{-1} {}^WT_R
|
||||
\end{align}
|
||||
|
||||
#+begin_src matlab
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
simscape_subsystems/metrology_6dof_homog_transform.slx
Normal file
BIN
simscape_subsystems/metrology_6dof_homog_transform.slx
Normal file
Binary file not shown.
BIN
simscape_subsystems/metrology_6dof_rotation_matrix.slx
Normal file
BIN
simscape_subsystems/metrology_6dof_rotation_matrix.slx
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
64
src/computeReferencePose.m
Normal file
64
src/computeReferencePose.m
Normal file
@ -0,0 +1,64 @@
|
||||
% computeReferencePose
|
||||
% :PROPERTIES:
|
||||
% :header-args:matlab+: :tangle ../src/computeReferencePose.m
|
||||
% :header-args:matlab+: :comments org :mkdirp yes
|
||||
% :header-args:matlab+: :eval no :results none
|
||||
% :END:
|
||||
% <<sec:computeReferencePose>>
|
||||
|
||||
% This Matlab function is accessible [[file:src/computeReferencePose.m][here]].
|
||||
|
||||
|
||||
function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh)
|
||||
% computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
|
||||
%
|
||||
% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh)
|
||||
%
|
||||
% Inputs:
|
||||
% - Dy, Ry, Rz, Dh -
|
||||
%
|
||||
% Outputs:
|
||||
% - WTr -
|
||||
|
||||
%% Translation Stage
|
||||
Rty = [1 0 0 0;
|
||||
0 1 0 Dy;
|
||||
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
|
||||
Rhx = [1 0 0;
|
||||
0 cos(Dh(4)) -sin(Dh(4));
|
||||
0 sin(Dh(4)) cos(Dh(4))];
|
||||
|
||||
Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
|
||||
0 1 0;
|
||||
-sin(Dh(5)) 0 cos(Dh(5))];
|
||||
|
||||
Rhz = [cos(Dh(6)) -sin(Dh(6)) 0;
|
||||
sin(Dh(6)) cos(Dh(6)) 0;
|
||||
0 0 1];
|
||||
|
||||
Rh = [1 0 0 Dh(1) ;
|
||||
0 1 0 Dh(2) ;
|
||||
0 0 1 Dh(3) ;
|
||||
0 0 0 1 ];
|
||||
|
||||
Rh(1:3, 1:3) = Rhx*Rhy*Rhz;
|
||||
|
||||
%% Total Homogeneous transformation
|
||||
WTr = Rty*Rry*Rrz*Rh;
|
||||
end
|
||||
72
src/identifyPlantFRF.m
Normal file
72
src/identifyPlantFRF.m
Normal file
@ -0,0 +1,72 @@
|
||||
% identifyPlantFRF
|
||||
% :PROPERTIES:
|
||||
% :header-args:matlab+: :tangle ../src/identifyPlantFRF.m
|
||||
% :header-args:matlab+: :comments org :mkdirp yes
|
||||
% :header-args:matlab+: :eval no :results none
|
||||
% :END:
|
||||
% <<sec:identifyPlant>>
|
||||
|
||||
% This Matlab function is accessible [[file:../src/identifyPlant.m][here]].
|
||||
|
||||
|
||||
function [sys] = identifyPlantFRF(opts_param)
|
||||
%% Default values for opts
|
||||
opts = struct();
|
||||
|
||||
%% Populate opts with input parameters
|
||||
if exist('opts_param','var')
|
||||
for opt = fieldnames(opts_param)'
|
||||
opts.(opt{1}) = opts_param.(opt{1});
|
||||
end
|
||||
end
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'sim_nano_station_id';
|
||||
|
||||
%% Input/Output definition
|
||||
io(1) = linio([mdl, '/Fn'], 1, 'input'); % Cartesian forces applied by NASS
|
||||
io(2) = linio([mdl, '/Dw'], 1, 'input'); % Ground Motion
|
||||
io(3) = linio([mdl, '/Fs'], 1, 'input'); % External forces on the sample
|
||||
io(4) = linio([mdl, '/Fnl'], 1, 'input'); % Forces applied on the NASS's legs
|
||||
io(5) = linio([mdl, '/Fd'], 1, 'input'); % Disturbance Forces
|
||||
io(6) = linio([mdl, '/Dsm'], 1, 'output'); % Displacement of the sample
|
||||
io(7) = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs
|
||||
io(8) = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs
|
||||
io(9) = linio([mdl, '/Es'], 1, 'output'); % Position Error w.r.t. NASS base
|
||||
io(10) = linio([mdl, '/Vlm'], 1, 'output'); % Measured absolute velocity of the legs
|
||||
|
||||
input = frest.Random('Ts',1e-4);
|
||||
|
||||
%% Run the linearization
|
||||
G = frestimate(mdl, io, input);
|
||||
% G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ...
|
||||
% 'Dgx', 'Dgy', 'Dgz', ...
|
||||
% 'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz', ...
|
||||
% 'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ...
|
||||
% 'Frzz', 'Ftyx', 'Ftyz'};
|
||||
% G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ...
|
||||
% 'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ...
|
||||
% 'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ...
|
||||
% 'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz', ...
|
||||
% 'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'};
|
||||
|
||||
% %% Create the sub transfer functions
|
||||
% minreal_tol = sqrt(eps);
|
||||
% % From forces applied in the cartesian frame to displacement of the sample in the cartesian frame
|
||||
% sys.G_cart = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
|
||||
% % From ground motion to Sample displacement
|
||||
% sys.G_gm = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}), minreal_tol, false);
|
||||
% % From direct forces applied on the sample to displacement of the sample
|
||||
% sys.G_fs = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz'}), minreal_tol, false);
|
||||
% % From forces applied on NASS's legs to force sensor in each leg
|
||||
% sys.G_iff = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
|
||||
% % From forces applied on NASS's legs to displacement of each leg
|
||||
% sys.G_dleg = minreal(G({'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
|
||||
% % From forces/torques applied by the NASS to position error
|
||||
% sys.G_plant = minreal(G({'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
|
||||
% % From forces/torques applied by the NASS to velocity of the actuator
|
||||
% sys.G_geoph = minreal(G({'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
|
||||
% % From various disturbance forces to position error
|
||||
% sys.G_dist = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Frzz', 'Ftyx', 'Ftyz'}), minreal_tol, false);
|
||||
|
||||
end
|
||||
135
src/initDisturbances.m
Normal file
135
src/initDisturbances.m
Normal file
@ -0,0 +1,135 @@
|
||||
% Function that initialize the disturbances
|
||||
% :PROPERTIES:
|
||||
% :header-args:matlab+: :tangle ../src/initDisturbances.m
|
||||
% :header-args:matlab+: :comments org :mkdirp yes
|
||||
% :header-args:matlab+: :eval no :results none
|
||||
% :END:
|
||||
% <<sec:initDisturbances>>
|
||||
|
||||
% This Matlab function is accessible [[file:src/initDisturbances.m][here]].
|
||||
|
||||
|
||||
function [] = initDisturbances(opts_param)
|
||||
% initDisturbances - Initialize the disturbances
|
||||
%
|
||||
% Syntax: [] = initDisturbances(opts_param)
|
||||
%
|
||||
% Inputs:
|
||||
% - opts_param -
|
||||
|
||||
% Default values for the Options
|
||||
|
||||
%% Default values for opts
|
||||
opts = struct();
|
||||
|
||||
%% Populate opts with input parameters
|
||||
if exist('opts_param','var')
|
||||
for opt = fieldnames(opts_param)'
|
||||
opts.(opt{1}) = opts_param.(opt{1});
|
||||
end
|
||||
end
|
||||
|
||||
% Load Data
|
||||
|
||||
load('./disturbances/mat/dist_psd.mat', 'dist_f');
|
||||
|
||||
|
||||
|
||||
% We remove the first frequency point that usually is very large.
|
||||
|
||||
dist_f.f = dist_f.f(2:end);
|
||||
dist_f.psd_gm = dist_f.psd_gm(2:end);
|
||||
dist_f.psd_ty = dist_f.psd_ty(2:end);
|
||||
dist_f.psd_rz = dist_f.psd_rz(2:end);
|
||||
|
||||
% Parameters
|
||||
% We define some parameters that will be used in the algorithm.
|
||||
|
||||
Fs = 2*dist_f.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
|
||||
N = 2*length(dist_f.f); % Number of Samples match the one of the wanted PSD
|
||||
T0 = N/Fs; % Signal Duration [s]
|
||||
df = 1/T0; % Frequency resolution of the DFT [Hz]
|
||||
% Also equal to (dist_f.f(2)-dist_f.f(1))
|
||||
t = linspace(0, T0, N+1)'; % Time Vector [s]
|
||||
Ts = 1/Fs; % Sampling Time [s]
|
||||
|
||||
% Ground Motion
|
||||
|
||||
phi = dist_f.psd_gm;
|
||||
C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
C(i) = sqrt(phi(i)*df);
|
||||
end
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
|
||||
% Dwx = struct('time', t, 'signals', struct('values', u));
|
||||
Dwx = u;
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
|
||||
Dwy = u;
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
|
||||
Dwz = u;
|
||||
|
||||
% Translation Stage - X direction
|
||||
|
||||
phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
|
||||
C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
C(i) = sqrt(phi(i)*df);
|
||||
end
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
|
||||
Fty_x = u;
|
||||
|
||||
% Translation Stage - Z direction
|
||||
|
||||
phi = dist_f.psd_ty;
|
||||
C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
C(i) = sqrt(phi(i)*df);
|
||||
end
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
|
||||
Fty_z = u;
|
||||
|
||||
% Spindle - Z direction
|
||||
|
||||
phi = dist_f.psd_rz;
|
||||
C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
C(i) = sqrt(phi(i)*df);
|
||||
end
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
|
||||
Frz_z = u;
|
||||
|
||||
% Direct Forces
|
||||
|
||||
u = zeros(length(t), 6);
|
||||
Fd = u;
|
||||
|
||||
% Set initial value to zero
|
||||
|
||||
Dwx = Dwx - Dwx(1);
|
||||
Dwy = Dwy - Dwy(1);
|
||||
Dwz = Dwz - Dwz(1);
|
||||
Fty_x = Fty_x - Fty_x(1);
|
||||
Fty_z = Fty_z - Fty_z(1);
|
||||
Frz_z = Frz_z - Frz_z(1);
|
||||
|
||||
% Save
|
||||
|
||||
save('./mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't');
|
||||
128
src/initializeReferences.m
Normal file
128
src/initializeReferences.m
Normal file
@ -0,0 +1,128 @@
|
||||
% Generate Reference Signals
|
||||
% :PROPERTIES:
|
||||
% :header-args:matlab+: :tangle ../src/initializeReferences.m
|
||||
% :header-args:matlab+: :comments org :mkdirp yes
|
||||
% :header-args:matlab+: :eval no :results none
|
||||
% :END:
|
||||
% <<sec:initializeReferences>>
|
||||
|
||||
% This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
|
||||
|
||||
|
||||
function [ref] = initializeReferences(opts_param)
|
||||
%% Default values for opts
|
||||
opts = struct( ...
|
||||
'Ts', 1e-3, ... % Sampling Frequency [s]
|
||||
'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
|
||||
'Dy_amplitude', 0, ... % Amplitude of the displacement [m]
|
||||
'Dy_period', 1, ... % Period of the displacement [s]
|
||||
'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
|
||||
'Ry_amplitude', 0, ... % Amplitude [deg]
|
||||
'Ry_period', 10, ... % Period of the displacement [s]
|
||||
'Rz_type', 'constant', ... % Either "constant" / "rotating"
|
||||
'Rz_amplitude', 0, ... % Initial angle [deg]
|
||||
'Rz_period', 1, ... % Period of the rotating [s]
|
||||
'Dh_type', 'constant', ... % For now, only constant is implemented
|
||||
'Dh_pos', [0; 0; 0; 0; 0; 0], ... % Initial position [m,m,m,rad,rad,rad] of the top platform
|
||||
'Rm_type', 'constant', ... % For now, only constant is implemented
|
||||
'Rm_pos', [0; pi], ... % Initial position of the two masses
|
||||
'Dn_type', 'constant', ... % For now, only constant is implemented
|
||||
'Dn_pos', [0; 0; 0; 0; 0; 0] ... % Initial position [m,m,m,rad,rad,rad] of the top platform
|
||||
);
|
||||
|
||||
%% Populate opts with input parameters
|
||||
if exist('opts_param','var')
|
||||
for opt = fieldnames(opts_param)'
|
||||
opts.(opt{1}) = opts_param.(opt{1});
|
||||
end
|
||||
end
|
||||
|
||||
%% Set Sampling Time
|
||||
Ts = opts.Ts;
|
||||
|
||||
%% Translation stage - Dy
|
||||
t = 0:Ts:opts.Dy_period-Ts; % Time Vector [s]
|
||||
Dy = zeros(length(t), 1);
|
||||
switch opts.Dy_type
|
||||
case 'constant'
|
||||
Dy(:) = opts.Dy_amplitude;
|
||||
case 'triangular'
|
||||
Dy(:) = -4*opts.Dy_amplitude + 4*opts.Dy_amplitude/opts.Dy_period*t;
|
||||
Dy(t<0.75*opts.Dy_period) = 2*opts.Dy_amplitude - 4*opts.Dy_amplitude/opts.Dy_period*t(t<0.75*opts.Dy_period);
|
||||
Dy(t<0.25*opts.Dy_period) = 4*opts.Dy_amplitude/opts.Dy_period*t(t<0.25*opts.Dy_period);
|
||||
case 'sinusoidal'
|
||||
Dy(:) = opts.Dy_amplitude*sin(2*pi/opts.Dy_period*t);
|
||||
otherwise
|
||||
warning('Dy_type is not set correctly');
|
||||
end
|
||||
Dy = struct('time', t, 'signals', struct('values', Dy));
|
||||
|
||||
|
||||
%% Tilt Stage - Ry
|
||||
t = 0:Ts:opts.Ry_period-Ts;
|
||||
Ry = zeros(length(t), 1);
|
||||
|
||||
switch opts.Ry_type
|
||||
case 'constant'
|
||||
Ry(:) = pi/180*opts.Ry_amplitude;
|
||||
case 'triangular'
|
||||
Ry(:) = -4*(pi/180*opts.Ry_amplitude) + 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t;
|
||||
Ry(t<0.75*opts.Ry_period) = 2*(pi/180*opts.Ry_amplitude) - 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t(t<0.75*opts.Ry_period);
|
||||
Ry(t<0.25*opts.Ry_period) = 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t(t<0.25*opts.Ry_period);
|
||||
case 'sinusoidal'
|
||||
Ry(:) = opts.Ry_amplitude*sin(2*pi/opts.Ry_period*t);
|
||||
otherwise
|
||||
warning('Ry_type is not set correctly');
|
||||
end
|
||||
Ry = struct('time', t, 'signals', struct('values', Ry));
|
||||
|
||||
%% Spindle - Rz
|
||||
t = 0:Ts:opts.Rz_period-Ts;
|
||||
Rz = zeros(length(t), 1);
|
||||
|
||||
switch opts.Rz_type
|
||||
case 'constant'
|
||||
Rz(:) = pi/180*opts.Rz_amplitude;
|
||||
case 'rotating'
|
||||
Rz(:) = pi/180*opts.Rz_amplitude+2*pi/opts.Rz_period*t;
|
||||
otherwise
|
||||
warning('Rz_type is not set correctly');
|
||||
end
|
||||
Rz = struct('time', t, 'signals', struct('values', Rz));
|
||||
|
||||
%% Micro-Hexapod
|
||||
t = [0, Ts];
|
||||
Dh = zeros(length(t), 6);
|
||||
|
||||
opts.Dh_pos(4:6) = pi/180*opts.Dh_pos(4:6); % convert from [deg] to [rad]
|
||||
|
||||
switch opts.Dh_type
|
||||
case 'constant'
|
||||
Dh = [opts.Dh_pos, opts.Dh_pos];
|
||||
otherwise
|
||||
warning('Dh_type is not set correctly');
|
||||
end
|
||||
Dh = struct('time', t, 'signals', struct('values', Dh));
|
||||
|
||||
%% Axis Compensation - Rm
|
||||
t = [0, Ts];
|
||||
Rm = [opts.Rm_pos, opts.Rm_pos];
|
||||
Rm = struct('time', t, 'signals', struct('values', Rm));
|
||||
|
||||
%% Nano-Hexapod
|
||||
t = [0, Ts];
|
||||
Dn = zeros(length(t), 6);
|
||||
|
||||
opts.Dn_pos(4:6) = pi/180*opts.Dn_pos(4:6); % convert from [deg] to [rad]
|
||||
|
||||
switch opts.Dn_type
|
||||
case 'constant'
|
||||
Dn = [opts.Dn_pos, opts.Dn_pos];
|
||||
otherwise
|
||||
warning('Dn_type is not set correctly');
|
||||
end
|
||||
Dn = struct('time', t, 'signals', struct('values', Dn));
|
||||
|
||||
%% Save
|
||||
save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Rm', 'Dn', 'Ts');
|
||||
end
|
||||
651
tomo-exp/index.html
Normal file
651
tomo-exp/index.html
Normal file
@ -0,0 +1,651 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
|
||||
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
|
||||
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
|
||||
<head>
|
||||
<!-- 2019-12-04 mer. 21:55 -->
|
||||
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1" />
|
||||
<title>Tomography Experiment</title>
|
||||
<meta name="generator" content="Org mode" />
|
||||
<meta name="author" content="Dehaeze Thomas" />
|
||||
<style type="text/css">
|
||||
<!--/*--><![CDATA[/*><!--*/
|
||||
.title { text-align: center;
|
||||
margin-bottom: .2em; }
|
||||
.subtitle { text-align: center;
|
||||
font-size: medium;
|
||||
font-weight: bold;
|
||||
margin-top:0; }
|
||||
.todo { font-family: monospace; color: red; }
|
||||
.done { font-family: monospace; color: green; }
|
||||
.priority { font-family: monospace; color: orange; }
|
||||
.tag { background-color: #eee; font-family: monospace;
|
||||
padding: 2px; font-size: 80%; font-weight: normal; }
|
||||
.timestamp { color: #bebebe; }
|
||||
.timestamp-kwd { color: #5f9ea0; }
|
||||
.org-right { margin-left: auto; margin-right: 0px; text-align: right; }
|
||||
.org-left { margin-left: 0px; margin-right: auto; text-align: left; }
|
||||
.org-center { margin-left: auto; margin-right: auto; text-align: center; }
|
||||
.underline { text-decoration: underline; }
|
||||
#postamble p, #preamble p { font-size: 90%; margin: .2em; }
|
||||
p.verse { margin-left: 3%; }
|
||||
pre {
|
||||
border: 1px solid #ccc;
|
||||
box-shadow: 3px 3px 3px #eee;
|
||||
padding: 8pt;
|
||||
font-family: monospace;
|
||||
overflow: auto;
|
||||
margin: 1.2em;
|
||||
}
|
||||
pre.src {
|
||||
position: relative;
|
||||
overflow: visible;
|
||||
padding-top: 1.2em;
|
||||
}
|
||||
pre.src:before {
|
||||
display: none;
|
||||
position: absolute;
|
||||
background-color: white;
|
||||
top: -10px;
|
||||
right: 10px;
|
||||
padding: 3px;
|
||||
border: 1px solid black;
|
||||
}
|
||||
pre.src:hover:before { display: inline;}
|
||||
/* Languages per Org manual */
|
||||
pre.src-asymptote:before { content: 'Asymptote'; }
|
||||
pre.src-awk:before { content: 'Awk'; }
|
||||
pre.src-C:before { content: 'C'; }
|
||||
/* pre.src-C++ doesn't work in CSS */
|
||||
pre.src-clojure:before { content: 'Clojure'; }
|
||||
pre.src-css:before { content: 'CSS'; }
|
||||
pre.src-D:before { content: 'D'; }
|
||||
pre.src-ditaa:before { content: 'ditaa'; }
|
||||
pre.src-dot:before { content: 'Graphviz'; }
|
||||
pre.src-calc:before { content: 'Emacs Calc'; }
|
||||
pre.src-emacs-lisp:before { content: 'Emacs Lisp'; }
|
||||
pre.src-fortran:before { content: 'Fortran'; }
|
||||
pre.src-gnuplot:before { content: 'gnuplot'; }
|
||||
pre.src-haskell:before { content: 'Haskell'; }
|
||||
pre.src-hledger:before { content: 'hledger'; }
|
||||
pre.src-java:before { content: 'Java'; }
|
||||
pre.src-js:before { content: 'Javascript'; }
|
||||
pre.src-latex:before { content: 'LaTeX'; }
|
||||
pre.src-ledger:before { content: 'Ledger'; }
|
||||
pre.src-lisp:before { content: 'Lisp'; }
|
||||
pre.src-lilypond:before { content: 'Lilypond'; }
|
||||
pre.src-lua:before { content: 'Lua'; }
|
||||
pre.src-matlab:before { content: 'MATLAB'; }
|
||||
pre.src-mscgen:before { content: 'Mscgen'; }
|
||||
pre.src-ocaml:before { content: 'Objective Caml'; }
|
||||
pre.src-octave:before { content: 'Octave'; }
|
||||
pre.src-org:before { content: 'Org mode'; }
|
||||
pre.src-oz:before { content: 'OZ'; }
|
||||
pre.src-plantuml:before { content: 'Plantuml'; }
|
||||
pre.src-processing:before { content: 'Processing.js'; }
|
||||
pre.src-python:before { content: 'Python'; }
|
||||
pre.src-R:before { content: 'R'; }
|
||||
pre.src-ruby:before { content: 'Ruby'; }
|
||||
pre.src-sass:before { content: 'Sass'; }
|
||||
pre.src-scheme:before { content: 'Scheme'; }
|
||||
pre.src-screen:before { content: 'Gnu Screen'; }
|
||||
pre.src-sed:before { content: 'Sed'; }
|
||||
pre.src-sh:before { content: 'shell'; }
|
||||
pre.src-sql:before { content: 'SQL'; }
|
||||
pre.src-sqlite:before { content: 'SQLite'; }
|
||||
/* additional languages in org.el's org-babel-load-languages alist */
|
||||
pre.src-forth:before { content: 'Forth'; }
|
||||
pre.src-io:before { content: 'IO'; }
|
||||
pre.src-J:before { content: 'J'; }
|
||||
pre.src-makefile:before { content: 'Makefile'; }
|
||||
pre.src-maxima:before { content: 'Maxima'; }
|
||||
pre.src-perl:before { content: 'Perl'; }
|
||||
pre.src-picolisp:before { content: 'Pico Lisp'; }
|
||||
pre.src-scala:before { content: 'Scala'; }
|
||||
pre.src-shell:before { content: 'Shell Script'; }
|
||||
pre.src-ebnf2ps:before { content: 'ebfn2ps'; }
|
||||
/* additional language identifiers per "defun org-babel-execute"
|
||||
in ob-*.el */
|
||||
pre.src-cpp:before { content: 'C++'; }
|
||||
pre.src-abc:before { content: 'ABC'; }
|
||||
pre.src-coq:before { content: 'Coq'; }
|
||||
pre.src-groovy:before { content: 'Groovy'; }
|
||||
/* additional language identifiers from org-babel-shell-names in
|
||||
ob-shell.el: ob-shell is the only babel language using a lambda to put
|
||||
the execution function name together. */
|
||||
pre.src-bash:before { content: 'bash'; }
|
||||
pre.src-csh:before { content: 'csh'; }
|
||||
pre.src-ash:before { content: 'ash'; }
|
||||
pre.src-dash:before { content: 'dash'; }
|
||||
pre.src-ksh:before { content: 'ksh'; }
|
||||
pre.src-mksh:before { content: 'mksh'; }
|
||||
pre.src-posh:before { content: 'posh'; }
|
||||
/* Additional Emacs modes also supported by the LaTeX listings package */
|
||||
pre.src-ada:before { content: 'Ada'; }
|
||||
pre.src-asm:before { content: 'Assembler'; }
|
||||
pre.src-caml:before { content: 'Caml'; }
|
||||
pre.src-delphi:before { content: 'Delphi'; }
|
||||
pre.src-html:before { content: 'HTML'; }
|
||||
pre.src-idl:before { content: 'IDL'; }
|
||||
pre.src-mercury:before { content: 'Mercury'; }
|
||||
pre.src-metapost:before { content: 'MetaPost'; }
|
||||
pre.src-modula-2:before { content: 'Modula-2'; }
|
||||
pre.src-pascal:before { content: 'Pascal'; }
|
||||
pre.src-ps:before { content: 'PostScript'; }
|
||||
pre.src-prolog:before { content: 'Prolog'; }
|
||||
pre.src-simula:before { content: 'Simula'; }
|
||||
pre.src-tcl:before { content: 'tcl'; }
|
||||
pre.src-tex:before { content: 'TeX'; }
|
||||
pre.src-plain-tex:before { content: 'Plain TeX'; }
|
||||
pre.src-verilog:before { content: 'Verilog'; }
|
||||
pre.src-vhdl:before { content: 'VHDL'; }
|
||||
pre.src-xml:before { content: 'XML'; }
|
||||
pre.src-nxml:before { content: 'XML'; }
|
||||
/* add a generic configuration mode; LaTeX export needs an additional
|
||||
(add-to-list 'org-latex-listings-langs '(conf " ")) in .emacs */
|
||||
pre.src-conf:before { content: 'Configuration File'; }
|
||||
|
||||
table { border-collapse:collapse; }
|
||||
caption.t-above { caption-side: top; }
|
||||
caption.t-bottom { caption-side: bottom; }
|
||||
td, th { vertical-align:top; }
|
||||
th.org-right { text-align: center; }
|
||||
th.org-left { text-align: center; }
|
||||
th.org-center { text-align: center; }
|
||||
td.org-right { text-align: right; }
|
||||
td.org-left { text-align: left; }
|
||||
td.org-center { text-align: center; }
|
||||
dt { font-weight: bold; }
|
||||
.footpara { display: inline; }
|
||||
.footdef { margin-bottom: 1em; }
|
||||
.figure { padding: 1em; }
|
||||
.figure p { text-align: center; }
|
||||
.equation-container {
|
||||
display: table;
|
||||
text-align: center;
|
||||
width: 100%;
|
||||
}
|
||||
.equation {
|
||||
vertical-align: middle;
|
||||
}
|
||||
.equation-label {
|
||||
display: table-cell;
|
||||
text-align: right;
|
||||
vertical-align: middle;
|
||||
}
|
||||
.inlinetask {
|
||||
padding: 10px;
|
||||
border: 2px solid gray;
|
||||
margin: 10px;
|
||||
background: #ffffcc;
|
||||
}
|
||||
#org-div-home-and-up
|
||||
{ text-align: right; font-size: 70%; white-space: nowrap; }
|
||||
textarea { overflow-x: auto; }
|
||||
.linenr { font-size: smaller }
|
||||
.code-highlighted { background-color: #ffff00; }
|
||||
.org-info-js_info-navigation { border-style: none; }
|
||||
#org-info-js_console-label
|
||||
{ font-size: 10px; font-weight: bold; white-space: nowrap; }
|
||||
.org-info-js_search-highlight
|
||||
{ background-color: #ffff00; color: #000000; font-weight: bold; }
|
||||
.org-svg { width: 90%; }
|
||||
/*]]>*/-->
|
||||
</style>
|
||||
<script type="text/javascript">
|
||||
/*
|
||||
@licstart The following is the entire license notice for the
|
||||
JavaScript code in this tag.
|
||||
|
||||
Copyright (C) 2012-2019 Free Software Foundation, Inc.
|
||||
|
||||
The JavaScript code in this tag is free software: you can
|
||||
redistribute it and/or modify it under the terms of the GNU
|
||||
General Public License (GNU GPL) as published by the Free Software
|
||||
Foundation, either version 3 of the License, or (at your option)
|
||||
any later version. The code is distributed WITHOUT ANY WARRANTY;
|
||||
without even the implied warranty of MERCHANTABILITY or FITNESS
|
||||
FOR A PARTICULAR PURPOSE. See the GNU GPL for more details.
|
||||
|
||||
As additional permission under GNU GPL version 3 section 7, you
|
||||
may distribute non-source (e.g., minimized or compacted) forms of
|
||||
that code without the copy of the GNU GPL normally required by
|
||||
section 4, provided you include this license notice and a URL
|
||||
through which recipients can access the Corresponding Source.
|
||||
|
||||
|
||||
@licend The above is the entire license notice
|
||||
for the JavaScript code in this tag.
|
||||
*/
|
||||
<!--/*--><![CDATA[/*><!--*/
|
||||
function CodeHighlightOn(elem, id)
|
||||
{
|
||||
var target = document.getElementById(id);
|
||||
if(null != target) {
|
||||
elem.cacheClassElem = elem.className;
|
||||
elem.cacheClassTarget = target.className;
|
||||
target.className = "code-highlighted";
|
||||
elem.className = "code-highlighted";
|
||||
}
|
||||
}
|
||||
function CodeHighlightOff(elem, id)
|
||||
{
|
||||
var target = document.getElementById(id);
|
||||
if(elem.cacheClassElem)
|
||||
elem.className = elem.cacheClassElem;
|
||||
if(elem.cacheClassTarget)
|
||||
target.className = elem.cacheClassTarget;
|
||||
}
|
||||
/*]]>*///-->
|
||||
</script>
|
||||
<script type="text/x-mathjax-config">
|
||||
MathJax.Hub.Config({
|
||||
displayAlign: "center",
|
||||
displayIndent: "0em",
|
||||
|
||||
"HTML-CSS": { scale: 100,
|
||||
linebreaks: { automatic: "false" },
|
||||
webFont: "TeX"
|
||||
},
|
||||
SVG: {scale: 100,
|
||||
linebreaks: { automatic: "false" },
|
||||
font: "TeX"},
|
||||
NativeMML: {scale: 100},
|
||||
TeX: { equationNumbers: {autoNumber: "AMS"},
|
||||
MultLineWidth: "85%",
|
||||
TagSide: "right",
|
||||
TagIndent: ".8em"
|
||||
}
|
||||
});
|
||||
</script>
|
||||
<script type="text/javascript"
|
||||
src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.0/MathJax.js?config=TeX-AMS_HTML"></script>
|
||||
</head>
|
||||
<body>
|
||||
<div id="org-div-home-and-up">
|
||||
<a accesskey="h" href="../index.html"> UP </a>
|
||||
|
|
||||
<a accesskey="H" href="../index.html"> HOME </a>
|
||||
</div><div id="content">
|
||||
<h1 class="title">Tomography Experiment</h1>
|
||||
<div id="table-of-contents">
|
||||
<h2>Table of Contents</h2>
|
||||
<div id="text-table-of-contents">
|
||||
<ul>
|
||||
<li><a href="#orgab97c4b">1. Initialize Experiment</a></li>
|
||||
<li><a href="#org26c554f">2. Run the Tomography Experiment</a></li>
|
||||
<li><a href="#orgc507e1e">3. <span class="todo TODO">TODO</span> Tests on the transformation from reference to wanted position</a>
|
||||
<ul>
|
||||
<li><a href="#org4537a86">3.1. Wanted Position of the Sample with respect to the Granite</a></li>
|
||||
<li><a href="#org6c67ae5">3.2. Measured Position of the Sample with respect to the Granite</a></li>
|
||||
<li><a href="#org69e38aa">3.3. Positioning Error with respect to the Granite</a></li>
|
||||
<li><a href="#org5246d94">3.4. Position Error Expressed in the Nano-Hexapod Frame</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
</ul>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgab97c4b" class="outline-2">
|
||||
<h2 id="orgab97c4b"><span class="section-number-2">1</span> Initialize Experiment</h2>
|
||||
<div class="outline-text-2" id="text-1">
|
||||
<p>
|
||||
We first initialize all the stages.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>initializeGround<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeGranite<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeTy<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeRy<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeRz<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeMicroHexapod<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeAxisc<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeMirror<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeNanoHexapod<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'actuator'</span>, <span class="org-string">'piezo'</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<code>initializeSample<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'mass'</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
We initialize the reference path for all the stages.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>initializeReferences<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'Rz_type'</span>, <span class="org-string">'rotating'</span>, <span class="org-string">'Rz_period'</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
And we initialize the disturbances.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>initDisturbances<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org26c554f" class="outline-2">
|
||||
<h2 id="org26c554f"><span class="section-number-2">2</span> Run the Tomography Experiment</h2>
|
||||
<div class="outline-text-2" id="text-2">
|
||||
<p>
|
||||
We first load the simulation configuration
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'simscape/conf_simscape.mat'</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code><span class="org-matlab-simulink-keyword">set_param</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">conf_simscape</span>, <span class="org-string">'StopTime'</span>, '<span class="org-highlight-numbers-number">1</span><span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code><span class="org-matlab-simulink-keyword">set_param</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'sim_nano_station_tomo'</span>, <span class="org-string">'SimulationCommand'</span>, <span class="org-string">'start'</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgc507e1e" class="outline-2">
|
||||
<h2 id="orgc507e1e"><span class="section-number-2">3</span> <span class="todo TODO">TODO</span> Tests on the transformation from reference to wanted position</h2>
|
||||
<div class="outline-text-2" id="text-3">
|
||||
<ul class="org-ul">
|
||||
<li class="on"><code>[X]</code> Are the rotation matrix commutable? => no</li>
|
||||
<li class="on"><code>[X]</code> How to express the measured rotation errors? => screw axis coordinate seems nice (used in Taghirad's book)</li>
|
||||
<li class="off"><code>[ ]</code> Should ask Veijo how he specifies the position of the Symetrie Hexapod</li>
|
||||
<li class="off"><code>[ ]</code> Create functions for all distinct part and then include that in Simulink</li>
|
||||
<li class="off"><code>[ ]</code> How the express the orientation error?</li>
|
||||
<li class="off"><code>[ ]</code> If we use screw coordinate, can we add/subtract them?</li>
|
||||
<li class="off"><code>[ ]</code> Do some simple tests to verify that the algorithm is working fine</li>
|
||||
</ul>
|
||||
|
||||
<blockquote>
|
||||
<p>
|
||||
Rx = [1 0 0;
|
||||
0 cos(t) -sin(t);
|
||||
0 sin(t) cos(t)];
|
||||
</p>
|
||||
|
||||
<p>
|
||||
Ry = [ cos(t) 0 sin(t);
|
||||
0 1 0;
|
||||
-sin(t) 0 cos(t)];
|
||||
</p>
|
||||
|
||||
<p>
|
||||
Rz = [cos(t) -sin(t) 0;
|
||||
sin(t) cos(t) 0;
|
||||
0 0 1];
|
||||
</p>
|
||||
</blockquote>
|
||||
|
||||
<p>
|
||||
Let's define the following frames:
|
||||
</p>
|
||||
<ul class="org-ul">
|
||||
<li>\(\{W\}\) the frame that is <b>fixed to the granite</b> and its origin at the theoretical meeting point between the X-ray and the spindle axis.</li>
|
||||
<li>\(\{S\}\) the frame <b>attached to the sample</b> (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\).</li>
|
||||
<li>\(\{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.</li>
|
||||
</ul>
|
||||
|
||||
<p>
|
||||
Thus:
|
||||
</p>
|
||||
<ul class="org-ul">
|
||||
<li>the <b>measurement</b> 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</li>
|
||||
<li>the <b>wanted position</b> 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</li>
|
||||
</ul>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org4537a86" class="outline-3">
|
||||
<h3 id="org4537a86"><span class="section-number-3">3.1</span> Wanted Position of the Sample with respect to the Granite</h3>
|
||||
<div class="outline-text-3" id="text-3-1">
|
||||
<p>
|
||||
Let's define the wanted position of each stage.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>Ty = <span class="org-highlight-numbers-number">1</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code>Ry = <span class="org-highlight-numbers-number">3</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
|
||||
<code>Rz = <span class="org-highlight-numbers-number">180</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
|
||||
<code></code>
|
||||
<code><span class="org-comment">% Hexapod (first consider only translations)</span></code>
|
||||
<code>Thx = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code>Thy = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code>Thz = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
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\).
|
||||
</p>
|
||||
|
||||
<p>
|
||||
To do so, we have to define the homogeneous transformation for each stage.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code><span class="org-comment">% Translation Stage</span></code>
|
||||
<code>Rty = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> Ty;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code></code>
|
||||
<code><span class="org-comment">% Tilt Stage - Pure rotating aligned with Ob</span></code>
|
||||
<code>Rry = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code></code>
|
||||
<code><span class="org-comment">% Spindle - Rotation along the Z axis</span></code>
|
||||
<code>Rrz = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;</code>
|
||||
<code> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rz<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> ;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code></code>
|
||||
<code><span class="org-comment">% Micro-Hexapod (only rotations first)</span></code>
|
||||
<code>Rh = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> Thx ;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> Thy ;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> Thz ;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
We combine the individual homogeneous transformations into one homogeneous transformation for all the station.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>Ttot = Rty<span class="org-type">*</span>Rry<span class="org-type">*</span>Rrz<span class="org-type">*</span>Rh;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
Using this homogeneous transformation, we can compute the wanted position and orientation of the sample with respect to the granite.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>WOr = Ttot<span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>;<span class="org-highlight-numbers-number">0</span>;<span class="org-highlight-numbers-number">0</span>;<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>WOr = WOr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>thetar = acos<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>trace<span class="org-rainbow-delimiters-depth-3">(</span>Ttot<span class="org-rainbow-delimiters-depth-4">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-4">)</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span></code>
|
||||
<code><span class="org-keyword">if</span> thetar <span class="org-type">==</span> <span class="org-highlight-numbers-number">0</span></code>
|
||||
<code> WSr = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code><span class="org-keyword">else</span></code>
|
||||
<code> <span class="org-rainbow-delimiters-depth-1">[</span>V, D<span class="org-rainbow-delimiters-depth-1">]</span> = eig<span class="org-rainbow-delimiters-depth-1">(</span>Ttot<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<code> WSr = thetar<span class="org-type">*</span>V<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, abs<span class="org-rainbow-delimiters-depth-2">(</span>diag<span class="org-rainbow-delimiters-depth-3">(</span>D<span class="org-rainbow-delimiters-depth-3">)</span> <span class="org-type">-</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type"><</span> <span class="org-constant">eps</span><span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<code><span class="org-keyword">end</span></code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>WPr = <span class="org-rainbow-delimiters-depth-1">[</span>WOr ; WSr<span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org6c67ae5" class="outline-3">
|
||||
<h3 id="org6c67ae5"><span class="section-number-3">3.2</span> Measured Position of the Sample with respect to the Granite</h3>
|
||||
<div class="outline-text-3" id="text-3-2">
|
||||
<p>
|
||||
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.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code><span class="org-comment">% Measurements: Xm, Ym, Zm, Rx, Ry, Rz</span></code>
|
||||
<code>Dxm = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code>Dym = <span class="org-highlight-numbers-number">1</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code>Dzm = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code></code>
|
||||
<code>Rxm = <span class="org-highlight-numbers-number">0</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
|
||||
<code>Rym = <span class="org-highlight-numbers-number">3</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
|
||||
<code>Rzm = <span class="org-highlight-numbers-number">0</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
Let's compute the corresponding orientation using screw axis.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>Trxm = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>Trym = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>Trzm = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code></code>
|
||||
<code>STw = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-rainbow-delimiters-depth-2">[</span> Trym<span class="org-type">*</span>Trxm<span class="org-type">*</span>Trzm , <span class="org-rainbow-delimiters-depth-3">[</span>Dxm; Dym; Dzm<span class="org-rainbow-delimiters-depth-3">]</span><span class="org-rainbow-delimiters-depth-2">]</span>; <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
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:
|
||||
</p>
|
||||
<ul class="org-ul">
|
||||
<li>\(\theta_m = \cos^{-1} \frac{\text{Tr}(R) - 1}{2}\)</li>
|
||||
<li>\({}^W\bm{s}_m\) is the eigen vector of the rotation matrix \(R\) corresponding to the eigen value \(\lambda = 1\)</li>
|
||||
</ul>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>thetam = acos<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>trace<span class="org-rainbow-delimiters-depth-3">(</span>STw<span class="org-rainbow-delimiters-depth-4">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-4">)</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [rad]</span></code>
|
||||
<code><span class="org-keyword">if</span> thetam <span class="org-type">==</span> <span class="org-highlight-numbers-number">0</span></code>
|
||||
<code> WSm = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code><span class="org-keyword">else</span></code>
|
||||
<code> <span class="org-rainbow-delimiters-depth-1">[</span>V, D<span class="org-rainbow-delimiters-depth-1">]</span> = eig<span class="org-rainbow-delimiters-depth-1">(</span>STw<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<code> WSm = thetam<span class="org-type">*</span>V<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, abs<span class="org-rainbow-delimiters-depth-2">(</span>diag<span class="org-rainbow-delimiters-depth-3">(</span>D<span class="org-rainbow-delimiters-depth-3">)</span> <span class="org-type">-</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type"><</span> <span class="org-constant">eps</span><span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<code><span class="org-keyword">end</span></code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>WPm = <span class="org-rainbow-delimiters-depth-1">[</span>Dxm ; Dym ; Dzm ; WSm<span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org69e38aa" class="outline-3">
|
||||
<h3 id="org69e38aa"><span class="section-number-3">3.3</span> Positioning Error with respect to the Granite</h3>
|
||||
<div class="outline-text-3" id="text-3-3">
|
||||
<p>
|
||||
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 <b>position error</b> 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 \]
|
||||
</p>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>WPe = WPr <span class="org-type">-</span> WPm;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<blockquote>
|
||||
<p>
|
||||
Now we want to express this error in a frame attached to the <b>base of the nano-hexapod</b> 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).
|
||||
</p>
|
||||
|
||||
<p>
|
||||
Or maybe should we want to express this error with respect to the <b>top platform of the nano-hexapod</b>?
|
||||
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:
|
||||
</p>
|
||||
<ul class="org-ul">
|
||||
<li>from the encoders of each stage</li>
|
||||
<li>from the measurement of the nano-hexapod top platform + the internal metrology in the nano-hexapod (capacitive sensors e.g)</li>
|
||||
</ul>
|
||||
|
||||
<p>
|
||||
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.
|
||||
</p>
|
||||
</blockquote>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org5246d94" class="outline-3">
|
||||
<h3 id="org5246d94"><span class="section-number-3">3.4</span> Position Error Expressed in the Nano-Hexapod Frame</h3>
|
||||
<div class="outline-text-3" id="text-3-4">
|
||||
<p>
|
||||
We now want the position error to be expressed in \(\{S\}\) (the frame attach to the sample):
|
||||
\[ {}^S E = {}^S T_W \cdot {}^W E \]
|
||||
</p>
|
||||
|
||||
<p>
|
||||
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.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>Trxm = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>Trym = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>Trzm = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code></code>
|
||||
<code>STw = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-rainbow-delimiters-depth-2">[</span> Trym<span class="org-type">*</span>Trxm<span class="org-type">*</span>Trzm , <span class="org-rainbow-delimiters-depth-3">[</span>Dxm; Dym; Dzm<span class="org-rainbow-delimiters-depth-3">]</span><span class="org-rainbow-delimiters-depth-2">]</span>; <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
Translation Error.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>SEm = STw <span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">[</span>WPe<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span>; <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>SEm = SEm<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
Rotation Error.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>SEr = STw <span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">[</span>WPe<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">4</span><span class="org-type">:</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>SEr = SEr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>Etot = <span class="org-rainbow-delimiters-depth-1">[</span>SEm ; SEr<span class="org-rainbow-delimiters-depth-1">]</span></code>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<div id="postamble" class="status">
|
||||
<p class="author">Author: Dehaeze Thomas</p>
|
||||
<p class="date">Created: 2019-12-04 mer. 21:55</p>
|
||||
<p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p>
|
||||
</div>
|
||||
</body>
|
||||
</html>
|
||||
@ -9,13 +9,13 @@
|
||||
#+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_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
|
||||
|
||||
@ -59,6 +59,7 @@
|
||||
#+end_src
|
||||
|
||||
* Initialize Experiment
|
||||
We first initialize all the stages.
|
||||
#+begin_src matlab
|
||||
initializeGround();
|
||||
initializeGranite();
|
||||
@ -72,280 +73,274 @@
|
||||
initializeSample(struct('mass', 1));
|
||||
#+end_src
|
||||
|
||||
We initialize the reference path for all the stages.
|
||||
#+begin_src matlab
|
||||
t = 0:Ts:Tsim;
|
||||
initializeReferences(struct('Rz_type', 'rotating', 'Rz_period', 1));
|
||||
#+end_src
|
||||
|
||||
Generate perturbations
|
||||
And we initialize the disturbances.
|
||||
#+begin_src matlab
|
||||
win = hanning(ceil(1/Ts));
|
||||
[pxx, f] = pwelch(sqrt(1/(2*Ts))*randn(length(t), 1), win, [], [], 1/Ts);
|
||||
initDisturbances();
|
||||
#+end_src
|
||||
|
||||
* Run the Tomography Experiment
|
||||
We first load the simulation configuration
|
||||
#+begin_src matlab
|
||||
load('simscape/conf_simscape.mat');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(pxx));
|
||||
hold off;
|
||||
set(gca, 'xscale', 'log');
|
||||
set(gca, 'yscale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('ASD of the measured velocity $\left[\frac{m/s}{\sqrt{Hz}}\right]$')
|
||||
ylim([0.01, 100]);
|
||||
set_param(conf_simscape, 'StopTime', '1');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
load('./disturbances/mat/dist_psd.mat', 'dist_f');
|
||||
|
||||
Dwx = lsim(dist_f.G_gm, sqrt(1/(2*Ts))*randn(length(t), 1), t);
|
||||
Dwy = lsim(dist_f.G_gm, sqrt(1/(2*Ts))*randn(length(t), 1), t);
|
||||
Dwz = lsim(dist_f.G_gm, sqrt(1/(2*Ts))*randn(length(t), 1), t);
|
||||
|
||||
Dw = [Dwx, Dwy, Dwz];
|
||||
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
|
||||
figure;
|
||||
hold on;
|
||||
plot(t, Dwx);
|
||||
plot(t, Dwy);
|
||||
plot(t, Dwz);
|
||||
hold off;
|
||||
xlabel('Time [s]'); ylabel('Displacement [m]');
|
||||
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
|
||||
Fty_z = lsim(dist_f.G_ty, sqrt(1/(2*Ts))*randn(length(t), 1), t);
|
||||
Frz_z = lsim(dist_f.G_rz, sqrt(1/(2*Ts))*randn(length(t), 1), t);
|
||||
% 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
|
||||
figure;
|
||||
hold on;
|
||||
plot(t, Fty_z);
|
||||
plot(t, Frz_z);
|
||||
hold off;
|
||||
xlabel('Time [s]'); ylabel('Force [N]');
|
||||
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
|
||||
% Spindle
|
||||
Rz = 2*pi*t;
|
||||
|
||||
% Axisc
|
||||
Rm = zeros(length(t), 2);
|
||||
Rm(:, 2) = pi*ones(length(t), 1);
|
||||
|
||||
inputs = struct( ...
|
||||
'Ts', Ts, ...
|
||||
'Dw', timeseries(Dw, t), ...
|
||||
'Dy', timeseries(zeros(length(t), 1), t), ...
|
||||
'Ry', timeseries(zeros(length(t), 1), t), ...
|
||||
'Rz', timeseries(Rz, t), ...
|
||||
'Dh', timeseries(zeros(length(t), 6), t), ...
|
||||
'Rm', timeseries(Rm, t), ...
|
||||
'Dn', timeseries(zeros(length(t), 6), t), ...
|
||||
'Ds', timeseries(zeros(length(t), 6), t), ...
|
||||
'Fg', timeseries(zeros(length(t), 3), t), ...
|
||||
'Fn', timeseries(zeros(length(t), 6), t), ...
|
||||
'Fnl', timeseries(zeros(length(t), 6), t), ...
|
||||
'Fty_x', timeseries(zeros(length(t), 1), t), ...
|
||||
'Fty_z', timeseries(Fty_z, t), ...
|
||||
'Frz_z', timeseries(Frz_z, t), ...
|
||||
'Fs', timeseries(zeros(length(t), 6), t) ...
|
||||
);
|
||||
#+end_src
|
||||
|
||||
* Test
|
||||
#+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>>
|
||||
WOr = Ttot*[0;0;0;1];
|
||||
WOr = WOr(1:3);
|
||||
#+end_src
|
||||
|
||||
Rotation.
|
||||
#+begin_src matlab
|
||||
cd ..
|
||||
#+end_src
|
||||
|
||||
We define some parameters:
|
||||
- =T0= is the duration in seconds
|
||||
- =N= is the number of samples
|
||||
|
||||
#+begin_src matlab
|
||||
T0 = 10; % Signal Duration [s]
|
||||
N = 10000; % Number of Samples (should be even)
|
||||
#+end_src
|
||||
|
||||
Then, we have:
|
||||
- $f_s = N/T_0$ is the sampling frequency in Hz
|
||||
- $f_c = \frac{1}{2} N/T_0$ is the cut-off frequency in Hz
|
||||
- $f_0 = 1/T_0$ is the frequency resolution of the DFT in Hz
|
||||
#+begin_src matlab
|
||||
Fs = N/T0; % Sampling frequency [Hz]
|
||||
Fc = (1/2)*N/T0; % Sampling frequency [Hz]
|
||||
df = 1/T0; % Frequency resolution of the DFT [Hz]
|
||||
#+end_src
|
||||
|
||||
We then specify the wanted PSD.
|
||||
#+begin_src matlab
|
||||
phi = ones(N/2, 1);
|
||||
% phi = logspace(3, 0, N/2);
|
||||
phi(df:df:Fs/2>10) = 0;
|
||||
#+end_src
|
||||
|
||||
We create $C_x(k)$ such that:
|
||||
\[ C_x(k) = \sqrt{\Phi_{xx}(k\omega_0)\omega_0} \quad k = 1 \dots N/2 \]
|
||||
where $\Phi_{xx}$ is the wanted PSD.
|
||||
#+begin_src matlab
|
||||
C = zeros(N/2, 1);
|
||||
for i = 1:N/2
|
||||
C(i) = sqrt(phi(i))*2*Fs; % TODO - Why this normalization?
|
||||
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
|
||||
|
||||
We generate some random phase that will be added to =C=.
|
||||
#+begin_src matlab
|
||||
theta = 2*pi*rand(N/2, 1); % Generate random phase [rad]
|
||||
WPr = [WOr ; WSr];
|
||||
#+end_src
|
||||
|
||||
In order to have
|
||||
\[ C_x(N/2+i) = C_x^*(N/2-i) \quad i = 1 \dots N/2 \]
|
||||
We do the following
|
||||
** 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
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
% 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
|
||||
|
||||
The time domain data is generated by an inverse FFT.
|
||||
Let's compute the corresponding orientation using screw axis.
|
||||
#+begin_src matlab
|
||||
u = ifft(Cx);
|
||||
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
|
||||
A = fft(u);
|
||||
figure; plot(2*A.*conj(A))
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
t = linspace(0, T0, N+1); % Time Vector [s]
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
figure;
|
||||
plot(t, u)
|
||||
xlabel('Time [s]');
|
||||
ylabel('Amplitude');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
nx = length(u);
|
||||
na = 16;
|
||||
win = hanning(floor(nx/na));
|
||||
|
||||
[pxx, f] = pwelch(u, win, 0, [], Fs);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
figure;
|
||||
hold on;
|
||||
plot(f,pxx)
|
||||
plot(df:df:Fc,phi)
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('Power Spectral Density');
|
||||
set(gca, 'xscale', 'log');
|
||||
set(gca, 'yscale', 'log');
|
||||
#+end_src
|
||||
|
||||
* Test
|
||||
#+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
|
||||
cd ..
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
load('./disturbances/mat/dist_psd.mat', 'dist_f');
|
||||
#+end_src
|
||||
|
||||
We remove the first value with very high PSD.
|
||||
#+begin_src matlab
|
||||
dist_f.f = dist_f.f(3:end);
|
||||
dist_f.psd_gm = dist_f.psd_gm(3:end);
|
||||
#+end_src
|
||||
|
||||
We define some parameters.
|
||||
#+begin_src matlab
|
||||
Fs = 2*dist_f.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
|
||||
N = 2*length(dist_f.f); % Number of Samples match the one of the wanted PSD
|
||||
T0 = N/Fs; % Signal Duration [s]
|
||||
df = 1/T0; % Frequency resolution of the DFT [Hz]
|
||||
% Also equal to (dist_f.f(2)-dist_f.f(1))
|
||||
#+end_src
|
||||
|
||||
We then specify the wanted PSD.
|
||||
#+begin_src matlab
|
||||
phi = dist_f.psd_gm;
|
||||
#+end_src
|
||||
|
||||
Create amplitudes corresponding to wanted PSD.
|
||||
#+begin_src matlab
|
||||
C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
C(i) = sqrt(phi(i)*df);
|
||||
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
|
||||
|
||||
Add random phase to =C=.
|
||||
#+begin_src matlab
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
WPm = [Dxm ; Dym ; Dzm ; WSm];
|
||||
#+end_src
|
||||
|
||||
The time domain data is generated by an inverse FFT.
|
||||
We normalize the =ifft= Matlab command.
|
||||
** 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
|
||||
u = 1/(sqrt(2)*df*1/Fs)*ifft(Cx); % Normalisation of the IFFT
|
||||
t = linspace(0, T0, N+1); % Time Vector [s]
|
||||
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
|
||||
figure;
|
||||
plot(t, u)
|
||||
xlabel('Time [s]');
|
||||
ylabel('Amplitude');
|
||||
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
|
||||
u_rep = [u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u];
|
||||
MTr = STw'*Ttot;
|
||||
#+end_src
|
||||
|
||||
Position error:
|
||||
#+begin_src matlab
|
||||
nx = length(u_rep);
|
||||
na = 16;
|
||||
win = hanning(floor(nx/na));
|
||||
|
||||
[pxx, f] = pwelch(u_rep, win, 0, [], Fs);
|
||||
MTr(1:3, 1:4)*[0; 0; 0; 1]
|
||||
#+end_src
|
||||
|
||||
Orientation error:
|
||||
#+begin_src matlab
|
||||
figure;
|
||||
hold on;
|
||||
plot(dist_f.f, dist_f.psd_gm, 'DisplayName', 'Original PSD')
|
||||
plot(f, pxx, 'DisplayName', 'Computed')
|
||||
% plot(f, pxx./dist_f.psd_gm, 'k-')
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('Power Spectral Density');
|
||||
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||||
legend('location', 'northeast');
|
||||
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