Change all the organization of the files
This commit is contained in:
3531
org/active_damping.org
Normal file
3531
org/active_damping.org
Normal file
File diff suppressed because it is too large
Load Diff
2063
org/active_damping_uniaxial.org
Normal file
2063
org/active_damping_uniaxial.org
Normal file
File diff suppressed because it is too large
Load Diff
42
org/control.org
Normal file
42
org/control.org
Normal file
@@ -0,0 +1,42 @@
|
||||
#+TITLE: Control of the NASS
|
||||
:DRAWER:
|
||||
#+STARTUP: overview
|
||||
|
||||
#+LANGUAGE: en
|
||||
#+EMAIL: dehaeze.thomas@gmail.com
|
||||
#+AUTHOR: Dehaeze Thomas
|
||||
|
||||
#+HTML_LINK_HOME: ../index.html
|
||||
#+HTML_LINK_UP: ../index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
||||
|
||||
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
#+PROPERTY: header-args:matlab+ :tangle matlab/modal_frf_coh.m
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
|
||||
#+PROPERTY: header-args:shell :eval no-export
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
568
org/disturbances.org
Normal file
568
org/disturbances.org
Normal file
@@ -0,0 +1,568 @@
|
||||
#+TITLE: Identification of the disturbances
|
||||
:DRAWER:
|
||||
#+STARTUP: overview
|
||||
|
||||
#+LANGUAGE: en
|
||||
#+EMAIL: dehaeze.thomas@gmail.com
|
||||
#+AUTHOR: Dehaeze Thomas
|
||||
|
||||
#+HTML_LINK_HOME: ../index.html
|
||||
#+HTML_LINK_UP: ../index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
||||
|
||||
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
#+PROPERTY: header-args:matlab+ :tangle no
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
|
||||
#+PROPERTY: header-args:shell :eval no-export
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
:PROPERTIES:
|
||||
:CUSTOM_ID: Introduction
|
||||
:END:
|
||||
The goal here is to extract the Power Spectral Density of the sources of perturbation.
|
||||
|
||||
The sources of perturbations are (schematically shown in figure [[fig:uniaxial-model-micro-station]]):
|
||||
- $D_w$: Ground Motion
|
||||
- Parasitic forces applied in the system when scanning with the Translation Stage and the Spindle ($F_{rz}$ and $F_{ty}$).
|
||||
These forces can be due to imperfect guiding for instance.
|
||||
|
||||
Because we cannot measure directly the perturbation forces, we have the measure the effect of those perturbations on the system (in terms of velocity for instance using geophones, $D$ on figure [[fig:uniaxial-model-micro-station]]) and then, using a model, compute the forces that induced such velocity.
|
||||
|
||||
|
||||
#+begin_src latex :file uniaxial-model-micro-station.pdf :post pdf2svg(file=*this*, ext="png") :exports results
|
||||
\begin{tikzpicture}
|
||||
% ====================
|
||||
% Parameters
|
||||
% ====================
|
||||
\def\massw{2.2} % Width of the masses
|
||||
\def\massh{0.8} % Height of the masses
|
||||
\def\spaceh{1.2} % Height of the springs/dampers
|
||||
\def\dispw{0.4} % Width of the dashed line for the displacement
|
||||
\def\disph{0.3} % Height of the arrow for the displacements
|
||||
\def\bracs{0.05} % Brace spacing vertically
|
||||
\def\brach{-12pt} % Brace shift horizontaly
|
||||
\def\fsensh{0.2} % Height of the force sensor
|
||||
\def\velsize{0.2} % Size of the velocity sensor
|
||||
% ====================
|
||||
|
||||
|
||||
% ====================
|
||||
% Ground
|
||||
% ====================
|
||||
\draw (-0.5*\massw, 0) -- (0.5*\massw, 0);
|
||||
\draw[dashed] (0.5*\massw, 0) -- ++(1, 0);
|
||||
\draw[->, dasehd] (0.5*\massw+0.8, 0) -- ++(0, 0.5) node[right]{$D_{w}$};
|
||||
% ====================
|
||||
|
||||
|
||||
% ====================
|
||||
% Marble
|
||||
\begin{scope}[shift={(0, 0)}]
|
||||
% Mass
|
||||
\draw[fill=white] (-0.5*\massw, \spaceh) rectangle (0.5*\massw, \spaceh+\massh) node[pos=0.5]{$m_{m}$};
|
||||
|
||||
% Spring, Damper, and Actuator
|
||||
\draw[spring] (-0.4*\massw, 0) -- (-0.4*\massw, \spaceh) node[midway, left=0.1]{$k_{m}$};
|
||||
\draw[damper] (0, 0) -- ( 0, \spaceh) node[midway, left=0.2]{$c_{m}$};
|
||||
|
||||
% Legend
|
||||
\draw[decorate, decoration={brace, amplitude=8pt}, xshift=\brach] %
|
||||
(-0.5*\massw, \bracs) -- (-0.5*\massw, \spaceh+\massh-\bracs) node[midway,rotate=90,anchor=south,yshift=10pt]{Marble};
|
||||
|
||||
% Displacements
|
||||
\draw[dashed] (0.5*\massw, \spaceh+\massh) -- ++(2*\dispw, 0) coordinate(xm) -- ++(2.2*\dispw, 0) coordinate(dbot);
|
||||
\end{scope}
|
||||
% ====================
|
||||
|
||||
|
||||
% ====================
|
||||
% Ty
|
||||
\begin{scope}[shift={(0, \spaceh+\massh)}]
|
||||
% Mass
|
||||
\draw[fill=white] (-0.5*\massw, \spaceh) rectangle (0.5*\massw, \spaceh+\massh) node[pos=0.5]{$m_{t}$};
|
||||
|
||||
% Spring, Damper, and Actuator
|
||||
\draw[spring] (-0.4*\massw, 0) -- (-0.4*\massw, \spaceh) node[midway, left=0.1]{$k_{t}$};
|
||||
\draw[damper] (0, 0) -- ( 0, \spaceh) node[midway, left=0.2]{$c_{t}$};
|
||||
\draw[actuator={0.45}{0.2}] ( 0.4*\massw, 0) -- ( 0.4*\massw, \spaceh) node[midway, right=0.1](ft){$F_{ty}$};
|
||||
|
||||
% Legend
|
||||
\draw[decorate, decoration={brace, amplitude=8pt}, xshift=\brach] %
|
||||
(-0.5*\massw, \bracs) -- (-0.5*\massw, \spaceh+\massh-\bracs) node[midway,rotate=90,anchor=south,yshift=10pt]{Ty};
|
||||
\end{scope}
|
||||
% ====================
|
||||
|
||||
|
||||
% ====================
|
||||
% Rz
|
||||
\begin{scope}[shift={(0, 2*(\spaceh+\massh))}]
|
||||
% Mass
|
||||
\draw[fill=white] (-0.5*\massw, \spaceh) rectangle (0.5*\massw, \spaceh+\massh) node[pos=0.5]{$m_{z}$};
|
||||
|
||||
% Spring, Damper, and Actuator
|
||||
\draw[spring] (-0.4*\massw, 0) -- (-0.4*\massw, \spaceh) node[midway, left=0.1]{$k_{z}$};
|
||||
\draw[damper] (0, 0) -- ( 0, \spaceh) node[midway, left=0.2]{$c_{z}$};
|
||||
\draw[actuator] ( 0.4*\massw, 0) -- ( 0.4*\massw, \spaceh) node[midway, right=0.1](F){$F_{rz}$};
|
||||
|
||||
% Legend
|
||||
\draw[decorate, decoration={brace, amplitude=8pt}, xshift=\brach] %
|
||||
(-0.5*\massw, \bracs) -- (-0.5*\massw, \spaceh+\massh-\bracs) node[midway,rotate=90,anchor=south,yshift=10pt]{Rz};
|
||||
\end{scope}
|
||||
% ====================
|
||||
|
||||
|
||||
% ====================
|
||||
% Hexapod
|
||||
\begin{scope}[shift={(0, 3*(\spaceh+\massh))}]
|
||||
% Mass
|
||||
\draw[fill=white] (-0.5*\massw, \spaceh) rectangle (0.5*\massw, \spaceh+\massh) node[pos=0.5]{$m_{h}$};
|
||||
|
||||
% Spring, Damper, and Actuator
|
||||
\draw[spring] (-0.4*\massw, 0) -- (-0.4*\massw, \spaceh) node[midway, left=0.1]{$k_{h}$};
|
||||
\draw[damper] (0, 0) -- ( 0, \spaceh) node[midway, left=0.2]{$c_{h}$};
|
||||
|
||||
% Displacements
|
||||
\draw[dashed] (0.5*\massw, \spaceh+\massh) -- ++(2*\dispw, 0) coordinate(xs) -- ++(2.2*\dispw, 0) coordinate(dtop);
|
||||
|
||||
% Legend
|
||||
\draw[decorate, decoration={brace, amplitude=8pt}, xshift=\brach] %
|
||||
(-0.5*\massw, \bracs) -- (-0.5*\massw, \spaceh+\massh-\bracs) node[midway,rotate=90,anchor=south,yshift=10pt]{Hexapod};
|
||||
\end{scope}
|
||||
% ====================
|
||||
|
||||
\draw[<->] ($(dbot)+(-0.3,0)$) --node[midway, right]{$D$} ($(dtop)+(-0.3,0)$);
|
||||
\end{tikzpicture}
|
||||
#+end_src
|
||||
|
||||
#+name: fig:uniaxial-model-micro-station
|
||||
#+caption: Schematic of the Micro Station and the sources of disturbance
|
||||
#+RESULTS:
|
||||
[[file:figs/uniaxial-model-micro-station.png]]
|
||||
|
||||
|
||||
This file is divided in the following sections:
|
||||
- Section [[sec:simscape_model]]: the simscape model used here is presented
|
||||
- Section [[sec:identification]]: transfer functions from the disturbance forces to the relative velocity of the hexapod with respect to the granite are computed using the Simscape Model representing the experimental setup
|
||||
- Section [[sec:sensitivity_disturbances]]: the bode plot of those transfer functions are shown
|
||||
- Section [[sec:psd_dist]]: the measured PSD of the effect of the disturbances are shown
|
||||
- Section [[sec:psd_force_dist]]: from the model and the measured PSD, the PSD of the disturbance forces are computed
|
||||
- Section [[sec:noise_budget]]: with the computed PSD, the noise budget of the system is done
|
||||
|
||||
* Matlab Init :noexport:ignore:
|
||||
:PROPERTIES:
|
||||
:CUSTOM_ID: Matlab-Init
|
||||
:END:
|
||||
#+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('nass_model.slx')
|
||||
#+end_src
|
||||
|
||||
* Simscape Model
|
||||
:PROPERTIES:
|
||||
:CUSTOM_ID: Simscape-Model
|
||||
:END:
|
||||
<<sec:simscape_model>>
|
||||
|
||||
The following Simscape model of the micro-station is the same model used for the dynamical analysis.
|
||||
However, we here constrain all the stage to only move in the vertical direction.
|
||||
|
||||
We add disturbances forces in the vertical direction for the Translation Stage and the Spindle.
|
||||
Also, we measure the absolute displacement of the granite and of the top platform of the Hexapod.
|
||||
|
||||
We load the configuration and we set a small =StopTime=.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '0.5');
|
||||
#+end_src
|
||||
|
||||
We initialize all the stages.
|
||||
#+begin_src matlab
|
||||
initializeGround();
|
||||
initializeGranite('type', 'modal-analysis');
|
||||
initializeTy();
|
||||
initializeRy();
|
||||
initializeRz();
|
||||
initializeMicroHexapod('type', 'modal-analysis');
|
||||
initializeAxisc('type', 'none');
|
||||
initializeMirror('type', 'none');
|
||||
initializeNanoHexapod('type', 'none');
|
||||
initializeSample('type', 'none');
|
||||
#+end_src
|
||||
|
||||
* Identification
|
||||
:PROPERTIES:
|
||||
:CUSTOM_ID: Identification
|
||||
:END:
|
||||
<<sec:identification>>
|
||||
The transfer functions from the disturbance forces to the relative velocity of the hexapod with respect to the granite are computed using the Simscape Model representing the experimental setup with the code below.
|
||||
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'nass_model';
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
%% Micro-Hexapod
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz'); io_i = io_i + 1; % Vertical Ground Motion
|
||||
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fty_z'); io_i = io_i + 1; % Parasitic force Ty
|
||||
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Frz_z'); io_i = io_i + 1; % Parasitic force Rz
|
||||
io(io_i) = linio([mdl, '/Micro-Station/Granite/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1; % Absolute motion - Granite
|
||||
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Motion - Hexapod
|
||||
% io(io_i) = linio([mdl, '/Vm'], 1, 'openoutput'); io_i = io_i + 1; % Relative Velocity hexapod/granite
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
% Run the linearization
|
||||
G = linearize(mdl, io, 0);
|
||||
|
||||
% We Take only the outputs corresponding to the vertical acceleration
|
||||
G = G([3,9], :);
|
||||
|
||||
% Input/Output names
|
||||
G.InputName = {'Dw', 'Fty', 'Frz'};
|
||||
G.OutputName = {'Agm', 'Ahm'};
|
||||
|
||||
% We integrate 1 time the output to have the velocity and we
|
||||
% substract the absolute velocities to have the relative velocity
|
||||
G = (1/s)*tf([-1, 1])*G;
|
||||
|
||||
% Input/Output names
|
||||
G.InputName = {'Dw', 'Fty', 'Frz'};
|
||||
G.OutputName = {'Vm'};
|
||||
#+end_src
|
||||
|
||||
* Sensitivity to Disturbances
|
||||
:PROPERTIES:
|
||||
:CUSTOM_ID: Sensitivity-to-Disturbances
|
||||
:END:
|
||||
<<sec:sensitivity_disturbances>>
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
title('$D_w$ to $D$');
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G('Vm', 'Dw')/s, freqs, 'Hz'))), 'k-');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/sensitivity_dist_gm.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:sensitivity_dist_gm
|
||||
#+CAPTION: Sensitivity to Ground Motion ([[./figs/sensitivity_dist_gm.png][png]], [[./figs/sensitivity_dist_gm.pdf][pdf]])
|
||||
[[file:figs/sensitivity_dist_gm.png]]
|
||||
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
title('$F_{ty}$ to $D$');
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G('Vm', 'Fty')/s, freqs, 'Hz'))), 'k-');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/sensitivity_dist_fty.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:sensitivity_dist_fty
|
||||
#+CAPTION: Sensitivity to vertical forces applied by the Ty stage ([[./figs/sensitivity_dist_fty.png][png]], [[./figs/sensitivity_dist_fty.pdf][pdf]])
|
||||
[[file:figs/sensitivity_dist_fty.png]]
|
||||
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
title('$F_{rz}$ to $D$');
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G('Vm', 'Frz')/s, freqs, 'Hz'))), 'k-');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/sensitivity_dist_frz.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:sensitivity_dist_frz
|
||||
#+CAPTION: Sensitivity to vertical forces applied by the Rz stage ([[./figs/sensitivity_dist_frz.png][png]], [[./figs/sensitivity_dist_frz.pdf][pdf]])
|
||||
[[file:figs/sensitivity_dist_frz.png]]
|
||||
|
||||
* Power Spectral Density of the effect of the disturbances
|
||||
:PROPERTIES:
|
||||
:CUSTOM_ID: Power-Spectral-Density-of-the-effect-of-the-disturbances
|
||||
:END:
|
||||
<<sec:psd_dist>>
|
||||
The PSD of the relative velocity between the hexapod and the marble in $[(m/s)^2/Hz]$ are loaded for the following sources of disturbance:
|
||||
- Slip Ring Rotation
|
||||
- Scan of the translation stage (effect in the vertical direction and in the horizontal direction)
|
||||
|
||||
Also, the Ground Motion is measured.
|
||||
|
||||
#+begin_src matlab
|
||||
gm = load('./disturbances/mat/psd_gm.mat', 'f', 'psd_gm', 'psd_gv');
|
||||
rz = load('./disturbances/mat/pxsp_r.mat', 'f', 'pxsp_r');
|
||||
tyz = load('./disturbances/mat/pxz_ty_r.mat', 'f', 'pxz_ty_r');
|
||||
tyx = load('./disturbances/mat/pxe_ty_r.mat', 'f', 'pxe_ty_r');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
gm.f = gm.f(2:end);
|
||||
rz.f = rz.f(2:end);
|
||||
tyz.f = tyz.f(2:end);
|
||||
tyx.f = tyx.f(2:end);
|
||||
|
||||
gm.psd_gm = gm.psd_gm(2:end); % PSD of Ground Motion [m^2/Hz]
|
||||
gm.psd_gv = gm.psd_gv(2:end); % PSD of Ground Velocity [(m/s)^2/Hz]
|
||||
rz.pxsp_r = rz.pxsp_r(2:end); % PSD of Relative Velocity [(m/s)^2/Hz]
|
||||
tyz.pxz_ty_r = tyz.pxz_ty_r(2:end); % PSD of Relative Velocity [(m/s)^2/Hz]
|
||||
tyx.pxe_ty_r = tyx.pxe_ty_r(2:end); % PSD of Relative Velocity [(m/s)^2/Hz]
|
||||
#+end_src
|
||||
|
||||
We now compute the relative velocity between the hexapod and the granite due to ground motion.
|
||||
#+begin_src matlab
|
||||
gm.psd_rv = gm.psd_gm.*abs(squeeze(freqresp(G('Vm', 'Dw'), gm.f, 'Hz'))).^2;
|
||||
#+end_src
|
||||
|
||||
The Power Spectral Density of the relative motion/velocity of the hexapod with respect to the granite are shown in figures [[fig:dist_effect_relative_velocity]] and [[fig:dist_effect_relative_motion]].
|
||||
|
||||
The Cumulative Amplitude Spectrum of the relative motion is shown in figure [[fig:dist_effect_relative_motion_cas]].
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
hold on;
|
||||
plot(gm.f, sqrt(gm.psd_rv), 'DisplayName', 'Ground Motion');
|
||||
plot(tyz.f, sqrt(tyz.pxz_ty_r), 'DisplayName', 'Ty');
|
||||
plot(rz.f, sqrt(rz.pxsp_r), 'DisplayName', 'Rz');
|
||||
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]$')
|
||||
legend('Location', 'southwest');
|
||||
xlim([2, 500]);
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/dist_effect_relative_velocity.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:dist_effect_relative_velocity
|
||||
#+CAPTION: Amplitude Spectral Density of the relative velocity of the hexapod with respect to the granite due to different sources of perturbation ([[./figs/dist_effect_relative_velocity.png][png]], [[./figs/dist_effect_relative_velocity.pdf][pdf]])
|
||||
[[file:figs/dist_effect_relative_velocity.png]]
|
||||
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
hold on;
|
||||
plot(gm.f, sqrt(gm.psd_rv)./(2*pi*gm.f), 'DisplayName', 'Ground Motion');
|
||||
plot(tyz.f, sqrt(tyz.pxz_ty_r)./(2*pi*tyz.f), 'DisplayName', 'Ty');
|
||||
plot(rz.f, sqrt(rz.pxsp_r)./(2*pi*rz.f), 'DisplayName', 'Rz');
|
||||
hold off;
|
||||
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('ASD of the displacement $\left[\frac{m}{\sqrt{Hz}}\right]$')
|
||||
legend('Location', 'southwest');
|
||||
xlim([2, 500]);
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/dist_effect_relative_motion.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:dist_effect_relative_motion
|
||||
#+CAPTION: Amplitude Spectral Density of the relative displacement of the hexapod with respect to the granite due to different sources of perturbation ([[./figs/dist_effect_relative_motion.png][png]], [[./figs/dist_effect_relative_motion.pdf][pdf]])
|
||||
[[file:figs/dist_effect_relative_motion.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
hold on;
|
||||
plot(gm.f, flip(sqrt(-cumtrapz(flip(gm.f), flip(gm.psd_rv./((2*pi*gm.f).^2))))), 'DisplayName', 'Ground Motion');
|
||||
plot(tyz.f, flip(sqrt(-cumtrapz(flip(tyz.f), flip(tyz.pxz_ty_r./((2*pi*tyz.f).^2))))), 'DisplayName', 'Ty');
|
||||
plot(rz.f, flip(sqrt(-cumtrapz(flip(rz.f), flip(rz.pxsp_r./((2*pi*rz.f).^2))))), 'DisplayName', 'Rz');
|
||||
hold off;
|
||||
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('CAS of the relative displacement $[m]$')
|
||||
legend('Location', 'southwest');
|
||||
xlim([2, 500]); ylim([1e-11, 1e-6]);
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/dist_effect_relative_motion_cas.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:dist_effect_relative_motion_cas
|
||||
#+CAPTION: Cumulative Amplitude Spectrum of the relative motion due to different sources of perturbation ([[./figs/dist_effect_relative_motion_cas.png][png]], [[./figs/dist_effect_relative_motion_cas.pdf][pdf]])
|
||||
[[file:figs/dist_effect_relative_motion_cas.png]]
|
||||
|
||||
* Compute the Power Spectral Density of the disturbance force
|
||||
:PROPERTIES:
|
||||
:CUSTOM_ID: Compute-the-Power-Spectral-Density-of-the-disturbance-force
|
||||
:END:
|
||||
<<sec:psd_force_dist>>
|
||||
|
||||
Now, from the extracted transfer functions from the disturbance force to the relative motion of the hexapod with respect to the granite (section [[sec:sensitivity_disturbances]]) and from the measured PSD of the relative motion (section [[sec:psd_dist]]), we can compute the PSD of the disturbance force.
|
||||
|
||||
#+begin_src matlab
|
||||
rz.psd_f = rz.pxsp_r./abs(squeeze(freqresp(G('Vm', 'Frz'), rz.f, 'Hz'))).^2;
|
||||
tyz.psd_f = tyz.pxz_ty_r./abs(squeeze(freqresp(G('Vm', 'Fty'), tyz.f, 'Hz'))).^2;
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
hold on;
|
||||
set(gca,'ColorOrderIndex',2);
|
||||
plot(tyz.f, sqrt(tyz.psd_f), 'DisplayName', 'F - Ty');
|
||||
plot(rz.f, sqrt(rz.psd_f), 'DisplayName', 'F - Rz');
|
||||
hold off;
|
||||
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('ASD of the disturbance force $\left[\frac{F}{\sqrt{Hz}}\right]$')
|
||||
legend('Location', 'southwest');
|
||||
xlim([2, 500]);
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/dist_force_psd.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:dist_force_psd
|
||||
#+CAPTION: Amplitude Spectral Density of the disturbance force ([[./figs/dist_force_psd.png][png]], [[./figs/dist_force_psd.pdf][pdf]])
|
||||
[[file:figs/dist_force_psd.png]]
|
||||
|
||||
* Noise Budget
|
||||
:PROPERTIES:
|
||||
:CUSTOM_ID: Noise-Budget
|
||||
:END:
|
||||
<<sec:noise_budget>>
|
||||
|
||||
Now, from the compute spectral density of the disturbance sources, we can compute the resulting relative motion of the Hexapod with respect to the granite using the model.
|
||||
We should verify that this is coherent with the measurements.
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
% Power Spectral Density of the relative Displacement
|
||||
psd_gm_d = gm.psd_gm.*abs(squeeze(freqresp(G('Vm', 'Dw')/s, gm.f, 'Hz'))).^2;
|
||||
psd_ty_d = tyz.psd_f.*abs(squeeze(freqresp(G('Vm', 'Fty')/s, tyz.f, 'Hz'))).^2;
|
||||
psd_rz_d = rz.psd_f.*abs(squeeze(freqresp(G('Vm', 'Frz')/s, rz.f, 'Hz'))).^2;
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
hold on;
|
||||
plot(gm.f, sqrt(psd_gm_d), 'DisplayName', 'Ground Motion');
|
||||
plot(tyz.f, sqrt(psd_ty_d), 'DisplayName', 'Ty');
|
||||
plot(rz.f, sqrt(psd_rz_d), 'DisplayName', 'Rz');
|
||||
plot(rz.f, sqrt(psd_gm_d + psd_ty_d + psd_rz_d), 'k--', 'DisplayName', 'tot');
|
||||
hold off;
|
||||
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('ASD of the relative motion $\left[\frac{m}{\sqrt{Hz}}\right]$')
|
||||
legend('Location', 'southwest');
|
||||
xlim([2, 500]);
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/psd_effect_dist_verif.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:psd_effect_dist_verif
|
||||
#+CAPTION: Computed Effect of the disturbances on the relative displacement hexapod/granite ([[./figs/psd_effect_dist_verif.png][png]], [[./figs/psd_effect_dist_verif.pdf][pdf]])
|
||||
[[file:figs/psd_effect_dist_verif.png]]
|
||||
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
hold on;
|
||||
plot(gm.f, flip(sqrt(-cumtrapz(flip(gm.f), flip(psd_gm_d)))), 'DisplayName', 'Ground Motion');
|
||||
plot(gm.f, flip(sqrt(-cumtrapz(flip(gm.f), flip(psd_ty_d)))), 'DisplayName', 'Ty');
|
||||
plot(gm.f, flip(sqrt(-cumtrapz(flip(gm.f), flip(psd_rz_d)))), 'DisplayName', 'Rz');
|
||||
plot(gm.f, flip(sqrt(-cumtrapz(flip(gm.f), flip(psd_gm_d + psd_ty_d + psd_rz_d)))), 'k-', 'DisplayName', 'tot');
|
||||
hold off;
|
||||
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Cumulative Amplitude Spectrum [m]')
|
||||
legend('location', 'northeast');
|
||||
xlim([2, 500]); ylim([1e-11, 1e-6]);
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/cas_computed_relative_displacement.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:cas_computed_relative_displacement
|
||||
#+CAPTION: CAS of the total Relative Displacement due to all considered sources of perturbation ([[./figs/cas_computed_relative_displacement.png][png]], [[./figs/cas_computed_relative_displacement.pdf][pdf]])
|
||||
[[file:figs/cas_computed_relative_displacement.png]]
|
||||
|
||||
* Save
|
||||
:PROPERTIES:
|
||||
:CUSTOM_ID: Save
|
||||
:END:
|
||||
The PSD of the disturbance force are now saved for further analysis (the mat file is accessible [[file:mat/dist_psd.mat][here]]).
|
||||
|
||||
#+begin_src matlab
|
||||
dist_f = struct();
|
||||
|
||||
dist_f.f = gm.f; % Frequency Vector [Hz]
|
||||
|
||||
dist_f.psd_gm = gm.psd_gm; % Power Spectral Density of the Ground Motion [m^2/Hz]
|
||||
dist_f.psd_ty = tyz.psd_f; % Power Spectral Density of the force induced by the Ty stage in the Z direction [N^2/Hz]
|
||||
dist_f.psd_rz = rz.psd_f; % Power Spectral Density of the force induced by the Rz stage in the Z direction [N^2/Hz]
|
||||
|
||||
save('./disturbances/mat/dist_psd.mat', 'dist_f');
|
||||
#+end_src
|
||||
575
org/experiments.org
Normal file
575
org/experiments.org
Normal file
@@ -0,0 +1,575 @@
|
||||
#+TITLE: Tomography Experiment
|
||||
:DRAWER:
|
||||
#+STARTUP: overview
|
||||
|
||||
#+LANGUAGE: en
|
||||
#+EMAIL: dehaeze.thomas@gmail.com
|
||||
#+AUTHOR: Dehaeze Thomas
|
||||
|
||||
#+HTML_LINK_HOME: ../index.html
|
||||
#+HTML_LINK_UP: ../index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
||||
|
||||
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
#+PROPERTY: header-args:matlab+ :tangle matlab/tomo_exp.m
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
|
||||
#+PROPERTY: header-args:shell :eval no-export
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
The goal here is to simulate some scientific experiments with the tuned Simscape model when no control is applied to the nano-hexapod.
|
||||
|
||||
This has several goals:
|
||||
- Validate the model
|
||||
- Estimate the expected error motion for the experiments
|
||||
- Estimate the stroke that we may need for the nano-hexapod
|
||||
|
||||
The document in organized as follow:
|
||||
- In section [[sec:simscape_model]] the Simscape model is initialized
|
||||
- In section [[sec:tomo_no_dist]] a tomography experiment is performed where the sample is aligned with the rotation axis. No disturbance is included
|
||||
- In section [[sec:tomo_dist]], the same is done but with disturbance included
|
||||
- In section [[sec:tomo_hexa_trans]] the micro-hexapod translate the sample such that its center of mass is no longer aligned with the rotation axis. No disturbance is included
|
||||
- In section [[sec:ty_scans]], scans with the translation stage are simulated with no perturbation included
|
||||
|
||||
* 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
|
||||
|
||||
* Simscape Model
|
||||
<<sec:simscape_model>>
|
||||
|
||||
#+begin_src matlab
|
||||
open('nass_model.slx');
|
||||
#+end_src
|
||||
|
||||
We load the shared simulink configuration and we set the =StopTime=.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '5');
|
||||
#+end_src
|
||||
|
||||
We first initialize all the stages.
|
||||
#+begin_src matlab
|
||||
initializeGround();
|
||||
initializeGranite();
|
||||
initializeTy();
|
||||
initializeRy();
|
||||
initializeRz();
|
||||
initializeMicroHexapod();
|
||||
initializeAxisc();
|
||||
initializeMirror();
|
||||
initializeNanoHexapod('actuator', 'piezo');
|
||||
initializeSample('mass', 1);
|
||||
#+end_src
|
||||
|
||||
We initialize the reference path for all the stages.
|
||||
All stage is set to its zero position except the Spindle which is rotating at 60rpm.
|
||||
#+begin_src matlab
|
||||
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
|
||||
#+end_src
|
||||
|
||||
* Tomography Experiment with no disturbances
|
||||
<<sec:tomo_no_dist>>
|
||||
** Introduction :ignore:
|
||||
|
||||
** Simulation Setup
|
||||
And we initialize the disturbances to be equal to zero.
|
||||
#+begin_src matlab
|
||||
initializeDisturbances(...
|
||||
'Dwx', false, ... % Ground Motion - X direction
|
||||
'Dwy', false, ... % Ground Motion - Y direction
|
||||
'Dwz', false, ... % Ground Motion - Z direction
|
||||
'Fty_x', false, ... % Translation Stage - X direction
|
||||
'Fty_z', false, ... % Translation Stage - Z direction
|
||||
'Frz_z', false ... % Spindle - Z direction
|
||||
);
|
||||
#+end_src
|
||||
|
||||
We simulate the model.
|
||||
#+begin_src matlab
|
||||
sim('nass_model');
|
||||
#+end_src
|
||||
|
||||
And we save the obtained data.
|
||||
#+begin_src matlab
|
||||
tomo_align_no_dist = struct('t', t, 'MTr', MTr);
|
||||
save('experiment_tomography/mat/experiment.mat', 'tomo_align_no_dist', '-append');
|
||||
#+end_src
|
||||
|
||||
** Analysis
|
||||
#+begin_src matlab
|
||||
load('experiment_tomography/mat/experiment.mat', 'tomo_align_no_dist');
|
||||
t = tomo_align_no_dist.t;
|
||||
MTr = tomo_align_no_dist.MTr;
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
Edx = squeeze(MTr(1, 4, :));
|
||||
Edy = squeeze(MTr(2, 4, :));
|
||||
Edz = squeeze(MTr(3, 4, :));
|
||||
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
|
||||
Ery = atan2( squeeze(MTr(1, 3, :)), squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
|
||||
Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
|
||||
Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
ax1 = subplot(1, 3, 1);
|
||||
plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
|
||||
ylabel('Displacement [m]');
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax2 = subplot(1, 3, 2);
|
||||
plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
|
||||
xlabel('Time [s]');
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax3 = subplot(1, 3, 3);
|
||||
plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
|
||||
legend('location', 'northeast');
|
||||
|
||||
linkaxes([ax1,ax2,ax3],'x');
|
||||
xlim([2, inf]);
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/exp_tomo_without_dist_trans.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:exp_tomo_without_dist_trans
|
||||
#+CAPTION: X-Y-Z translation of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_tomo_without_dist_trans.png][png]], [[./figs/exp_tomo_without_dist_trans.pdf][pdf]])
|
||||
[[file:figs/exp_tomo_without_dist_trans.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
ax1 = subplot(1, 3, 1);
|
||||
plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
|
||||
ylabel('Rotation [rad]');
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax2 = subplot(1, 3, 2);
|
||||
plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
|
||||
xlabel('Time [s]');
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax3 = subplot(1, 3, 3);
|
||||
plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
|
||||
legend('location', 'northeast');
|
||||
|
||||
linkaxes([ax1,ax2,ax3],'x');
|
||||
xlim([2, inf]);
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/exp_tomo_without_dist_rot.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:exp_tomo_without_dist_rot
|
||||
#+CAPTION: X-Y-Z rotations of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_tomo_without_dist_rot.png][png]], [[./figs/exp_tomo_without_dist_rot.pdf][pdf]])
|
||||
[[file:figs/exp_tomo_without_dist_rot.png]]
|
||||
|
||||
** Conclusion
|
||||
|
||||
#+begin_important
|
||||
When everything is aligned, the resulting error motion is very small (nm range) and is quite negligible with respect to the error when disturbances are included.
|
||||
This residual error motion probably comes from a small misalignment somewhere.
|
||||
#+end_important
|
||||
|
||||
* Tomography Experiment with included perturbations
|
||||
<<sec:tomo_dist>>
|
||||
** Introduction :ignore:
|
||||
|
||||
** Simulation Setup
|
||||
We now activate the disturbances.
|
||||
#+begin_src matlab
|
||||
initializeDisturbances(...
|
||||
'Dwx', true, ... % Ground Motion - X direction
|
||||
'Dwy', true, ... % Ground Motion - Y direction
|
||||
'Dwz', true, ... % Ground Motion - Z direction
|
||||
'Fty_x', true, ... % Translation Stage - X direction
|
||||
'Fty_z', true, ... % Translation Stage - Z direction
|
||||
'Frz_z', true ... % Spindle - Z direction
|
||||
);
|
||||
#+end_src
|
||||
|
||||
We simulate the model.
|
||||
#+begin_src matlab
|
||||
sim('nass_model');
|
||||
#+end_src
|
||||
|
||||
And we save the obtained data.
|
||||
#+begin_src matlab
|
||||
tomo_align_dist = struct('t', t, 'MTr', MTr);
|
||||
save('experiment_tomography/mat/experiment.mat', 'tomo_align_dist', '-append');
|
||||
#+end_src
|
||||
|
||||
** Analysis
|
||||
#+begin_src matlab
|
||||
load('experiment_tomography/mat/experiment.mat', 'tomo_align_dist');
|
||||
t = tomo_align_dist.t;
|
||||
MTr = tomo_align_dist.MTr;
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
Edx = squeeze(MTr(1, 4, :));
|
||||
Edy = squeeze(MTr(2, 4, :));
|
||||
Edz = squeeze(MTr(3, 4, :));
|
||||
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
|
||||
Ery = atan2( squeeze(MTr(1, 3, :)), squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
|
||||
Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
|
||||
Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
ax1 = subplot(1, 3, 1);
|
||||
plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
|
||||
ylabel('Displacement [m]');
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax2 = subplot(1, 3, 2);
|
||||
plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
|
||||
xlabel('Time [s]');
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax3 = subplot(1, 3, 3);
|
||||
plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
|
||||
legend('location', 'northeast');
|
||||
|
||||
linkaxes([ax1,ax2,ax3],'x');
|
||||
xlim([2, inf]);
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/exp_tomo_dist_trans.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:exp_tomo_dist_trans
|
||||
#+CAPTION: X-Y-Z translation of the sample w.r.t. the granite when performing tomography experiment with disturbances ([[./figs/exp_tomo_dist_trans.png][png]], [[./figs/exp_tomo_dist_trans.pdf][pdf]])
|
||||
[[file:figs/exp_tomo_dist_trans.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
ax1 = subplot(1, 3, 1);
|
||||
plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
|
||||
ylabel('Rotation [rad]');
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax2 = subplot(1, 3, 2);
|
||||
plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
|
||||
xlabel('Time [s]');
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax3 = subplot(1, 3, 3);
|
||||
plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
|
||||
legend('location', 'northeast');
|
||||
|
||||
linkaxes([ax1,ax2,ax3],'x');
|
||||
xlim([2, inf]);
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/exp_tomo_dist_rot.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:exp_tomo_dist_rot
|
||||
#+CAPTION: X-Y-Z rotations of the sample w.r.t. the granite when performing tomography experiment with disturbances ([[./figs/exp_tomo_dist_rot.png][png]], [[./figs/exp_tomo_dist_rot.pdf][pdf]])
|
||||
[[file:figs/exp_tomo_dist_rot.png]]
|
||||
|
||||
** Conclusion
|
||||
|
||||
#+begin_important
|
||||
Error motion is what expected from the disturbance measurements.
|
||||
#+end_important
|
||||
|
||||
* Tomography when the micro-hexapod is not centered
|
||||
<<sec:tomo_hexa_trans>>
|
||||
** Introduction :ignore:
|
||||
|
||||
** Simulation Setup
|
||||
We first set the wanted translation of the Micro Hexapod.
|
||||
#+begin_src matlab
|
||||
P_micro_hexapod = [0.01; 0; 0]; % [m]
|
||||
#+end_src
|
||||
|
||||
We initialize the reference path.
|
||||
#+begin_src matlab
|
||||
initializeReferences('Dh_pos', [P_micro_hexapod; 0; 0; 0], 'Rz_type', 'rotating', 'Rz_period', 1);
|
||||
#+end_src
|
||||
|
||||
We initialize the stages.
|
||||
#+begin_src matlab
|
||||
initializeMicroHexapod('AP', P_micro_hexapod);
|
||||
#+end_src
|
||||
|
||||
And we initialize the disturbances to zero.
|
||||
#+begin_src matlab
|
||||
initializeDisturbances(...
|
||||
'Dwx', false, ... % Ground Motion - X direction
|
||||
'Dwy', false, ... % Ground Motion - Y direction
|
||||
'Dwz', false, ... % Ground Motion - Z direction
|
||||
'Fty_x', false, ... % Translation Stage - X direction
|
||||
'Fty_z', false, ... % Translation Stage - Z direction
|
||||
'Frz_z', false ... % Spindle - Z direction
|
||||
);
|
||||
#+end_src
|
||||
|
||||
We simulate the model.
|
||||
#+begin_src matlab
|
||||
sim('nass_model');
|
||||
#+end_src
|
||||
|
||||
And we save the obtained data.
|
||||
#+begin_src matlab
|
||||
tomo_not_align = struct('t', t, 'MTr', MTr);
|
||||
save('experiment_tomography/mat/experiment.mat', 'tomo_not_align', '-append');
|
||||
#+end_src
|
||||
|
||||
** Analysis
|
||||
#+begin_src matlab
|
||||
load('experiment_tomography/mat/experiment.mat', 'tomo_not_align');
|
||||
t = tomo_not_align.t;
|
||||
MTr = tomo_not_align.MTr;
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
Edx = squeeze(MTr(1, 4, :));
|
||||
Edy = squeeze(MTr(2, 4, :));
|
||||
Edz = squeeze(MTr(3, 4, :));
|
||||
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
|
||||
Ery = atan2( squeeze(MTr(1, 3, :)), squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
|
||||
Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
|
||||
Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
ax1 = subplot(1, 3, 1);
|
||||
plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
|
||||
ylabel('Displacement [m]');
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax2 = subplot(1, 3, 2);
|
||||
plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
|
||||
xlabel('Time [s]');
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax3 = subplot(1, 3, 3);
|
||||
plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
|
||||
legend('location', 'northeast');
|
||||
|
||||
linkaxes([ax1,ax2,ax3],'x');
|
||||
xlim([2, inf]);
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/exp_tomo_offset_trans.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:exp_tomo_offset_trans
|
||||
#+CAPTION: X-Y-Z translation of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_tomo_offset_trans.png][png]], [[./figs/exp_tomo_offset_trans.pdf][pdf]])
|
||||
[[file:figs/exp_tomo_offset_trans.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
ax1 = subplot(1, 3, 1);
|
||||
plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
|
||||
ylabel('Rotation [rad]');
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax2 = subplot(1, 3, 2);
|
||||
plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
|
||||
xlabel('Time [s]');
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax3 = subplot(1, 3, 3);
|
||||
plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
|
||||
legend('location', 'northeast');
|
||||
|
||||
linkaxes([ax1,ax2,ax3],'x');
|
||||
xlim([2, inf]);
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/exp_tomo_offset_rot.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:exp_tomo_offset_rot
|
||||
#+CAPTION: X-Y-Z rotations of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_tomo_offset_rot.png][png]], [[./figs/exp_tomo_offset_rot.pdf][pdf]])
|
||||
[[file:figs/exp_tomo_offset_rot.png]]
|
||||
|
||||
** Conclusion
|
||||
|
||||
#+begin_important
|
||||
The main motions are translations in the X direction of the mobile platform (corresponds to the eccentricity of the micro-hexapod) and rotations along the rotating Y axis.
|
||||
#+end_important
|
||||
|
||||
* Raster Scans with the translation stage
|
||||
<<sec:ty_scans>>
|
||||
** Introduction :ignore:
|
||||
|
||||
** Simulation Setup
|
||||
We set the reference path.
|
||||
#+begin_src matlab
|
||||
initializeReferences('Dy_type', 'triangular', 'Dy_amplitude', 10e-3, 'Dy_period', 1);
|
||||
#+end_src
|
||||
|
||||
We initialize the stages.
|
||||
#+begin_src matlab
|
||||
initializeGround();
|
||||
initializeGranite();
|
||||
initializeTy();
|
||||
initializeRy();
|
||||
initializeRz();
|
||||
initializeMicroHexapod();
|
||||
initializeAxisc();
|
||||
initializeMirror();
|
||||
initializeNanoHexapod('actuator', 'piezo');
|
||||
initializeSample('mass', 1);
|
||||
#+end_src
|
||||
|
||||
And we initialize the disturbances to zero.
|
||||
#+begin_src matlab
|
||||
initializeDisturbances(...
|
||||
'Dwx', false, ... % Ground Motion - X direction
|
||||
'Dwy', false, ... % Ground Motion - Y direction
|
||||
'Dwz', false, ... % Ground Motion - Z direction
|
||||
'Fty_x', false, ... % Translation Stage - X direction
|
||||
'Fty_z', false, ... % Translation Stage - Z direction
|
||||
'Frz_z', false ... % Spindle - Z direction
|
||||
);
|
||||
#+end_src
|
||||
|
||||
We simulate the model.
|
||||
#+begin_src matlab
|
||||
sim('nass_model');
|
||||
#+end_src
|
||||
|
||||
And we save the obtained data.
|
||||
#+begin_src matlab
|
||||
ty_scan = struct('t', t, 'MTr', MTr);
|
||||
save('experiment_tomography/mat/experiment.mat', 'ty_scan', '-append');
|
||||
#+end_src
|
||||
|
||||
** Analysis
|
||||
#+begin_src matlab
|
||||
load('experiment_tomography/mat/experiment.mat', 'ty_scan');
|
||||
t = ty_scan.t;
|
||||
MTr = ty_scan.MTr;
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
Edx = squeeze(MTr(1, 4, :));
|
||||
Edy = squeeze(MTr(2, 4, :));
|
||||
Edz = squeeze(MTr(3, 4, :));
|
||||
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
|
||||
Ery = atan2( squeeze(MTr(1, 3, :)), squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
|
||||
Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
|
||||
Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
ax1 = subplot(1, 3, 1);
|
||||
plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
|
||||
ylabel('Displacement [m]');
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax2 = subplot(1, 3, 2);
|
||||
plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
|
||||
xlabel('Time [s]');
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax3 = subplot(1, 3, 3);
|
||||
plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
|
||||
legend('location', 'northeast');
|
||||
|
||||
linkaxes([ax1,ax2,ax3],'x');
|
||||
xlim([2, inf]);
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/exp_ty_scan_trans.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:exp_ty_scan_trans
|
||||
#+CAPTION: X-Y-Z translation of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_ty_scan_trans.png][png]], [[./figs/exp_ty_scan_trans.pdf][pdf]])
|
||||
[[file:figs/exp_ty_scan_trans.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
ax1 = subplot(1, 3, 1);
|
||||
plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
|
||||
ylabel('Rotation [rad]');
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax2 = subplot(1, 3, 2);
|
||||
plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
|
||||
xlabel('Time [s]');
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax3 = subplot(1, 3, 3);
|
||||
plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
|
||||
legend('location', 'northeast');
|
||||
|
||||
linkaxes([ax1,ax2,ax3],'x');
|
||||
xlim([2, inf]);
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/exp_ty_scan_rot.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:exp_ty_scan_rot
|
||||
#+CAPTION: X-Y-Z rotations of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_ty_scan_rot.png][png]], [[./figs/exp_ty_scan_rot.pdf][pdf]])
|
||||
[[file:figs/exp_ty_scan_rot.png]]
|
||||
|
||||
** Conclusion
|
||||
|
||||
#+begin_important
|
||||
This is logic that the main error moving is translation along the Y axis and rotation along the X axis.
|
||||
In order to reduce the errors, we can make a smoother reference path for the translation stage.
|
||||
#+end_important
|
||||
405
org/functions.org
Normal file
405
org/functions.org
Normal file
@@ -0,0 +1,405 @@
|
||||
#+TITLE: Matlab Functions used for the NASS Project
|
||||
:DRAWER:
|
||||
#+STARTUP: overview
|
||||
|
||||
#+LANGUAGE: en
|
||||
#+EMAIL: dehaeze.thomas@gmail.com
|
||||
#+AUTHOR: Dehaeze Thomas
|
||||
|
||||
#+HTML_LINK_HOME: ../index.html
|
||||
#+HTML_LINK_UP: ../index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
||||
|
||||
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
#+PROPERTY: header-args:matlab+ :tangle no
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
|
||||
#+PROPERTY: header-args:shell :eval no-export
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* TODO computePsdDispl
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/computePsdDispl.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:computePsdDispl>>
|
||||
|
||||
This Matlab function is accessible [[file:../src/computePsdDispl.m][here]].
|
||||
|
||||
#+begin_src matlab
|
||||
function [psd_object] = computePsdDispl(sys_data, t_init, n_av)
|
||||
i_init = find(sys_data.time > t_init, 1);
|
||||
|
||||
han_win = hanning(ceil(length(sys_data.Dx(i_init:end, :))/n_av));
|
||||
Fs = 1/sys_data.time(2);
|
||||
|
||||
[pdx, f] = pwelch(sys_data.Dx(i_init:end, :), han_win, [], [], Fs);
|
||||
[pdy, ~] = pwelch(sys_data.Dy(i_init:end, :), han_win, [], [], Fs);
|
||||
[pdz, ~] = pwelch(sys_data.Dz(i_init:end, :), han_win, [], [], Fs);
|
||||
|
||||
[prx, ~] = pwelch(sys_data.Rx(i_init:end, :), han_win, [], [], Fs);
|
||||
[pry, ~] = pwelch(sys_data.Ry(i_init:end, :), han_win, [], [], Fs);
|
||||
[prz, ~] = pwelch(sys_data.Rz(i_init:end, :), han_win, [], [], Fs);
|
||||
|
||||
psd_object = struct(...
|
||||
'f', f, ...
|
||||
'dx', pdx, ...
|
||||
'dy', pdy, ...
|
||||
'dz', pdz, ...
|
||||
'rx', prx, ...
|
||||
'ry', pry, ...
|
||||
'rz', prz);
|
||||
end
|
||||
#+end_src
|
||||
|
||||
* TODO computeSetpoint
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/computeSetpoint.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:computeSetpoint>>
|
||||
|
||||
This Matlab function is accessible [[file:../src/computeSetpoint.m][here]].
|
||||
|
||||
#+begin_src matlab
|
||||
function setpoint = computeSetpoint(ty, ry, rz)
|
||||
%%
|
||||
setpoint = zeros(6, 1);
|
||||
|
||||
%% Ty
|
||||
Ty = [1 0 0 0 ;
|
||||
0 1 0 ty ;
|
||||
0 0 1 0 ;
|
||||
0 0 0 1 ];
|
||||
|
||||
% Tyinv = [1 0 0 0 ;
|
||||
% 0 1 0 -ty ;
|
||||
% 0 0 1 0 ;
|
||||
% 0 0 0 1 ];
|
||||
|
||||
%% Ry
|
||||
Ry = [ cos(ry) 0 sin(ry) 0 ;
|
||||
0 1 0 0 ;
|
||||
-sin(ry) 0 cos(ry) 0 ;
|
||||
0 0 0 1 ];
|
||||
|
||||
% TMry = Ty*Ry*Tyinv;
|
||||
|
||||
%% Rz
|
||||
Rz = [cos(rz) -sin(rz) 0 0 ;
|
||||
sin(rz) cos(rz) 0 0 ;
|
||||
0 0 1 0 ;
|
||||
0 0 0 1 ];
|
||||
|
||||
% TMrz = Ty*TMry*Rz*TMry'*Tyinv;
|
||||
|
||||
%% All stages
|
||||
% TM = TMrz*TMry*Ty;
|
||||
|
||||
TM = Ty*Ry*Rz;
|
||||
|
||||
[thetax, thetay, thetaz] = RM2angle(TM(1:3, 1:3));
|
||||
|
||||
setpoint(1:3) = TM(1:3, 4);
|
||||
setpoint(4:6) = [thetax, thetay, thetaz];
|
||||
|
||||
%% Custom Functions
|
||||
function [thetax, thetay, thetaz] = RM2angle(R)
|
||||
if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
|
||||
thetay = -asin(R(3, 1));
|
||||
thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
|
||||
thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
|
||||
else
|
||||
thetaz = 0;
|
||||
if abs(R(3, 1)+1) < 1e-6 % R31 = -1
|
||||
thetay = pi/2;
|
||||
thetax = thetaz + atan2(R(1, 2), R(1, 3));
|
||||
else
|
||||
thetay = -pi/2;
|
||||
thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
* TODO converErrorBasis
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/converErrorBasis.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:converErrorBasis>>
|
||||
|
||||
This Matlab function is accessible [[file:../src/converErrorBasis.m][here]].
|
||||
|
||||
#+begin_src matlab
|
||||
function error_nass = convertErrorBasis(pos, setpoint, ty, ry, rz)
|
||||
% convertErrorBasis -
|
||||
%
|
||||
% Syntax: convertErrorBasis(p_error, ty, ry, rz)
|
||||
%
|
||||
% Inputs:
|
||||
% - p_error - Position error of the sample w.r.t. the granite [m, rad]
|
||||
% - ty - Measured translation of the Ty stage [m]
|
||||
% - ry - Measured rotation of the Ry stage [rad]
|
||||
% - rz - Measured rotation of the Rz stage [rad]
|
||||
%
|
||||
% Outputs:
|
||||
% - P_nass - Position error of the sample w.r.t. the NASS base [m]
|
||||
% - R_nass - Rotation error of the sample w.r.t. the NASS base [rad]
|
||||
%
|
||||
% Example:
|
||||
%
|
||||
|
||||
%% If line vector => column vector
|
||||
if size(pos, 2) == 6
|
||||
pos = pos';
|
||||
end
|
||||
|
||||
if size(setpoint, 2) == 6
|
||||
setpoint = setpoint';
|
||||
end
|
||||
|
||||
%% Position of the sample in the frame fixed to the Granite
|
||||
P_granite = [pos(1:3); 1]; % Position [m]
|
||||
R_granite = [setpoint(1:3); 1]; % Reference [m]
|
||||
|
||||
%% Transformation matrices of the stages
|
||||
% T-y
|
||||
TMty = [1 0 0 0 ;
|
||||
0 1 0 ty ;
|
||||
0 0 1 0 ;
|
||||
0 0 0 1 ];
|
||||
|
||||
% R-y
|
||||
TMry = [ cos(ry) 0 sin(ry) 0 ;
|
||||
0 1 0 0 ;
|
||||
-sin(ry) 0 cos(ry) 0 ;
|
||||
0 0 0 1 ];
|
||||
|
||||
% R-z
|
||||
TMrz = [cos(rz) -sin(rz) 0 0 ;
|
||||
sin(rz) cos(rz) 0 0 ;
|
||||
0 0 1 0 ;
|
||||
0 0 0 1 ];
|
||||
|
||||
%% Compute Point coordinates in the new reference fixed to the NASS base
|
||||
% P_nass = TMrz*TMry*TMty*P_granite;
|
||||
P_nass = TMrz\TMry\TMty\P_granite;
|
||||
R_nass = TMrz\TMry\TMty\R_granite;
|
||||
|
||||
dx = R_nass(1)-P_nass(1);
|
||||
dy = R_nass(2)-P_nass(2);
|
||||
dz = R_nass(3)-P_nass(3);
|
||||
|
||||
%% Compute new basis vectors linked to the NASS base
|
||||
% ux_nass = TMrz*TMry*TMty*[1; 0; 0; 0];
|
||||
% ux_nass = ux_nass(1:3);
|
||||
% uy_nass = TMrz*TMry*TMty*[0; 1; 0; 0];
|
||||
% uy_nass = uy_nass(1:3);
|
||||
% uz_nass = TMrz*TMry*TMty*[0; 0; 1; 0];
|
||||
% uz_nass = uz_nass(1:3);
|
||||
|
||||
ux_nass = TMrz\TMry\TMty\[1; 0; 0; 0];
|
||||
ux_nass = ux_nass(1:3);
|
||||
uy_nass = TMrz\TMry\TMty\[0; 1; 0; 0];
|
||||
uy_nass = uy_nass(1:3);
|
||||
uz_nass = TMrz\TMry\TMty\[0; 0; 1; 0];
|
||||
uz_nass = uz_nass(1:3);
|
||||
|
||||
%% Rotations error w.r.t. granite Frame
|
||||
% Rotations error w.r.t. granite Frame
|
||||
rx_nass = pos(4);
|
||||
ry_nass = pos(5);
|
||||
rz_nass = pos(6);
|
||||
|
||||
% Rotation matrices of the Sample w.r.t. the Granite
|
||||
Mrx_error = [1 0 0 ;
|
||||
0 cos(-rx_nass) -sin(-rx_nass) ;
|
||||
0 sin(-rx_nass) cos(-rx_nass)];
|
||||
|
||||
Mry_error = [ cos(-ry_nass) 0 sin(-ry_nass) ;
|
||||
0 1 0 ;
|
||||
-sin(-ry_nass) 0 cos(-ry_nass)];
|
||||
|
||||
Mrz_error = [cos(-rz_nass) -sin(-rz_nass) 0 ;
|
||||
sin(-rz_nass) cos(-rz_nass) 0 ;
|
||||
0 0 1];
|
||||
|
||||
% Rotation matrix of the Sample w.r.t. the Granite
|
||||
Mr_error = Mrz_error*Mry_error*Mrx_error;
|
||||
|
||||
%% Use matrix to solve
|
||||
R = Mr_error/[ux_nass, uy_nass, uz_nass]; % Rotation matrix from NASS base to Sample
|
||||
|
||||
[thetax, thetay, thetaz] = RM2angle(R);
|
||||
|
||||
error_nass = [dx; dy; dz; thetax; thetay; thetaz];
|
||||
|
||||
%% Custom Functions
|
||||
function [thetax, thetay, thetaz] = RM2angle(R)
|
||||
if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
|
||||
thetay = -asin(R(3, 1));
|
||||
% thetaybis = pi-thetay;
|
||||
thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
|
||||
% thetaxbis = atan2(R(3, 2)/cos(thetaybis), R(3, 3)/cos(thetaybis));
|
||||
thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
|
||||
% thetazbis = atan2(R(2, 1)/cos(thetaybis), R(1, 1)/cos(thetaybis));
|
||||
else
|
||||
thetaz = 0;
|
||||
if abs(R(3, 1)+1) < 1e-6 % R31 = -1
|
||||
thetay = pi/2;
|
||||
thetax = thetaz + atan2(R(1, 2), R(1, 3));
|
||||
else
|
||||
thetay = -pi/2;
|
||||
thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
#+end_src
|
||||
|
||||
* computeReferencePose
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/computeReferencePose.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:computeReferencePose>>
|
||||
|
||||
This Matlab function is accessible [[file:src/computeReferencePose.m][here]].
|
||||
|
||||
#+begin_src matlab
|
||||
function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
|
||||
% computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
|
||||
%
|
||||
% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
|
||||
%
|
||||
% Inputs:
|
||||
% - Dy - Reference of the Translation Stage [m]
|
||||
% - Ry - Reference of the Tilt Stage [rad]
|
||||
% - Rz - Reference of the Spindle [rad]
|
||||
% - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad]
|
||||
% - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad]
|
||||
%
|
||||
% 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) = Rhz*Rhy*Rhx;
|
||||
|
||||
%% Nano-Hexapod
|
||||
Rnx = [1 0 0;
|
||||
0 cos(Dn(4)) -sin(Dn(4));
|
||||
0 sin(Dn(4)) cos(Dn(4))];
|
||||
|
||||
Rny = [ cos(Dn(5)) 0 sin(Dn(5));
|
||||
0 1 0;
|
||||
-sin(Dn(5)) 0 cos(Dn(5))];
|
||||
|
||||
Rnz = [cos(Dn(6)) -sin(Dn(6)) 0;
|
||||
sin(Dn(6)) cos(Dn(6)) 0;
|
||||
0 0 1];
|
||||
|
||||
Rn = [1 0 0 Dn(1) ;
|
||||
0 1 0 Dn(2) ;
|
||||
0 0 1 Dn(3) ;
|
||||
0 0 0 1 ];
|
||||
|
||||
Rn(1:3, 1:3) = Rnz*Rny*Rnx;
|
||||
|
||||
%% Total Homogeneous transformation
|
||||
WTr = Rty*Rry*Rrz*Rh*Rn;
|
||||
end
|
||||
#+end_src
|
||||
* Compute the Sample Position Error w.r.t. the NASS
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/computeSampleError.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:computeSampleError>>
|
||||
|
||||
This Matlab function is accessible [[file:src/computeSampleError.m][here]].
|
||||
|
||||
#+begin_src matlab
|
||||
function [MTr] = computeSampleError(WTm, WTr)
|
||||
% computeSampleError -
|
||||
%
|
||||
% Syntax: [MTr] = computeSampleError(WTm, WTr)
|
||||
%
|
||||
% Inputs:
|
||||
% - WTm - Homoegeneous transformation that represent the
|
||||
% wanted pose of the sample with respect to the granite
|
||||
% - WTr - Homoegeneous transformation that represent the
|
||||
% measured pose of the sample with respect to the granite
|
||||
%
|
||||
% Outputs:
|
||||
% - MTr - Homoegeneous transformation that represent the
|
||||
% wanted pose of the sample expressed in a frame
|
||||
% attached to the top platform of the nano-hexapod
|
||||
|
||||
MTr = zeros(4,4);
|
||||
|
||||
MTr = [WTm(1:3,1:3)', -WTm(1:3,1:3)'*WTm(1:3,4) ; 0 0 0 1]*WTr;
|
||||
end
|
||||
#+end_src
|
||||
632
org/hac_lac.org
Normal file
632
org/hac_lac.org
Normal file
@@ -0,0 +1,632 @@
|
||||
#+TITLE: HAC-LAC applied on the Simscape Model
|
||||
:DRAWER:
|
||||
#+STARTUP: overview
|
||||
|
||||
#+LANGUAGE: en
|
||||
#+EMAIL: dehaeze.thomas@gmail.com
|
||||
#+AUTHOR: Dehaeze Thomas
|
||||
|
||||
#+HTML_LINK_HOME: ../index.html
|
||||
#+HTML_LINK_UP: ../index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
||||
|
||||
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
#+PROPERTY: header-args:matlab+ :tangle no
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
|
||||
#+PROPERTY: header-args:shell :eval no-export
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Undamped System
|
||||
<<sec:undamped_system>>
|
||||
|
||||
** Introduction :ignore:
|
||||
|
||||
** 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('nass_model.slx')
|
||||
#+end_src
|
||||
|
||||
** Identification of the plant
|
||||
*** Initialize the Simulation
|
||||
We initialize all the stages with the default parameters.
|
||||
#+begin_src matlab
|
||||
initializeGround();
|
||||
initializeGranite();
|
||||
initializeTy();
|
||||
initializeRy();
|
||||
initializeRz();
|
||||
initializeMicroHexapod();
|
||||
initializeAxisc();
|
||||
initializeMirror();
|
||||
#+end_src
|
||||
|
||||
The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
|
||||
#+begin_src matlab
|
||||
initializeNanoHexapod('actuator', 'piezo');
|
||||
initializeSample('mass', 50);
|
||||
#+end_src
|
||||
|
||||
No disturbances.
|
||||
#+begin_src matlab
|
||||
initializeDisturbances('enable', false);
|
||||
#+end_src
|
||||
|
||||
We set the references to zero.
|
||||
#+begin_src matlab
|
||||
initializeReferences();
|
||||
#+end_src
|
||||
|
||||
And all the controllers are set to 0.
|
||||
#+begin_src matlab
|
||||
initializeController('type', 'open-loop');
|
||||
#+end_src
|
||||
|
||||
*** Identification
|
||||
First, we identify the dynamics of the system using the =linearize= function.
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
||||
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En'); io_i = io_i + 1; % Metrology Outputs
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
load('mat/stages.mat', 'nano_hexapod');
|
||||
G_cart = minreal(G*inv(nano_hexapod.J'));
|
||||
G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
G_legs = minreal(inv(nano_hexapod.J)*G);
|
||||
G_legs.OutputName = {'e1', 'e2', 'e3', 'e4', 'e5', 'e6'};
|
||||
#+end_src
|
||||
|
||||
*** Display TF
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, abs(squeeze(freqresp(G_cart(i, i), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart(i, i), freqs, 'Hz'))), 'DisplayName', [G_cart.InputName{i}, ' to ', G_cart.OutputName{i}]);
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
legend('location', 'southwest');
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/plant_G_cart.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:plant_G_cart
|
||||
#+CAPTION: Transfer Function from forces applied by the nano-hexapod to position error ([[./figs/plant_G_cart.png][png]], [[./figs/plant_G_cart.pdf][pdf]])
|
||||
[[file:figs/plant_G_cart.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, abs(squeeze(freqresp(G_legs(['e', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_legs(['e', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
*** Obtained Plants for Active Damping
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/nass_active_damping_iff_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:nass_active_damping_iff_plant
|
||||
#+CAPTION: =G_iff=: IFF Plant ([[./figs/nass_active_damping_iff_plant.png][png]], [[./figs/nass_active_damping_iff_plant.pdf][pdf]])
|
||||
[[file:figs/nass_active_damping_iff_plant.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, abs(squeeze(freqresp(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/nass_active_damping_dvf_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:nass_active_damping_dvf_plant
|
||||
#+CAPTION: =G_dvf=: Plant for Direct Velocity Feedback ([[./figs/nass_active_damping_dvf_plant.png][png]], [[./figs/nass_active_damping_dvf_plant.pdf][pdf]])
|
||||
[[file:figs/nass_active_damping_ine_plant.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, abs(squeeze(freqresp(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/nass_active_damping_inertial_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:nass_active_damping_inertial_plant
|
||||
#+CAPTION: Inertial Feedback Plant ([[./figs/nass_active_damping_inertial_plant.png][png]], [[./figs/nass_active_damping_inertial_plant.pdf][pdf]])
|
||||
[[file:figs/nass_active_damping_inertial_plant.png]]
|
||||
|
||||
** Tomography Experiment
|
||||
*** Simulation
|
||||
We initialize elements for the tomography experiment.
|
||||
#+begin_src matlab
|
||||
prepareTomographyExperiment();
|
||||
#+end_src
|
||||
|
||||
We change the simulation stop time.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '3');
|
||||
#+end_src
|
||||
|
||||
And we simulate the system.
|
||||
#+begin_src matlab
|
||||
sim('nass_model');
|
||||
#+end_src
|
||||
|
||||
Finally, we save the simulation results for further analysis
|
||||
#+begin_src matlab
|
||||
save('./active_damping/mat/tomo_exp.mat', 'En', 'Eg', '-append');
|
||||
#+end_src
|
||||
|
||||
*** Results
|
||||
We load the results of tomography experiments.
|
||||
#+begin_src matlab
|
||||
load('./active_damping/mat/tomo_exp.mat', 'En');
|
||||
t = linspace(0, 3, length(En(:,1)));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
hold on;
|
||||
plot(t, En(:,1), 'DisplayName', '$\epsilon_{x}$')
|
||||
plot(t, En(:,2), 'DisplayName', '$\epsilon_{y}$')
|
||||
plot(t, En(:,3), 'DisplayName', '$\epsilon_{z}$')
|
||||
hold off;
|
||||
legend();
|
||||
xlabel('Time [s]'); ylabel('Position Error [m]');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/nass_act_damp_undamped_sim_tomo_trans.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:nass_act_damp_undamped_sim_tomo_trans
|
||||
#+CAPTION: Position Error during tomography experiment - Translations ([[./figs/nass_act_damp_undamped_sim_tomo_trans.png][png]], [[./figs/nass_act_damp_undamped_sim_tomo_trans.pdf][pdf]])
|
||||
[[file:figs/nass_act_damp_undamped_sim_tomo_trans.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
hold on;
|
||||
plot(t, En(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
|
||||
plot(t, En(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
|
||||
plot(t, En(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
|
||||
hold off;
|
||||
xlim([0.5,inf]);
|
||||
legend();
|
||||
xlabel('Time [s]'); ylabel('Position Error [rad]');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/nass_act_damp_undamped_sim_tomo_rot.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:nass_act_damp_undamped_sim_tomo_rot
|
||||
#+CAPTION: Position Error during tomography experiment - Rotations ([[./figs/nass_act_damp_undamped_sim_tomo_rot.png][png]], [[./figs/nass_act_damp_undamped_sim_tomo_rot.pdf][pdf]])
|
||||
[[file:figs/nass_act_damp_undamped_sim_tomo_rot.png]]
|
||||
|
||||
** Verification of the transfer function from nano hexapod to metrology
|
||||
*** Initialize the Simulation
|
||||
We initialize all the stages with the default parameters.
|
||||
#+begin_src matlab
|
||||
initializeGround();
|
||||
initializeGranite();
|
||||
initializeTy();
|
||||
initializeRy();
|
||||
initializeRz();
|
||||
initializeMicroHexapod();
|
||||
initializeAxisc();
|
||||
initializeMirror();
|
||||
#+end_src
|
||||
|
||||
The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
|
||||
#+begin_src matlab
|
||||
initializeNanoHexapod('actuator', 'piezo');
|
||||
initializeSample('mass', 50);
|
||||
#+end_src
|
||||
|
||||
No disturbances.
|
||||
#+begin_src matlab
|
||||
initializeDisturbances('enable', false);
|
||||
#+end_src
|
||||
|
||||
We set the references to zero.
|
||||
#+begin_src matlab
|
||||
initializeReferences();
|
||||
#+end_src
|
||||
|
||||
And all the controllers are set to 0.
|
||||
#+begin_src matlab
|
||||
initializeController('type', 'open-loop');
|
||||
#+end_src
|
||||
|
||||
*** Identification
|
||||
First, we identify the dynamics of the system using the =linearize= function.
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
||||
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En'); io_i = io_i + 1; % Metrology Outputs
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
load('mat/stages.mat', 'nano_hexapod');
|
||||
G_cart = minreal(G*inv(nano_hexapod.J'));
|
||||
G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
G_legs = minreal(inv(nano_hexapod.J)*G);
|
||||
G_legs.OutputName = {'e1', 'e2', 'e3', 'e4', 'e5', 'e6'};
|
||||
#+end_src
|
||||
|
||||
# And we save them for further analysis.
|
||||
# #+begin_src matlab
|
||||
# save('./hac_lac/mat/undamped_plant.mat', 'G');
|
||||
# #+end_src
|
||||
|
||||
*** Display TF
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, abs(squeeze(freqresp(G_cart(i, i), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart(i, i), freqs, 'Hz'))), 'DisplayName', [G_cart.InputName{i}, ' to ', G_cart.OutputName{i}]);
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
legend();
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/plant_G_cart.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:plant_G_cart
|
||||
#+CAPTION: Transfer Function from forces applied by the nano-hexapod to position error ([[./figs/plant_G_cart.png][png]], [[./figs/plant_G_cart.pdf][pdf]])
|
||||
[[file:figs/plant_G_cart.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, abs(squeeze(freqresp(G_legs(['e', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_legs(['e', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
*** Obtained Plants for Active Damping
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/nass_active_damping_iff_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:nass_active_damping_iff_plant
|
||||
#+CAPTION: =G_iff=: IFF Plant ([[./figs/nass_active_damping_iff_plant.png][png]], [[./figs/nass_active_damping_iff_plant.pdf][pdf]])
|
||||
[[file:figs/nass_active_damping_iff_plant.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, abs(squeeze(freqresp(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/nass_active_damping_dvf_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:nass_active_damping_dvf_plant
|
||||
#+CAPTION: =G_dvf=: Plant for Direct Velocity Feedback ([[./figs/nass_active_damping_dvf_plant.png][png]], [[./figs/nass_active_damping_dvf_plant.pdf][pdf]])
|
||||
[[file:figs/nass_active_damping_ine_plant.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, abs(squeeze(freqresp(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/nass_active_damping_inertial_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:nass_active_damping_inertial_plant
|
||||
#+CAPTION: Inertial Feedback Plant ([[./figs/nass_active_damping_inertial_plant.png][png]], [[./figs/nass_active_damping_inertial_plant.pdf][pdf]])
|
||||
[[file:figs/nass_active_damping_inertial_plant.png]]
|
||||
592
org/identification.org
Normal file
592
org/identification.org
Normal file
@@ -0,0 +1,592 @@
|
||||
#+TITLE: Identification
|
||||
:DRAWER:
|
||||
#+STARTUP: overview
|
||||
|
||||
#+LANGUAGE: en
|
||||
#+EMAIL: dehaeze.thomas@gmail.com
|
||||
#+AUTHOR: Dehaeze Thomas
|
||||
|
||||
#+HTML_LINK_HOME: ../index.html
|
||||
#+HTML_LINK_UP: ../index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
||||
|
||||
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
#+PROPERTY: header-args:matlab+ :tangle matlab/modal_frf_coh.m
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
|
||||
#+PROPERTY: header-args:shell :eval no-export
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
The goal here is to tune the Simscape model of the station in order to have a good dynamical representation of the real system.
|
||||
|
||||
In order to do so, we reproduce the Modal Analysis done on the station using the Simscape model.
|
||||
|
||||
We can then compare the measured Frequency Response Functions with the identified dynamics of the model.
|
||||
|
||||
Finally, this should help to tune the parameters of the model such that the dynamics is closer to the measured FRF.
|
||||
|
||||
* Identification of the Micro-Station :noexport:
|
||||
** Introduction :ignore:
|
||||
|
||||
** 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
|
||||
|
||||
** Simscape Model
|
||||
The simulink file for the identification is =sim_micro_station_id.slx=.
|
||||
#+begin_src matlab
|
||||
open('identification/matlab/sim_micro_station_id.slx')
|
||||
#+end_src
|
||||
|
||||
We load the configuration and we set a small =StopTime=.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '0.5');
|
||||
#+end_src
|
||||
|
||||
We initialize all the stages.
|
||||
#+begin_src matlab
|
||||
initializeGround();
|
||||
initializeGranite();
|
||||
initializeTy();
|
||||
initializeRy();
|
||||
initializeRz();
|
||||
initializeMicroHexapod();
|
||||
initializeAxisc();
|
||||
initializeMirror();
|
||||
initializeNanoHexapod('actuator', 'piezo');
|
||||
initializeSample('mass', 50);
|
||||
#+end_src
|
||||
|
||||
** Compute the transfer functions
|
||||
We first define some parameters for the identification.
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'sim_micro_station_id';
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
%% Micro-Hexapod
|
||||
% Input/Output definition
|
||||
io(1) = linio([mdl, '/Micro-Station/Fm_ext'],1,'openinput');
|
||||
io(2) = linio([mdl, '/Micro-Station/Fg_ext'],1,'openinput');
|
||||
io(3) = linio([mdl, '/Micro-Station/Dm_inertial'],1,'output');
|
||||
io(4) = linio([mdl, '/Micro-Station/Ty_inertial'],1,'output');
|
||||
io(5) = linio([mdl, '/Micro-Station/Ry_inertial'],1,'output');
|
||||
io(6) = linio([mdl, '/Micro-Station/Dg_inertial'],1,'output');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
% Run the linearization
|
||||
G_ms = linearize(mdl, io, 0);
|
||||
|
||||
% Input/Output names
|
||||
G_ms.InputName = {'Fmx', 'Fmy', 'Fmz',...
|
||||
'Fgx', 'Fgy', 'Fgz'};
|
||||
G_ms.OutputName = {'Dmx', 'Dmy', 'Dmz', ...
|
||||
'Tyx', 'Tyy', 'Tyz', ...
|
||||
'Ryx', 'Ryy', 'Ryz', ...
|
||||
'Dgx', 'Dgy', 'Dgz'};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
%% Save the obtained transfer functions
|
||||
save('./mat/id_micro_station.mat', 'G_ms');
|
||||
#+end_src
|
||||
|
||||
** Plots the transfer functions
|
||||
|
||||
** Compare with the measurements
|
||||
|
||||
* Modal Analysis of the Micro-Station :noexport:
|
||||
** 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
|
||||
|
||||
** Simscape Model
|
||||
The simulink file for the analysis is =sim_micro_station_modal_analysis.slx=.
|
||||
#+begin_src matlab
|
||||
open('identification/matlab/sim_micro_station_modal_analysis.slx')
|
||||
#+end_src
|
||||
|
||||
We load the configuration and we set a small =StopTime=.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '0.5');
|
||||
#+end_src
|
||||
|
||||
We initialize all the stages.
|
||||
#+begin_src matlab
|
||||
initializeGround();
|
||||
initializeGranite();
|
||||
initializeTy();
|
||||
initializeRy();
|
||||
initializeRz();
|
||||
initializeMicroHexapod();
|
||||
initializeAxisc();
|
||||
initializeMirror();
|
||||
initializeNanoHexapod('actuator', 'piezo');
|
||||
initializeSample('mass', 50);
|
||||
#+end_src
|
||||
|
||||
** Identification
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'sim_micro_station_modal_analysis';
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
%% Micro-Hexapod
|
||||
% Input/Output definition
|
||||
io(1) = linio([mdl, '/Micro-Station/F_hammer'],1,'openinput');
|
||||
io(2) = linio([mdl, '/Micro-Station/acc9'],1,'output');
|
||||
io(3) = linio([mdl, '/Micro-Station/acc10'],1,'output');
|
||||
io(4) = linio([mdl, '/Micro-Station/acc11'],1,'output');
|
||||
io(5) = linio([mdl, '/Micro-Station/acc12'],1,'output');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
% Run the linearization
|
||||
G_ms = linearize(mdl, io, 0);
|
||||
|
||||
% Input/Output names
|
||||
G_ms.InputName = {'Fx', 'Fy', 'Fz'};
|
||||
G_ms.OutputName = {'x9', 'y9', 'z9', ...
|
||||
'x10', 'y10', 'z10', ...
|
||||
'x11', 'y11', 'z11', ...
|
||||
'x12', 'y12', 'z12'};
|
||||
#+end_src
|
||||
|
||||
** Plot Results
|
||||
#+begin_src matlab
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_ms('x9', 'Fx'), freqs, 'Hz'))));
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]');
|
||||
hold off;
|
||||
#+end_src
|
||||
|
||||
** Compare with measurements
|
||||
#+begin_src matlab
|
||||
load('../meas/modal-analysis/mat/frf_coh_matrices.mat', 'FRFs', 'COHs', 'freqs');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
dirs = {'x', 'y', 'z'};
|
||||
|
||||
n_acc = 9;
|
||||
n_dir = 1; % x, y, z
|
||||
n_exc = 1; % x, y, z
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(FRFs(3*(n_acc-1) + n_dir, n_exc, :)))./((2*pi*freqs).^2)');
|
||||
plot(freqs, abs(squeeze(freqresp(G_ms([dirs{n_dir}, num2str(n_acc)], ['F', dirs{n_dir}]), freqs, 'Hz'))));
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]');
|
||||
hold off;
|
||||
#+end_src
|
||||
|
||||
* Some notes about the Simscape Model
|
||||
The Simscape Model of the micro-station consists of several solid bodies:
|
||||
- Bottom Granite
|
||||
- Top Granite
|
||||
- Translation Stage
|
||||
- Tilt Stage
|
||||
- Spindle
|
||||
- Hexapod
|
||||
|
||||
Each solid body has some characteristics: Center of Mass, mass, moment of inertia, etc...
|
||||
These parameters are automatically computed from the geometry and from the density of the materials.
|
||||
|
||||
Then, the solid bodies are connected with springs and dampers.
|
||||
Some of the springs and dampers values can be estimated from the joints/stages specifications, however, we here prefer to tune these values based on the measurements.
|
||||
|
||||
* Compare with measurements at the CoM of each element
|
||||
** Introduction :ignore:
|
||||
[[file:../../meas/modal-analysis/index.org][here]]
|
||||
|
||||
** 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
|
||||
|
||||
** Prepare the Simulation
|
||||
#+begin_src matlab
|
||||
open('nass_model.slx')
|
||||
#+end_src
|
||||
|
||||
We load the configuration.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simulink.mat');
|
||||
#+end_src
|
||||
|
||||
We set a small =StopTime=.
|
||||
#+begin_src matlab
|
||||
set_param(conf_simulink, 'StopTime', '0.5');
|
||||
#+end_src
|
||||
|
||||
We initialize all the stages.
|
||||
#+begin_src matlab
|
||||
initializeGround( 'type', 'rigid');
|
||||
initializeGranite( 'type', 'modal-analysis');
|
||||
initializeTy( 'type', 'modal-analysis');
|
||||
initializeRy( 'type', 'modal-analysis');
|
||||
initializeRz( 'type', 'modal-analysis');
|
||||
initializeMicroHexapod('type', 'modal-analysis');
|
||||
initializeAxisc( 'type', 'flexible');
|
||||
|
||||
initializeMirror( 'type', 'none');
|
||||
initializeNanoHexapod( 'type', 'none');
|
||||
initializeSample( 'type', 'none');
|
||||
|
||||
initializeController( 'type', 'open-loop');
|
||||
|
||||
initializeLoggingConfiguration('log', 'none');
|
||||
|
||||
initializeReferences();
|
||||
initializeDisturbances('enable', false);
|
||||
#+end_src
|
||||
|
||||
** Estimate the position of the CoM of each solid and compare with the one took for the Measurement Analysis
|
||||
Thanks to the [[https://fr.mathworks.com/help/physmod/sm/ref/inertiasensor.html][Inertia Sensor]] simscape block, it is possible to estimate the position of the Center of Mass of a solid body with respect to a defined frame.
|
||||
|
||||
#+begin_src matlab
|
||||
sim('nass_model')
|
||||
#+end_src
|
||||
|
||||
The results are shown in the table [[tab:com_simscape]].
|
||||
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||||
stages_com = 1e3*[granite_bot_com.Data(end, :) ;
|
||||
granite_top_com.Data(end, :) ;
|
||||
ty_com.Data(end, :) ;
|
||||
ry_com.Data(end, :) ;
|
||||
rz_com.Data(end, :) ;
|
||||
hexa_com.Data(end, :) ]';
|
||||
|
||||
data2orgtable(stages_com, {'X [mm]', 'Y [mm]', 'Z [mm]'}, {'granite bot', 'granite top', 'ty', 'ry', 'rz', 'hexa'}, ' %.1f ');
|
||||
#+end_src
|
||||
|
||||
#+name: tab:com_simscape
|
||||
#+caption: Center of Mass of each solid body as defined in Simscape
|
||||
#+RESULTS:
|
||||
| | granite bot | granite top | ty | ry | rz | hexa |
|
||||
|--------+-------------+-------------+--------+--------+--------+--------|
|
||||
| X [mm] | 52.4 | 51.7 | 0.9 | -0.1 | 0.0 | -0.0 |
|
||||
| Y [mm] | 190.4 | 263.2 | 0.7 | 5.2 | -0.0 | 0.1 |
|
||||
| Z [mm] | -1200.0 | -777.1 | -598.9 | -627.7 | -643.2 | -317.1 |
|
||||
|
||||
We can compare the obtained center of mass (table [[tab:com_simscape]]) with the one used for the Modal Analysis shown in table [[tab:com_solidworks]].
|
||||
|
||||
#+name: tab:com_solidworks
|
||||
#+caption: Estimated Center of Mass of each solid body using Solidworks
|
||||
| | granite bot | granite top | ty | ry | rz | hexa |
|
||||
|--------+-------------+-------------+------+------+------+------|
|
||||
| X [mm] | 45 | 52 | 0 | 0 | 0 | -4 |
|
||||
| Y [mm] | 144 | 258 | 14 | -5 | 0 | 6 |
|
||||
| Z [mm] | -1251 | -778 | -600 | -628 | -580 | -319 |
|
||||
|
||||
The results are quite similar.
|
||||
The differences can be explained by some differences in the chosen density of the materials or by the fact that not exactly all the same elements have been chosen for each stage.
|
||||
|
||||
For instance, on simscape, the fixed part of the translation stage counts for the top granite solid body.
|
||||
However, in SolidWorks, this has probably not be included with the top granite.
|
||||
|
||||
** Create a frame at the CoM of each solid body
|
||||
Now we use one =inertiasensor= block connected on each solid body that measured the center of mass of this solid with respect to the same connected frame.
|
||||
|
||||
We do that in order to position an accelerometer on the Simscape model at this particular point.
|
||||
|
||||
#+begin_src matlab
|
||||
open('identification/matlab/sim_micro_station_com_estimation.slx')
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
sim('sim_micro_station_com_estimation')
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||||
stages_com = 1e3*[granite_bot_com.Data(end, :) ;
|
||||
granite_top_com.Data(end, :) ;
|
||||
ty_com.Data(end, :) ;
|
||||
ry_com.Data(end, :) ;
|
||||
rz_com.Data(end, :) ;
|
||||
hexa_com.Data(end, :) ]';
|
||||
|
||||
data2orgtable(stages_com, {'X [mm]', 'Y [mm]', 'Z [mm]'}, {'granite bot', 'granite top', 'ty', 'ry', 'rz', 'hexa'}, ' %.1f ');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
| | granite bot | granite top | ty | ry | rz | hexa |
|
||||
|--------+-------------+-------------+-------+--------+-------+-------|
|
||||
| X [mm] | 0.0 | 51.7 | 0.9 | -0.1 | 0.0 | -0.0 |
|
||||
| Y [mm] | 0.0 | 753.2 | 0.7 | 5.2 | -0.0 | 0.1 |
|
||||
| Z [mm] | -250.0 | 22.9 | -17.1 | -146.5 | -23.2 | -47.1 |
|
||||
|
||||
We now same this for further use:
|
||||
#+begin_src matlab
|
||||
granite_bot_com = granite_bot_com.Data(end, :)';
|
||||
granite_top_com = granite_top_com.Data(end, :)';
|
||||
ty_com = ty_com.Data(end, :)';
|
||||
ry_com = ry_com.Data(end, :)';
|
||||
rz_com = rz_com.Data(end, :)';
|
||||
hexa_com = hexa_com.Data(end, :)';
|
||||
|
||||
save('mat/solids_com.mat', 'granite_bot_com', 'granite_top_com', 'ty_com', 'ry_com', 'rz_com', 'hexa_com');
|
||||
#+end_src
|
||||
|
||||
Then, we use the obtained results to add a =rigidTransform= block in order to create a new frame at the center of mass of each solid body.
|
||||
|
||||
** Identification of the dynamics of the Simscape Model
|
||||
We now use a new Simscape Model where 6DoF inertial sensors are located at the Center of Mass of each solid body.
|
||||
|
||||
#+begin_src matlab
|
||||
% load('mat/solids_com.mat', 'granite_bot_com', 'granite_top_com', 'ty_com', 'ry_com', 'rz_com', 'hexa_com');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
open('nass_model.slx')
|
||||
#+end_src
|
||||
|
||||
We use the =linearize= function in order to estimate the dynamics from forces applied on the Translation stage at the same position used for the real modal analysis to the inertial sensors.
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Micro-Station/Translation Stage/Modal Analysis/F_hammer'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Micro-Station/Granite/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Micro-Station/Translation Stage/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Micro-Station/Tilt Stage/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Micro-Station/Spindle/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
% Run the linearization
|
||||
G_ms = linearize(mdl, io, 0);
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
G_ms.InputName = {'Fx', 'Fy', 'Fz'};
|
||||
G_ms.OutputName = {'gtop_x', 'gtop_y', 'gtop_z', 'gtop_rx', 'gtop_ry', 'gtop_rz', ...
|
||||
'ty_x', 'ty_y', 'ty_z', 'ty_rx', 'ty_ry', 'ty_rz', ...
|
||||
'ry_x', 'ry_y', 'ry_z', 'ry_rx', 'ry_ry', 'ry_rz', ...
|
||||
'rz_x', 'rz_y', 'rz_z', 'rz_rx', 'rz_ry', 'rz_rz', ...
|
||||
'hexa_x', 'hexa_y', 'hexa_z', 'hexa_rx', 'hexa_ry', 'hexa_rz'};
|
||||
#+end_src
|
||||
|
||||
The output of =G_ms= is the acceleration of each solid body.
|
||||
In order to obtain a displacement, we divide the obtained transfer function by $1/s^{2}$;
|
||||
#+begin_src matlab
|
||||
G_ms = G_ms/s^2;
|
||||
#+end_src
|
||||
|
||||
** Compare with measurements
|
||||
We now load the Frequency Response Functions measurements during the Modal Analysis (accessible [[file:../../meas/modal-analysis/index.org][here]]).
|
||||
|
||||
#+begin_src matlab
|
||||
load('../meas/modal-analysis/mat/frf_coh_matrices.mat', 'freqs');
|
||||
load('../meas/modal-analysis/mat/frf_com.mat', 'FRFs_CoM');
|
||||
#+end_src
|
||||
|
||||
We then compare the measurements with the identified transfer functions using the Simscape Model.
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
|
||||
stages = {'gbot', 'gtop', 'ty', 'ry', 'rz', 'hexa'}
|
||||
|
||||
n_stg = 3;
|
||||
n_dir = 6; % x, y, z, Rx, Ry, Rz
|
||||
n_exc = 2; % x, y, z
|
||||
|
||||
f = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(FRFs_CoM(6*(n_stg-1) + n_dir, n_exc, :)))./((2*pi*freqs).^2)');
|
||||
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_exc}]), f, 'Hz'))));
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]');
|
||||
hold off;
|
||||
xlim([1, 200]);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
|
||||
stages = {'gtop', 'ty', 'ry', 'rz', 'hexa'}
|
||||
|
||||
f = logspace(1, 3, 1000);
|
||||
|
||||
figure;
|
||||
for n_stg = 1:2
|
||||
for n_dir = 1:3
|
||||
subplot(3, 2, (n_dir-1)*2 + n_stg);
|
||||
title(['F ', dirs{n_dir}, ' to ', stages{n_stg}, ' ', dirs{n_dir}]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(FRFs_CoM(6*(n_stg) + n_dir, n_dir, :)))./((2*pi*freqs).^2)');
|
||||
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_dir}]), f, 'Hz'))));
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]');
|
||||
if n_dir == 3
|
||||
xlabel('Frequency [Hz]');
|
||||
end
|
||||
hold off;
|
||||
xlim([10, 1000]);
|
||||
ylim([1e-12, 1e-6]);
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/identification_comp_bot_stages.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:identification_comp_bot_stages
|
||||
#+CAPTION: caption ([[./figs/identification_comp_bot_stages.png][png]], [[./figs/identification_comp_bot_stages.pdf][pdf]])
|
||||
[[file:figs/identification_comp_bot_stages.png]]
|
||||
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
|
||||
stages = {'ry', 'rz', 'hexa'}
|
||||
|
||||
f = logspace(1, 3, 1000);
|
||||
|
||||
figure;
|
||||
for n_stg = 1:2
|
||||
for n_dir = 1:3
|
||||
subplot(3, 2, (n_dir-1)*2 + n_stg);
|
||||
title(['F ', dirs{n_dir}, ' to ', stages{n_stg}, ' ', dirs{n_dir}]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(FRFs_CoM(6*(n_stg+2) + n_dir, n_dir, :)))./((2*pi*freqs).^2)');
|
||||
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_dir}]), f, 'Hz'))));
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]');
|
||||
if n_dir == 3
|
||||
xlabel('Frequency [Hz]');
|
||||
end
|
||||
hold off;
|
||||
xlim([10, 1000]);
|
||||
ylim([1e-12, 1e-6]);
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/identification_comp_mid_stages.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:identification_comp_mid_stages
|
||||
#+CAPTION: caption ([[./figs/identification_comp_mid_stages.png][png]], [[./figs/identification_comp_mid_stages.pdf][pdf]])
|
||||
[[file:figs/identification_comp_mid_stages.png]]
|
||||
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
|
||||
stages = {'hexa'}
|
||||
|
||||
f = logspace(1, 3, 1000);
|
||||
|
||||
figure;
|
||||
for n_stg = 1
|
||||
for n_dir = 1:3
|
||||
subplot(3, 1, (n_dir-1) + n_stg);
|
||||
title(['F ', dirs{n_dir}, ' to ', stages{n_stg}, ' ', dirs{n_dir}]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(FRFs_CoM(6*(n_stg+4) + n_dir, n_dir, :)))./((2*pi*freqs).^2)');
|
||||
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_dir}]), f, 'Hz'))));
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]');
|
||||
if n_dir == 3
|
||||
xlabel('Frequency [Hz]');
|
||||
end
|
||||
hold off;
|
||||
xlim([10, 1000]);
|
||||
ylim([1e-12, 1e-6]);
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/identification_comp_top_stages.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:identification_comp_top_stages
|
||||
#+CAPTION: caption ([[./figs/identification_comp_top_stages.png][png]], [[./figs/identification_comp_top_stages.pdf][pdf]])
|
||||
[[file:figs/identification_comp_top_stages.png]]
|
||||
|
||||
|
||||
* Conclusion
|
||||
#+begin_important
|
||||
For such a complex system, we believe that the Simscape Model represents the dynamics of the system with enough fidelity.
|
||||
#+end_important
|
||||
97
org/index.org
Normal file
97
org/index.org
Normal file
@@ -0,0 +1,97 @@
|
||||
#+TITLE: Simscape Model of the Nano-Active-Stabilization-System
|
||||
:DRAWER:
|
||||
#+STARTUP: overview
|
||||
|
||||
#+LANGUAGE: en
|
||||
#+EMAIL: dehaeze.thomas@gmail.com
|
||||
#+AUTHOR: Dehaeze Thomas
|
||||
|
||||
#+HTML_LINK_HOME: ./index.html
|
||||
#+HTML_LINK_UP: ./index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/zenburn.css"/>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="./js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="./js/readtheorg.js"></script>
|
||||
|
||||
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
#+PROPERTY: header-args:matlab+ :tangle matlab/nass_simscape.m
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
|
||||
#+PROPERTY: header-args:shell :eval no-export
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
Here are links to the documents related to the Simscape model of the Nano-Active-Stabilization-System.
|
||||
|
||||
* Simulink Project ([[./org/simulink_project.org][link]])
|
||||
The project is managed with a Simulink Project.
|
||||
Such project is briefly presented [[./org/simulink_project.org][here]].
|
||||
|
||||
* Simscape Model ([[./org/simscape.org][link]])
|
||||
The model of the NASS is based on Simulink and Simscape Multi-Body.
|
||||
Such toolbox is presented [[./org/simscape.org][here]].
|
||||
|
||||
* Simscape Subsystems ([[./org/simscape_subsystems.org][link]])
|
||||
The model is decomposed of multiple subsystems.
|
||||
Subsystems can represent physical elements such as complete stages or basic simulink blocs.
|
||||
These subsystems are shared among multiple files.
|
||||
|
||||
All these subsystems are described [[./org/simscape_subsystems.org][here]].
|
||||
|
||||
* Kinematics of the Station ([[./org/kinematics.org][link]])
|
||||
First, we consider perfectly rigid elements and joints and we just study the kinematic of the station.
|
||||
This permits to test if each stage is moving correctly.
|
||||
This is detailed [[./org/kinematics.org][here]].
|
||||
|
||||
* Computation of the positioning error of the Sample ([[./org/positioning_error.org][link]])
|
||||
From the measurement of the position of the sample with respect to the granite and from the wanted position of each stage, we can compute the positioning error of the sample with respect to the nano-hexapod.
|
||||
This is done [[./org/positioning_error.org][here]].
|
||||
|
||||
* Tuning of the Dynamics of the Simscape model ([[./org/identification.org][link]])
|
||||
From dynamical measurements perform on the real positioning station, we tune the parameters of the simscape model to have similar dynamics.
|
||||
|
||||
This is explained [[./org/identification.org][here]].
|
||||
|
||||
* Disturbances ([[./org/disturbances.org][link]])
|
||||
The effect of disturbances on the position of the micro-station have been measured.
|
||||
These are now converted to force disturbances using the Simscape model.
|
||||
|
||||
This is discussed [[./org/disturbances.org][here]].
|
||||
|
||||
We also discuss how the disturbances are implemented in the model.
|
||||
|
||||
* Tomography Experiment ([[./org/experiments.org][link]])
|
||||
Now that the dynamics of the Model have been tuned and the Disturbances have included, we can simulate experiments.
|
||||
|
||||
Tomography experiments are simulated and the results are presented [[./org/experiments.org][here]].
|
||||
|
||||
* Active Damping Techniques on the Uni-axial Model ([[./org/active_damping_uniaxial.org][link]])
|
||||
Active damping techniques are applied to the Uniaxial Simscape model.
|
||||
|
||||
* Active Damping Techniques on the full Simscape Model ([[./org/active_damping.org][link]])
|
||||
Active damping techniques are applied to the full Simscape model.
|
||||
|
||||
* Useful Matlab Functions ([[./org/functions.org][link]])
|
||||
Many matlab functions are shared among all the files of the projects.
|
||||
|
||||
These functions are all defined [[./org/functions.org][here]].
|
||||
461
org/kinematics.org
Normal file
461
org/kinematics.org
Normal file
@@ -0,0 +1,461 @@
|
||||
#+TITLE: Kinematics of the station
|
||||
:DRAWER:
|
||||
#+STARTUP: overview
|
||||
|
||||
#+LANGUAGE: en
|
||||
#+EMAIL: dehaeze.thomas@gmail.com
|
||||
#+AUTHOR: Dehaeze Thomas
|
||||
|
||||
#+HTML_LINK_HOME: ../index.html
|
||||
#+HTML_LINK_UP: ../index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
||||
|
||||
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
#+PROPERTY: header-args:matlab+ :tangle matlab/modal_frf_coh.m
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
|
||||
#+PROPERTY: header-args:shell :eval no-export
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
In this document, we discuss the way the motion of each stage is defined.
|
||||
|
||||
* Micro Hexapod
|
||||
** How the Symetrie Hexapod is controlled on the micro station
|
||||
For the Micro-Hexapod, the convention for the angles are defined in =MAN_A_Software API_4.0.150918_EN.pdf= on page 13 (section 2.4 - Rotation Vectors):
|
||||
|
||||
#+begin_quote
|
||||
The *Euler type II convention* is used to express the rotation vector.
|
||||
This convention is mainly used in the aeronautics field (standard ISO 1151 concerning flight mechanics).
|
||||
|
||||
This convention uses the concepts of rotation of vehicles (ship, car and plane).
|
||||
Generally, we consider that the main movement of the vehicle is following the X-axis and the Z-axis is parallel to the axis of gravity (at the initial position).
|
||||
The roll rotation is around the X-axis, the pitch is around the Y-axis and yaw is the rotation around the Z-axis.
|
||||
*The order of rotation is: Rx, Ry and then Rz.*
|
||||
|
||||
In most case, rotations are related to a reference with fixed axis; thus we say the rotations are around fixed axes.
|
||||
The combination of these three rotations enables to write a rotation matrix.
|
||||
This writing is unique and equal to:
|
||||
\[ \bm{R} = \bm{R}_z(\gamma) \cdot \bm{R}_y(\beta) \cdot \bm{R}_x(\alpha) \]
|
||||
|
||||
The Euler type II convention corresponding to the *succession of rotations with respect to fixed axes*: first around X0, then Y0 and Z0.
|
||||
This is equivalent to the succession of rotations with respect to mobile axes: first around Z0, then Y1' and X2'.
|
||||
#+end_quote
|
||||
|
||||
More generally on the Control of the Micro-Hexapod:
|
||||
#+begin_quote
|
||||
Note that for all control modes, *the rotation center coincides with Object coordinate system origin*.
|
||||
Moreover, the movements are controlled with *translation components at first* (Tx, Ty, Tz) *then rotation components* (Rx, Ry, Rz).
|
||||
#+end_quote
|
||||
|
||||
Thus, it does the translations and then the rotation around the new translated frame.
|
||||
|
||||
** Control of the Micro-Hexapod using Simscape
|
||||
*** Introduction :ignore:
|
||||
We can think of two main ways to position the Micro-Hexapod using Simscape.
|
||||
|
||||
The first one is to use only one Bushing Joint between the base and the mobile platform.
|
||||
The advantage is that it is very easy to impose the wanted displacement, however, we loose the dynamical properties of the Hexapod.
|
||||
|
||||
The second way is to specify the wanted length of the legs of the Hexapod in order to have the wanted position of the mobile platform.
|
||||
This require a little bit more of mathematical derivations but this is the chosen solution.
|
||||
|
||||
*** Using Bushing Joint
|
||||
In the documentation of the Bushing Joint (=doc "Bushing Joint"=) that is used to position the Hexapods, it is mention that the following frame is positioned with respect to the base frame in a way shown in figure [[fig:bushing_joint_transform]].
|
||||
|
||||
#+name: fig:bushing_joint_transform
|
||||
#+caption: Joint Transformation Sequence for the Bushing Joint
|
||||
[[file:figs/bushing_joint_transform.png]]
|
||||
|
||||
Basically, it performs the translations, and then the rotation along the X, Y and Z axis of the moving frame.
|
||||
The three rotations that we define thus corresponds to the Euler U-V-W angles.
|
||||
|
||||
We should have the *same behavior* for the Micro-Hexapod on Simscape (same inputs at least).
|
||||
However, the Bushing Joint makes rotations around mobiles axes (X, Y' and then Z'') and not fixed axes (X, Y and Z).
|
||||
|
||||
*** Using Inverse Kinematics and Leg Actuators
|
||||
Here, we can use the Inverse Kinematic of the Hexapod to determine the length of each leg in order to obtain some defined translation and rotation of the mobile platform.
|
||||
|
||||
The advantages are:
|
||||
- we can position the Hexapod as we want by specifying a rotation matrix
|
||||
- the hexapod keeps its full flexibility as we don't specify any wanted displacements, only leg's rest position
|
||||
|
||||
However:
|
||||
- even though the rest position of each leg (the position where the stiffness force is zero) is set correctly, the hexapod will we deflected due to gravity
|
||||
|
||||
Thus, for this simulation, we *remove the gravity*.
|
||||
|
||||
**** Theory
|
||||
For inverse kinematic analysis, it is assumed that the position ${}^A\bm{P}$ and orientation of the moving platform ${}^A\bm{R}_B$ are given and the problem is to obtain the joint variables, namely, $\bm{L} = [l_1, l_2, \dots, l_6]^T$.
|
||||
|
||||
From the geometry of the manipulator, the loop closure for each limb, $i = 1, 2, \dots, 6$ can be written as
|
||||
\begin{align*}
|
||||
l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\
|
||||
&= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i
|
||||
\end{align*}
|
||||
|
||||
To obtain the length of each actuator and eliminate $\hat{\bm{s}}_i$, it is sufficient to dot multiply each side by itself:
|
||||
\begin{equation}
|
||||
l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]
|
||||
\end{equation}
|
||||
|
||||
Hence, for $i = 1, 2, \dots, 6$, each limb length can be uniquely determined by:
|
||||
\begin{equation}
|
||||
l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i}
|
||||
\end{equation}
|
||||
|
||||
If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation.
|
||||
Otherwise, when the limbs' lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable.
|
||||
|
||||
**** 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
|
||||
|
||||
**** Matlab Implementation
|
||||
We open the Simulink file.
|
||||
#+begin_src matlab
|
||||
open('nass_model.slx')
|
||||
#+end_src
|
||||
|
||||
We load the configuration and set a small =StopTime=.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '0.1');
|
||||
#+end_src
|
||||
|
||||
We define the wanted position/orientation of the Hexapod under study.
|
||||
#+begin_src matlab
|
||||
tx = 0.05; % [rad]
|
||||
ty = 0.1; % [rad]
|
||||
tz = 0.02; % [rad]
|
||||
|
||||
Rx = [1 0 0;
|
||||
0 cos(tx) -sin(tx);
|
||||
0 sin(tx) cos(tx)];
|
||||
|
||||
Ry = [ cos(ty) 0 sin(ty);
|
||||
0 1 0;
|
||||
-sin(ty) 0 cos(ty)];
|
||||
|
||||
Rz = [cos(tz) -sin(tz) 0;
|
||||
sin(tz) cos(tz) 0;
|
||||
0 0 1];
|
||||
|
||||
ARB = Rz*Ry*Rx;
|
||||
AP = [0.1; 0.005; 0.01]; % [m]
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
initializeSimscapeConfiguration('gravity', false);
|
||||
initializeGround('type', 'none');
|
||||
initializeGranite('type', 'none');
|
||||
initializeTy('type', 'none');
|
||||
initializeRy('type', 'none');
|
||||
initializeRz('type', 'none');
|
||||
initializeMicroHexapod('type', 'rigid', 'AP', AP, 'ARB', ARB);
|
||||
initializeAxisc('type', 'none');
|
||||
initializeMirror('type', 'none');
|
||||
initializeNanoHexapod('type', 'none');
|
||||
initializeSample('type', 'none');
|
||||
initializeLoggingConfiguration('log', 'all');
|
||||
#+end_src
|
||||
|
||||
We run the simulation.
|
||||
#+begin_src matlab
|
||||
sim('nass_model');
|
||||
#+end_src
|
||||
|
||||
And we verify that we indeed succeed to go to the wanted position.
|
||||
#+begin_src matlab :results table replace
|
||||
[simout.Dhm.x.Data(end) ; simout.Dhm.y.Data(end) ; simout.Dhm.z.Data(end)] - AP
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
| 8.4655e-16 |
|
||||
| 1.5586e-15 |
|
||||
| -2.1337e-16 |
|
||||
|
||||
#+begin_src matlab :results table replace
|
||||
simout.Dhm.R.Data(:, :, end)-ARB
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
| -1.1102e-16 | -1.36e-15 | 4.2744e-15 |
|
||||
| 1.0651e-15 | 6.6613e-16 | 5.1278e-15 |
|
||||
| -4.2882e-15 | -4.9336e-15 | 1.1102e-16 |
|
||||
|
||||
* TODO Tests on the transformation from reference to wanted position :noexport:
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :eval no
|
||||
:END:
|
||||
** Introduction :ignore:
|
||||
#+begin_quote
|
||||
Rx = [1 0 0;
|
||||
0 cos(t) -sin(t);
|
||||
0 sin(t) cos(t)];
|
||||
|
||||
Ry = [ cos(t) 0 sin(t);
|
||||
0 1 0;
|
||||
-sin(t) 0 cos(t)];
|
||||
|
||||
Rz = [cos(t) -sin(t) 0;
|
||||
sin(t) cos(t) 0;
|
||||
0 0 1];
|
||||
#+end_quote
|
||||
|
||||
Let's define the following frames:
|
||||
- $\{W\}$ the frame that is *fixed to the granite* and its origin at the theoretical meeting point between the X-ray and the spindle axis.
|
||||
- $\{S\}$ the frame *attached to the sample* (in reality attached to the top platform of the nano-hexapod) with its origin at 175mm above the top platform of the nano-hexapod.
|
||||
Its origin is $O_S$.
|
||||
- $\{T\}$ the theoretical wanted frame that correspond to the wanted pose of the frame $\{S\}$.
|
||||
$\{T\}$ is computed from the wanted position of each stage. It is thus theoretical and does not correspond to a real position.
|
||||
The origin of $T$ is $O_T$ and is the wanted position of the sample.
|
||||
|
||||
Thus:
|
||||
- the *measurement* of the position of the sample corresponds to ${}^W O_S = \begin{bmatrix} {}^WP_{x,m} & {}^WP_{y,m} & {}^WP_{z,m} \end{bmatrix}^T$ in translation and to $\theta_m {}^W\bm{s}_m = \theta_m \cdot \begin{bmatrix} {}^Ws_{x,m} & {}^Ws_{y,m} & {}^Ws_{z,m} \end{bmatrix}^T$ in rotations
|
||||
- the *wanted position* of the sample expressed w.r.t. the granite is ${}^W O_T = \begin{bmatrix} {}^WP_{x,r} & {}^WP_{y,r} & {}^WP_{z,r} \end{bmatrix}^T$ in translation and to $\theta_r {}^W\bm{s}_r = \theta_r \cdot \begin{bmatrix} {}^Ws_{x,r} & {}^Ws_{y,r} & {}^Ws_{z,r} \end{bmatrix}^T$ in rotations
|
||||
|
||||
** Wanted Position of the Sample with respect to the Granite
|
||||
Let's define the wanted position of each stage.
|
||||
#+begin_src matlab
|
||||
Ty = 0; % [m]
|
||||
Ry = 3*pi/180; % [rad]
|
||||
Rz = 180*pi/180; % [rad]
|
||||
|
||||
% Hexapod (first consider only translations)
|
||||
Thx = 0; % [m]
|
||||
Thy = 0; % [m]
|
||||
Thz = 0; % [m]
|
||||
#+end_src
|
||||
|
||||
Now, we compute the corresponding wanted translation and rotation of the sample with respect to the granite frame $\{W\}$.
|
||||
This corresponds to ${}^WO_T$ and $\theta_m {}^Ws_m$.
|
||||
|
||||
To do so, we have to define the homogeneous transformation for each stage.
|
||||
#+begin_src matlab
|
||||
% Translation Stage
|
||||
Rty = [1 0 0 0;
|
||||
0 1 0 Ty;
|
||||
0 0 1 0;
|
||||
0 0 0 1];
|
||||
|
||||
% Tilt Stage - Pure rotating aligned with Ob
|
||||
Rry = [ cos(Ry) 0 sin(Ry) 0;
|
||||
0 1 0 0;
|
||||
-sin(Ry) 0 cos(Ry) 0;
|
||||
0 0 0 1];
|
||||
|
||||
% Spindle - Rotation along the Z axis
|
||||
Rrz = [cos(Rz) -sin(Rz) 0 0 ;
|
||||
sin(Rz) cos(Rz) 0 0 ;
|
||||
0 0 1 0 ;
|
||||
0 0 0 1 ];
|
||||
|
||||
% Micro-Hexapod (only rotations first)
|
||||
Rh = [1 0 0 Thx ;
|
||||
0 1 0 Thy ;
|
||||
0 0 1 Thz ;
|
||||
0 0 0 1 ];
|
||||
#+end_src
|
||||
|
||||
We combine the individual homogeneous transformations into one homogeneous transformation for all the station.
|
||||
#+begin_src matlab
|
||||
Ttot = Rty*Rry*Rrz*Rh;
|
||||
#+end_src
|
||||
|
||||
Using this homogeneous transformation, we can compute the wanted position and orientation of the sample with respect to the granite.
|
||||
|
||||
Translation.
|
||||
#+begin_src matlab
|
||||
WOr = Ttot*[0;0;0;1];
|
||||
WOr = WOr(1:3);
|
||||
#+end_src
|
||||
|
||||
Rotation.
|
||||
#+begin_src matlab
|
||||
thetar = acos((trace(Ttot(1:3, 1:3))-1)/2)
|
||||
if thetar == 0
|
||||
WSr = [0; 0; 0];
|
||||
else
|
||||
[V, D] = eig(Ttot(1:3, 1:3));
|
||||
WSr = thetar*V(:, abs(diag(D) - 1) < eps(1));
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
WPr = [WOr ; WSr];
|
||||
#+end_src
|
||||
|
||||
** Measured Position of the Sample with respect to the Granite
|
||||
The measurement of the position of the sample using the metrology system gives the position and orientation of the sample with respect to the granite.
|
||||
#+begin_src matlab
|
||||
% Measurements: Xm, Ym, Zm, Rx, Ry, Rz
|
||||
Dxm = 0; % [m]
|
||||
Dym = 0; % [m]
|
||||
Dzm = 0; % [m]
|
||||
|
||||
Rxm = 0*pi/180; % [rad]
|
||||
Rym = 0*pi/180; % [rad]
|
||||
Rzm = 180*pi/180; % [rad]
|
||||
#+end_src
|
||||
|
||||
Let's compute the corresponding orientation using screw axis.
|
||||
#+begin_src matlab
|
||||
Trxm = [1 0 0;
|
||||
0 cos(Rxm) -sin(Rxm);
|
||||
0 sin(Rxm) cos(Rxm)];
|
||||
Trym = [ cos(Rym) 0 sin(Rym);
|
||||
0 1 0;
|
||||
-sin(Rym) 0 cos(Rym)];
|
||||
Trzm = [cos(Rzm) -sin(Rzm) 0;
|
||||
sin(Rzm) cos(Rzm) 0;
|
||||
0 0 1];
|
||||
|
||||
STw = [[ Trym*Trxm*Trzm , [Dxm; Dym; Dzm]]; 0 0 0 1];
|
||||
#+end_src
|
||||
|
||||
We then obtain the orientation measurement in the form of screw coordinate $\theta_m ({}^Ws_{x,m},\ {}^Ws_{y,m},\ {}^Ws_{z,m})^T$ where:
|
||||
- $\theta_m = \cos^{-1} \frac{\text{Tr}(R) - 1}{2}$
|
||||
- ${}^W\bm{s}_m$ is the eigen vector of the rotation matrix $R$ corresponding to the eigen value $\lambda = 1$
|
||||
|
||||
#+begin_src matlab
|
||||
thetam = acos((trace(STw(1:3, 1:3))-1)/2); % [rad]
|
||||
if thetam == 0
|
||||
WSm = [0; 0; 0];
|
||||
else
|
||||
[V, D] = eig(STw(1:3, 1:3));
|
||||
WSm = thetam*V(:, abs(diag(D) - 1) < eps(1));
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
WPm = [Dxm ; Dym ; Dzm ; WSm];
|
||||
#+end_src
|
||||
|
||||
** Positioning Error with respect to the Granite
|
||||
The wanted position expressed with respect to the granite is ${}^WO_T$ and the measured position with respect to the granite is ${}^WO_S$, thus the *position error* expressed in $\{W\}$ is
|
||||
\[ {}^W E = {}^W O_T - {}^W O_S \]
|
||||
The same is true for rotations:
|
||||
\[ \theta_\epsilon {}^W\bm{s}_\epsilon = \theta_r {}^W\bm{s}_r - \theta_m {}^W\bm{s}_m \]
|
||||
|
||||
#+begin_src matlab
|
||||
WPe = WPr - WPm;
|
||||
#+end_src
|
||||
|
||||
#+begin_quote
|
||||
Now we want to express this error in a frame attached to the *base of the nano-hexapod* with its origin at the same point where the Jacobian of the nano-hexapod is computed (175mm above the top platform + 90mm of total height of the nano-hexapod).
|
||||
|
||||
Or maybe should we want to express this error with respect to the *top platform of the nano-hexapod*?
|
||||
We are measuring the position of the top-platform, and we don't know exactly the position of the bottom platform.
|
||||
We could compute the position of the bottom platform in two ways:
|
||||
- from the encoders of each stage
|
||||
- from the measurement of the nano-hexapod top platform + the internal metrology in the nano-hexapod (capacitive sensors e.g)
|
||||
|
||||
A third option is to say that the maximum stroke of the nano-hexapod is so small that the error should no change to much by the change of base.
|
||||
#+end_quote
|
||||
|
||||
** Position Error Expressed in the Nano-Hexapod Frame
|
||||
We now want the position error to be expressed in $\{S\}$ (the frame attach to the sample) for control:
|
||||
\[ {}^S E = {}^S T_W \cdot {}^W E \]
|
||||
|
||||
Thus we need to compute the homogeneous transformation ${}^ST_W$.
|
||||
Fortunately, this homogeneous transformation can be computed from the measurement of the sample position and orientation with respect to the granite.
|
||||
#+begin_src matlab
|
||||
Trxm = [1 0 0;
|
||||
0 cos(Rxm) -sin(Rxm);
|
||||
0 sin(Rxm) cos(Rxm)];
|
||||
Trym = [ cos(Rym) 0 sin(Rym);
|
||||
0 1 0;
|
||||
-sin(Rym) 0 cos(Rym)];
|
||||
Trzm = [cos(Rzm) -sin(Rzm) 0;
|
||||
sin(Rzm) cos(Rzm) 0;
|
||||
0 0 1];
|
||||
|
||||
STw = [[ Trym*Trxm*Trzm , [Dxm; Dym; Dzm]]; 0 0 0 1];
|
||||
#+end_src
|
||||
|
||||
Translation Error.
|
||||
#+begin_src matlab
|
||||
SEm = STw * [WPe(1:3); 0];
|
||||
SEm = SEm(1:3);
|
||||
#+end_src
|
||||
|
||||
Rotation Error.
|
||||
#+begin_src matlab
|
||||
SEr = STw * [WPe(4:6); 0];
|
||||
SEr = SEr(1:3);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
Etot = [SEm ; SEr]
|
||||
#+end_src
|
||||
** Another try
|
||||
Let's denote:
|
||||
- $\{W\}$ the initial fixed frame
|
||||
- $\{R\}$ the reference frame corresponding to the wanted pose of the sample
|
||||
- $\{M\}$ the frame corresponding to the measured pose of the sample
|
||||
|
||||
We have then computed:
|
||||
- ${}^WT_R$
|
||||
- ${}^WT_M$
|
||||
|
||||
We have:
|
||||
\begin{align}
|
||||
{}^MT_R &= {}^MT_W {}^WT_R \\
|
||||
&= {}^WT_M^t {}^WT_R
|
||||
\end{align}
|
||||
|
||||
#+begin_src matlab
|
||||
MTr = STw'*Ttot;
|
||||
#+end_src
|
||||
|
||||
Position error:
|
||||
#+begin_src matlab
|
||||
MTr(1:3, 1:4)*[0; 0; 0; 1]
|
||||
#+end_src
|
||||
|
||||
Orientation error:
|
||||
#+begin_src matlab
|
||||
MTr(1:3, 1:3)
|
||||
#+end_src
|
||||
|
||||
** Verification
|
||||
How can we verify that the computation is correct?
|
||||
Options:
|
||||
- Test with simscape multi-body
|
||||
- Impose motion on each stage
|
||||
- Measure the position error w.r.t. the NASS
|
||||
- Compare with the computation
|
||||
473
org/positioning_error.org
Normal file
473
org/positioning_error.org
Normal file
@@ -0,0 +1,473 @@
|
||||
#+TITLE: Computation of the Positioning Error with respect to the nano-hexapod
|
||||
:DRAWER:
|
||||
#+STARTUP: overview
|
||||
|
||||
#+LANGUAGE: en
|
||||
#+EMAIL: dehaeze.thomas@gmail.com
|
||||
#+AUTHOR: Dehaeze Thomas
|
||||
|
||||
#+HTML_LINK_HOME: ../index.html
|
||||
#+HTML_LINK_UP: ../index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
||||
|
||||
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
#+PROPERTY: header-args:matlab+ :tangle no
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
|
||||
#+PROPERTY: header-args:shell :eval no-export
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
The global measurement and control schematic is shown in figure [[fig:control-schematic-nass]].
|
||||
|
||||
#+name: fig:control-schematic-nass
|
||||
#+caption: Global Control Schematic for the Station
|
||||
[[file:figs/control-schematic-nass.png]]
|
||||
|
||||
In this document, are interesting by the "compute Sample Position w.r.t. Granite" bloc and we develop and verify that the two green blocs are working.
|
||||
|
||||
We suppose that we are able to measure perfectly the position of the sample with respect to the granite.
|
||||
This means that we do not care about the bloc "Compute Sample Position w.r.t. Granite" that makes the transformation from the interferometer measurements to the position of the sample.
|
||||
We suppose that we can directly measure perfectly the position of the sample with respect to the granite.
|
||||
This is actually done with a "transform sensor block" that outputs the x-y-z translation of the sample with respect to the granite as well as the rotation matrix that maps the granite frame to the sample frame.
|
||||
|
||||
Also, all the stages can be perfectly positioned.
|
||||
|
||||
First, in section [[sec:measurement_principle]], is explained how the measurement of the position of the sample with respect to the granite is performed (using Simscape blocs).
|
||||
|
||||
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). This corresponds to the bloc "Compute Wanted Sample Position w.r.t. Granite" in figure [[fig:control-schematic-nass]].
|
||||
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 micro-station's 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 (corresponding to the green bloc "Compute Sample Position Error w.r.t. NASS" in figure [[fig:control-schematic-nass]]).
|
||||
Then, 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.
|
||||
|
||||
* How do we measure the position of the sample with respect to the granite
|
||||
<<sec:measurement_principle>>
|
||||
A transform sensor block gives the translation and orientation of the follower frame with respect to the base frame.
|
||||
|
||||
The base frame is fixed to the granite and located at the initial sample location that defines the zero position.
|
||||
|
||||
The follower frame is attached to the sample (or more precisely to the reflector).
|
||||
|
||||
The outputs of the transform sensor are:
|
||||
- the 3 translations x, y and z in meter
|
||||
- the *rotation matrix* $\bm{R}$ that permits to rotate the base frame into the follower frame.
|
||||
|
||||
We can then determine extract other orientation conventions such that Euler angles or screw axis.
|
||||
|
||||
* 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
|
||||
|
||||
#+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('nass_model.slx')
|
||||
#+end_src
|
||||
|
||||
** Prepare the Simulation
|
||||
We set a small =StopTime=.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '0.5');
|
||||
#+end_src
|
||||
|
||||
We initialize all the stages.
|
||||
#+begin_src matlab
|
||||
initializeGround('type', 'rigid');
|
||||
initializeGranite('type', 'rigid');
|
||||
initializeTy('type', 'rigid');
|
||||
initializeRy('type', 'rigid');
|
||||
initializeRz('type', 'rigid');
|
||||
initializeMicroHexapod('type', 'rigid');
|
||||
initializeAxisc('type', 'rigid');
|
||||
initializeMirror('type', 'rigid');
|
||||
initializeNanoHexapod('type', 'rigid', 'actuator', 'piezo');
|
||||
initializeSample('type', 'rigid', 'mass', 50);
|
||||
#+end_src
|
||||
|
||||
No disturbance and no gravity.
|
||||
#+begin_src matlab
|
||||
initializeSimscapeConfiguration('gravity', false);
|
||||
initializeDisturbances('enable', false);
|
||||
#+end_src
|
||||
|
||||
We setup the reference path to be constant.
|
||||
#+begin_src matlab
|
||||
initializeReferences(...
|
||||
'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*pi/180, ... % Amplitude [rad]
|
||||
'Ry_period', 10, ... % Period of the displacement [s]
|
||||
'Rz_type', 'constant', ... % Either "constant" / "rotating"
|
||||
'Rz_amplitude', -135*pi/180, ... % Initial angle [rad]
|
||||
'Rz_period', 1, ... % Period of the rotating [s]
|
||||
'Dh_type', 'constant', ... % For now, only constant is implemented
|
||||
'Dh_pos', [0.01; 0.02; -0.03; -3*pi/180; 1*pi/180; 3*pi/180], ... % 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', [1e-3; 2e-3; 3e-3; 1*pi/180; 0; 1*pi/180] ... % Initial position [m,m,m,rad,rad,rad] of the top platform
|
||||
);
|
||||
#+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,m,m,rad,rad,rad]
|
||||
Dhle = zeros(6,1); % [m,m,m,m,m,m]
|
||||
Dne = zeros(6,1); % [m,m,m,rad,rad,rad]
|
||||
#+end_src
|
||||
|
||||
We want to log the signals
|
||||
#+begin_src matlab
|
||||
initializeLoggingConfiguration('log', 'all');
|
||||
#+end_src
|
||||
|
||||
And we run the simulation.
|
||||
#+begin_src matlab
|
||||
sim('nass_model');
|
||||
#+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\bm{T}_R$ which corresponds to the wanted pose of the sample with respect to the granite
|
||||
- ${}^W\bm{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 ${}^W\bm{T}_R$.
|
||||
#+begin_src matlab
|
||||
n = length(simout.r.Dy.Time);
|
||||
WTr = zeros(4, 4, n);
|
||||
for i = 1:n
|
||||
WTr(:, :, i) = computeReferencePose(simout.r.Dy.Data(i), simout.r.Ry.Data(i), simout.r.Rz.Data(i), simout.r.Dh.Data(i,:), simout.r.Dn.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 ${}^W\bm{T}_M$.
|
||||
#+begin_src matlab
|
||||
n = length(simout.y.R.Time);
|
||||
WTm = zeros(4, 4, n);
|
||||
WTm(1:3, 1:3, :) = simout.y.R.Data;
|
||||
WTm(1:3, 4, :) = [simout.y.x.Data' ; simout.y.y.Data' ; simout.y.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\bm{T}_R = {}^W\bm{T}_M \]
|
||||
|
||||
Or are least:
|
||||
\[ {}^W\bm{T}_R(1:3, 4) = {}^W\bm{T}_M(1:3, 4) \]
|
||||
\[ {}^W\bm{R}_R^t \cdot {}^W\bm{R}_M = \bm{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 =
|
||||
8.53830894875784e-15
|
||||
-5.58580959264532e-15
|
||||
-5.89805981832114e-17
|
||||
WTr(1:3, 1:3, end)'*WTm(1:3, 1:3, end)-eye(3)
|
||||
ans =
|
||||
1.62092561595273e-14 -1.59832000065641e-14 1.11022302462516e-16
|
||||
1.61295672998496e-14 1.62092561595273e-14 -1.72431513512095e-15
|
||||
-1.65492619608187e-15 -9.8879238130678e-16 9.41469124882133e-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('nass_model.slx')
|
||||
#+end_src
|
||||
|
||||
** Prepare the Simulation
|
||||
We set a small =StopTime=.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '0.5');
|
||||
#+end_src
|
||||
|
||||
We initialize all the stages.
|
||||
#+begin_src matlab
|
||||
initializeGround('type', 'rigid');
|
||||
initializeGranite('type', 'rigid');
|
||||
initializeTy('type', 'rigid');
|
||||
initializeRy('type', 'rigid');
|
||||
initializeRz('type', 'rigid');
|
||||
initializeMicroHexapod('type', 'rigid');
|
||||
initializeAxisc('type', 'rigid');
|
||||
initializeMirror('type', 'rigid');
|
||||
initializeNanoHexapod('type', 'rigid', 'actuator', 'piezo');
|
||||
initializeSample('type', 'rigid', 'mass', 50);
|
||||
#+end_src
|
||||
|
||||
No disturbance and no gravity.
|
||||
#+begin_src matlab
|
||||
initializeSimscapeConfiguration('gravity', false);
|
||||
initializeDisturbances('enable', false);
|
||||
#+end_src
|
||||
|
||||
We setup the reference path to be constant.
|
||||
#+begin_src matlab
|
||||
initializeReferences(...
|
||||
'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 [rad]
|
||||
'Rz_type', 'constant', ... % Either "constant" / "rotating"
|
||||
'Rz_amplitude', 0*pi/180, ... % Initial angle [rad]
|
||||
'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
|
||||
);
|
||||
#+end_src
|
||||
|
||||
Now we introduce some positioning error.
|
||||
#+begin_src matlab
|
||||
Dye = 1e-6; % [m]
|
||||
Rye = 2e-4; % [rad]
|
||||
Rze = 1e-5; % [rad]
|
||||
initializePosError('error', true, 'Dy', Dye, 'Ry', Rye, 'Rz', Rze);
|
||||
#+end_src
|
||||
|
||||
And we run the simulation.
|
||||
#+begin_src matlab
|
||||
sim('nass_model');
|
||||
#+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\bm{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\bm{T}_R &= {}^M\bm{T}_W \cdot {}^W\bm{T}_R \\
|
||||
&= {}^W{\bm{T}_M}^{-1} \cdot {}^W\bm{T}_R
|
||||
\end{align}
|
||||
|
||||
The top platform of the nano-hexapod is considered to be rigidly connected to the sample, thus, ${}^M\bm{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\bm{T}_R$.
|
||||
#+begin_src matlab
|
||||
n = length(simout.r.Dy.Time);
|
||||
WTr = zeros(4, 4, n);
|
||||
for i = 1:n
|
||||
WTr(:, :, i) = computeReferencePose(simout.r.Dy.Data(i), simout.r.Ry.Data(i), simout.r.Rz.Data(i), simout.r.Dh.Data(i,:), simout.r.Dn.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\bm{T}_M$.
|
||||
#+begin_src matlab
|
||||
n = length(simout.y.R.Time);
|
||||
WTm = zeros(4, 4, n);
|
||||
WTm(1:3, 1:3, :) = simout.y.R.Data;
|
||||
WTm(1:3, 4, :) = [simout.y.x.Data' ; simout.y.y.Data' ; simout.y.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\bm{T}_A = {}^A\bm{T}_B^{-1} =
|
||||
\left[ \begin{array}{ccc|c}
|
||||
& & & \\
|
||||
& {}^A\bm{R}_B^T & & -{}^A \bm{R}_B^T {}^A\bm{P}_{O_B} \\
|
||||
& & & \\
|
||||
\hline
|
||||
0 & 0 & 0 & 1 \\
|
||||
\end{array} \right]
|
||||
\end{equation}
|
||||
|
||||
Finally, we compute ${}^M\bm{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
|
||||
MTr(1:3, 1:3, end)
|
||||
Rx = [1 0 0;
|
||||
0 cos(Erx) -sin(Erx);
|
||||
0 sin(Erx) cos(Erx)];
|
||||
|
||||
Ry = [ cos(Ery) 0 sin(Ery);
|
||||
0 1 0;
|
||||
-sin(Ery) 0 cos(Ery)];
|
||||
|
||||
Rz = [cos(Erz) -sin(Erz) 0;
|
||||
sin(Erz) cos(Erz) 0;
|
||||
0 0 1];
|
||||
#+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-11 | -1.0e-06 | -6.2e-16 | -2.0e-09 | -2.0e-04 | -1.0e-05 |
|
||||
|
||||
** Verify that be imposing the error motion on the nano-hexapod, we indeed have zero error at the end
|
||||
We now keep the wanted pose but we impose a displacement of the nano hexapod corresponding to the measured position error.
|
||||
#+begin_src matlab
|
||||
initializeReferences(...
|
||||
'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 [rad]
|
||||
'Rz_type', 'constant', ... % Either "constant" / "rotating"
|
||||
'Rz_amplitude', 0*pi/180, ... % Initial angle [rad]
|
||||
'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', [Edx; Edy; Edz; Erx; Ery; Erz] ... % Initial position [m,m,m,rad,rad,rad] of the top platform
|
||||
);
|
||||
#+end_src
|
||||
|
||||
And we run the simulation.
|
||||
#+begin_src matlab
|
||||
sim('nass_model');
|
||||
#+end_src
|
||||
|
||||
We keep the old computed computed reference pose ${}^W\bm{T}_r$ even though we have change the nano hexapod reference, but this is not a real wanted reference but rather a adaptation to reject the positioning errors.
|
||||
|
||||
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 ${}^W\bm{T}_M$.
|
||||
#+begin_src matlab
|
||||
n = length(simout.y.R.Time);
|
||||
WTm = zeros(4, 4, n);
|
||||
WTm(1:3, 1:3, :) = simout.y.R.Data;
|
||||
WTm(1:3, 4, :) = [simout.y.x.Data' ; simout.y.y.Data' ; simout.y.z.Data'];
|
||||
WTm(4, 4, :) = 1;
|
||||
#+end_src
|
||||
|
||||
Finally, we compute ${}^M\bm{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 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 | 2.4e-16 | -9.9e-17 | 4.4e-16 | 2.0e-09 | -8.9e-15 | 2.0e-13 |
|
||||
|
||||
** 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
|
||||
142
org/simscape.org
Normal file
142
org/simscape.org
Normal file
@@ -0,0 +1,142 @@
|
||||
#+TITLE: Simscape Model
|
||||
:DRAWER:
|
||||
#+STARTUP: overview
|
||||
|
||||
#+LANGUAGE: en
|
||||
#+EMAIL: dehaeze.thomas@gmail.com
|
||||
#+AUTHOR: Dehaeze Thomas
|
||||
|
||||
#+HTML_LINK_HOME: ../index.html
|
||||
#+HTML_LINK_UP: ../index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
||||
|
||||
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
#+PROPERTY: header-args:matlab+ :tangle no
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
|
||||
#+PROPERTY: header-args:shell :eval no-export
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
A [[https://.mathworks.com/products/simscape.html][simscape]] model permits to model multi-physics systems.
|
||||
|
||||
[[https://mathworks.com/products/simmechanics.html][Simscape Multibody]] permits to model multibody systems using blocks representing bodies, joints, constraints, force elements, and sensors.
|
||||
|
||||
* Solid bodies
|
||||
Each solid body is represented by a [[https://mathworks.com/help/physmod/sm/ref/solid.html][solid block]].
|
||||
The geometry of the solid body can be imported using a =step= file. The properties of the solid body such as its mass, center of mass and moment of inertia can be derived from its density and its geometry as defined by the =step= file. They also can be set by hand.
|
||||
|
||||
* Frames
|
||||
Frames are very important in simscape multibody, they defined where the forces are applied, where the joints are located and where the measurements are made.
|
||||
|
||||
They can be defined from the solid body geometry, or using the [[https://mathworks.com/help/physmod/sm/ref/rigidtransform.html][rigid transform block]].
|
||||
|
||||
* Joints
|
||||
Solid Bodies are connected with joints (between frames of the two solid bodies).
|
||||
|
||||
There are various types of joints that are all described [[https://mathworks.com/help/physmod/sm/ug/joints.html][here]].
|
||||
|
||||
#+name: tab:joint_dof
|
||||
#+caption: Degrees of freedom associated with each joint
|
||||
| Joint Block | Translational DOFs | Rotational DOFs |
|
||||
|-------------------+--------------------+-----------------|
|
||||
| 6-DOF | 3 | 3 |
|
||||
| Bearing | 1 | 3 |
|
||||
| Bushing | 3 | 3 |
|
||||
| Cartesian | 3 | 0 |
|
||||
| Constant Velocity | 0 | 2 |
|
||||
| Cylindrical | 1 | 1 |
|
||||
| Gimbal | 0 | 3 |
|
||||
| Leadscrew | 1 (coupled) | 1 (coupled) |
|
||||
| Pin Slot | 1 | 1 |
|
||||
| Planar | 2 | 1 |
|
||||
| Prismatic | 1 | 0 |
|
||||
| Rectangular | 2 | 0 |
|
||||
| Revolute | 0 | 1 |
|
||||
| Spherical | 1 | 3 |
|
||||
| Telescoping | 1 | 3 |
|
||||
| Universal | 0 | 2 |
|
||||
| Weld | 0 | 0 |
|
||||
|
||||
Joint blocks are assortments of joint primitives:
|
||||
- *Prismatic*: allows translation along a single standard axis: =Px=, =Py=, =Pz=
|
||||
- *Revolute*: allows rotation about a single standard axis: =Rx=, =Ry=, =Rz=
|
||||
- *Spherical*: allow rotation about any 3D axis: =S=
|
||||
- *Lead Screw*: allows coupled rotation and translation on a standard axis: =LSz=
|
||||
- *Constant Velocity*: Allows rotation at constant velocity between intersection through arbitrarily aligned shafts: =CV=
|
||||
|
||||
#+name: tab:joint_primitive
|
||||
#+caption: Joint primitives for each joint type
|
||||
| Joint Block | Px | Py | Pz | Rx | Ry | Rz | S | CV | LSz |
|
||||
|-------------------+----+----+----+----+----+----+---+----+-----|
|
||||
| 6-DOF | x | x | x | | | | x | | |
|
||||
| Bearing | | | x | x | x | x | | | |
|
||||
| Bushing | x | x | x | x | x | x | | | |
|
||||
| Cartesian | x | x | x | | | | | | |
|
||||
| Constant Velocity | | | | | | | | x | |
|
||||
| Cylindrical | | | x | | | x | | | |
|
||||
| Gimbal | | | | x | x | x | | | |
|
||||
| Leadscrew | | | | | | | | | x |
|
||||
| Pin Slot | x | | | | | x | | | |
|
||||
| Planar | x | x | | | | x | | | |
|
||||
| Prismatic | | | x | | | | | | |
|
||||
| Rectangular | x | x | | | | | | | |
|
||||
| Revolute | | | | | | x | | | |
|
||||
| Spherical | | | | | | | x | | |
|
||||
| Telescoping | | | x | | | | x | | |
|
||||
| Universal | | | | x | x | | | | |
|
||||
| Weld | | | | | | | | | |
|
||||
|
||||
For each of the joint primitive, we can specify the dynamical properties:
|
||||
- The *spring stiffness*: either linear or rotational one
|
||||
- The *damping coefficient*
|
||||
|
||||
For the actuation, we can either specify the motion or the force:
|
||||
- the force applied in the corresponding DOF is provided by the input
|
||||
- the motion is provided by the input
|
||||
|
||||
A sensor can be added to measure either the position, velocity or acceleration of the joint DOF.
|
||||
|
||||
Composite Force/Torque sensing:
|
||||
- Constraint force
|
||||
- Total force: gives the sum across all joint primitives over all sources: actuation inputs, internal springs and dampers.
|
||||
|
||||
* Measurements
|
||||
A transform sensor block measures the spatial relationship between two frames: the base =B= and the follower =F=.
|
||||
|
||||
It can give the rotational and translational position, velocity and acceleration.
|
||||
|
||||
The measurement frame should be specified: it corresponds to the frame in which to resolve the selected spatial measurements. The default is =world=.
|
||||
|
||||
If we want to simulate an *inertial sensor*, we just have to choose =B= to be the =world= frame.
|
||||
|
||||
*Force sensors* are included in the joints blocks.
|
||||
|
||||
* Excitation
|
||||
We can apply *external forces* to the model by using an [[https://mathworks.com/help/physmod/sm/ref/externalforceandtorque.html][external force and torque block]].
|
||||
|
||||
Internal force, acting reciprocally between base and following origins is implemented using the [[https://mathworks.com/help/physmod/sm/ref/internalforce.html][internal force block]] even though it is usually included in one joint block.
|
||||
2371
org/simscape_subsystems.org
Normal file
2371
org/simscape_subsystems.org
Normal file
File diff suppressed because it is too large
Load Diff
85
org/simulink_project.org
Normal file
85
org/simulink_project.org
Normal file
@@ -0,0 +1,85 @@
|
||||
#+TITLE: Simulink Project for the NASS
|
||||
:DRAWER:
|
||||
#+STARTUP: overview
|
||||
|
||||
#+LANGUAGE: en
|
||||
#+EMAIL: dehaeze.thomas@gmail.com
|
||||
#+AUTHOR: Dehaeze Thomas
|
||||
|
||||
#+HTML_LINK_HOME: ../index.html
|
||||
#+HTML_LINK_UP: ../index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
||||
|
||||
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
#+PROPERTY: header-args:matlab+ :tangle no
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
|
||||
#+PROPERTY: header-args:shell :eval no-export
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
Simulink Project is used for the study of the NASS using Simscape.
|
||||
|
||||
From the [[https://mathworks.com/products/simulink/projects.html][Simulink project]] mathworks page:
|
||||
#+begin_quote
|
||||
Simulink® and Simulink Projects provide a collaborative, scalable environment that enables teams to manage their files and data in one place.
|
||||
|
||||
With Simulink Projects, you can:
|
||||
- *Collaborate*: Enforce companywide standards such as company tools, libraries, and standard startup and shutdown scripts. Share your work with rich sharing options including MATLAB® toolboxes, email, and archives.
|
||||
- *Automate*: Set up your project environment correctly every time by automating steps such as loading the data, managing the path, and opening the models.
|
||||
- *Integrate with source control*: Enable easy integration with source control and configuration management tools.
|
||||
#+end_quote
|
||||
|
||||
The project can be opened using the =simulinkproject= function:
|
||||
|
||||
#+begin_src matlab
|
||||
simulinkproject('./');
|
||||
#+end_src
|
||||
|
||||
When the project opens, a startup script is ran.
|
||||
The startup script is defined below and is exported to the =project_startup.m= script.
|
||||
#+begin_src matlab :eval no :tangle ../src/project_startup.m
|
||||
project = simulinkproject;
|
||||
projectRoot = project.RootFolder;
|
||||
|
||||
myCacheFolder = fullfile(projectRoot, '.SimulinkCache');
|
||||
myCodeFolder = fullfile(projectRoot, '.SimulinkCode');
|
||||
|
||||
Simulink.fileGenControl('set',...
|
||||
'CacheFolder', myCacheFolder,...
|
||||
'CodeGenFolder', myCodeFolder,...
|
||||
'createDir', true);
|
||||
|
||||
%% Load the Simscape Configuration
|
||||
load('mat/conf_simulink.mat');
|
||||
#+end_src
|
||||
|
||||
When the project closes, it runs the =project_shutdown.m= script defined below.
|
||||
#+begin_src matlab :eval no :tangle ../src/project_shutdown.m
|
||||
Simulink.fileGenControl('reset');
|
||||
#+end_src
|
||||
|
||||
The project also permits to automatically add defined folder to the path when the project is opened.
|
||||
712
org/stewart_platform.org
Normal file
712
org/stewart_platform.org
Normal file
@@ -0,0 +1,712 @@
|
||||
#+TITLE: Stewart Platform - Simscape Model
|
||||
:DRAWER:
|
||||
#+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: <script src="./js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/readtheorg.js"></script>
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :noweb yes
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
Stewart platforms are generated in multiple steps.
|
||||
|
||||
We define 4 important *frames*:
|
||||
- $\{F\}$: Frame fixed to the *Fixed* base and located at the center of its bottom surface.
|
||||
This is used to fix the Stewart platform to some support.
|
||||
- $\{M\}$: Frame fixed to the *Moving* platform and located at the center of its top surface.
|
||||
This is used to place things on top of the Stewart platform.
|
||||
- $\{A\}$: Frame fixed to the fixed base.
|
||||
It defined the center of rotation of the moving platform.
|
||||
- $\{B\}$: Frame fixed to the moving platform.
|
||||
The motion of the moving platforms and forces applied to it are defined with respect to this frame $\{B\}$.
|
||||
|
||||
Then, we define the *location of the spherical joints*:
|
||||
- $\bm{a}_{i}$ are the position of the spherical joints fixed to the fixed base
|
||||
- $\bm{b}_{i}$ are the position of the spherical joints fixed to the moving platform
|
||||
|
||||
We define the *rest position* of the Stewart platform:
|
||||
- For simplicity, we suppose that the fixed base and the moving platform are parallel and aligned with the vertical axis at their rest position.
|
||||
- Thus, to define the rest position of the Stewart platform, we just have to defined its total height $H$.
|
||||
$H$ corresponds to the distance from the bottom of the fixed base to the top of the moving platform.
|
||||
|
||||
From $\bm{a}_{i}$ and $\bm{b}_{i}$, we can determine the *length and orientation of each strut*:
|
||||
- $l_{i}$ is the length of the strut
|
||||
- ${}^{A}\hat{\bm{s}}_{i}$ is the unit vector align with the strut
|
||||
|
||||
The position of the Spherical joints can be computed using various methods:
|
||||
- Cubic configuration
|
||||
- Circular configuration
|
||||
- Arbitrary position
|
||||
- These methods should be easily scriptable and corresponds to specific functions that returns ${}^{F}\bm{a}_{i}$ and ${}^{M}\bm{b}_{i}$.
|
||||
The input of these functions are the parameters corresponding to the wanted geometry.
|
||||
|
||||
For Simscape, we need:
|
||||
- The position and orientation of each spherical joint fixed to the fixed base: ${}^{F}\bm{a}_{i}$ and ${}^{F}\bm{R}_{a_{i}}$
|
||||
- The position and orientation of each spherical joint fixed to the moving platform: ${}^{M}\bm{b}_{i}$ and ${}^{M}\bm{R}_{b_{i}}$
|
||||
- The rest length of each strut: $l_{i}$
|
||||
- The stiffness and damping of each actuator: $k_{i}$ and $c_{i}$
|
||||
- The position of the frame $\{A\}$ with respect to the frame $\{F\}$: ${}^{F}\bm{O}_{A}$
|
||||
- The position of the frame $\{B\}$ with respect to the frame $\{M\}$: ${}^{M}\bm{O}_{B}$
|
||||
|
||||
* =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M}
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/initializeFramesPositions.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:initializeFramesPositions>>
|
||||
|
||||
This Matlab function is accessible [[file:src/initializeFramesPositions.m][here]].
|
||||
|
||||
** Function description
|
||||
#+begin_src matlab
|
||||
function [stewart] = initializeFramesPositions(args)
|
||||
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
|
||||
%
|
||||
% Syntax: [stewart] = initializeFramesPositions(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Can have the following fields:
|
||||
% - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
|
||||
% - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following fields:
|
||||
% - H [1x1] - Total Height of the Stewart Platform [m]
|
||||
% - FO_M [3x1] - Position of {M} with respect to {F} [m]
|
||||
% - MO_B [3x1] - Position of {B} with respect to {M} [m]
|
||||
% - FO_A [3x1] - Position of {A} with respect to {F} [m]
|
||||
#+end_src
|
||||
|
||||
** Documentation
|
||||
|
||||
#+name: fig:stewart-frames-position
|
||||
#+caption: Definition of the position of the frames
|
||||
[[file:figs/stewart-frames-position.png]]
|
||||
|
||||
** Optional Parameters
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
|
||||
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** Initialize the Stewart structure
|
||||
#+begin_src matlab
|
||||
stewart = struct();
|
||||
#+end_src
|
||||
|
||||
** Compute the position of each frame
|
||||
#+begin_src matlab
|
||||
stewart.H = args.H; % Total Height of the Stewart Platform [m]
|
||||
|
||||
stewart.FO_M = [0; 0; stewart.H]; % Position of {M} with respect to {F} [m]
|
||||
|
||||
stewart.MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
|
||||
|
||||
stewart.FO_A = stewart.MO_B + stewart.FO_M; % Position of {A} with respect to {F} [m]
|
||||
#+end_src
|
||||
|
||||
* Initialize the position of the Joints
|
||||
** =generateCubicConfiguration=: Generate a Cubic Configuration
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/generateCubicConfiguration.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:generateCubicConfiguration>>
|
||||
|
||||
This Matlab function is accessible [[file:src/generateCubicConfiguration.m][here]].
|
||||
|
||||
*** Function description
|
||||
#+begin_src matlab
|
||||
function [stewart] = generateCubicConfiguration(stewart, args)
|
||||
% generateCubicConfiguration - Generate a Cubic Configuration
|
||||
%
|
||||
% Syntax: [stewart] = generateCubicConfiguration(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - H [1x1] - Total height of the platform [m]
|
||||
% - args - Can have the following fields:
|
||||
% - Hc [1x1] - Height of the "useful" part of the cube [m]
|
||||
% - FOc [1x1] - Height of the center of the cube with respect to {F} [m]
|
||||
% - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m]
|
||||
% - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
#+end_src
|
||||
|
||||
*** Documentation
|
||||
#+name: fig:cubic-configuration-definition
|
||||
#+caption: Cubic Configuration
|
||||
[[file:figs/cubic-configuration-definition.png]]
|
||||
|
||||
*** Optional Parameters
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3
|
||||
args.FOc (1,1) double {mustBeNumeric} = 50e-3
|
||||
args.FHa (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.MHb (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Position of the Cube
|
||||
We define the useful points of the cube with respect to the Cube's center.
|
||||
${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is
|
||||
located at the center of the cube and aligned with {F} and {M}.
|
||||
|
||||
#+begin_src matlab
|
||||
sx = [ 2; -1; -1];
|
||||
sy = [ 0; 1; -1];
|
||||
sz = [ 1; 1; 1];
|
||||
|
||||
R = [sx, sy, sz]./vecnorm([sx, sy, sz]);
|
||||
|
||||
L = args.Hc*sqrt(3);
|
||||
|
||||
Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*args.Hc];
|
||||
|
||||
CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg
|
||||
CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg
|
||||
#+end_src
|
||||
|
||||
*** Compute the pose
|
||||
We can compute the vector of each leg ${}^{C}\hat{\bm{s}}_{i}$ (unit vector from ${}^{C}C_{f}$ to ${}^{C}C_{m}$).
|
||||
#+begin_src matlab
|
||||
CSi = (CCm - CCf)./vecnorm(CCm - CCf);
|
||||
#+end_src
|
||||
|
||||
We now which to compute the position of the joints $a_{i}$ and $b_{i}$.
|
||||
#+begin_src matlab
|
||||
stewart.Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
|
||||
stewart.Mb = CCf + [0; 0; args.FOc-stewart.H] + ((stewart.H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
|
||||
#+end_src
|
||||
|
||||
** =generateGeneralConfiguration=: Generate a Very General Configuration
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/generateGeneralConfiguration.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:generateGeneralConfiguration>>
|
||||
|
||||
This Matlab function is accessible [[file:src/generateGeneralConfiguration.m][here]].
|
||||
|
||||
*** Function description
|
||||
#+begin_src matlab
|
||||
function [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
% generateGeneralConfiguration - Generate a Very General Configuration
|
||||
%
|
||||
% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Can have the following fields:
|
||||
% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
|
||||
% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m]
|
||||
% - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
|
||||
% - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
|
||||
% - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m]
|
||||
% - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
#+end_src
|
||||
|
||||
*** Documentation
|
||||
Joints are positions on a circle centered with the Z axis of {F} and {M} and at a chosen distance from {F} and {M}.
|
||||
The radius of the circles can be chosen as well as the angles where the joints are located.
|
||||
|
||||
*** Optional Parameters
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
|
||||
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
|
||||
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 70e-3;
|
||||
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Compute the pose
|
||||
#+begin_src matlab
|
||||
stewart.Fa = zeros(3,6);
|
||||
stewart.Mb = zeros(3,6);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
for i = 1:6
|
||||
stewart.Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
|
||||
stewart.Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
|
||||
end
|
||||
#+end_src
|
||||
|
||||
* =computeJointsPose=: Compute the Pose of the Joints
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/computeJointsPose.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:computeJointsPose>>
|
||||
|
||||
This Matlab function is accessible [[file:src/computeJointsPose.m][here]].
|
||||
|
||||
** Function description
|
||||
#+begin_src matlab
|
||||
function [stewart] = computeJointsPose(stewart)
|
||||
% computeJointsPose -
|
||||
%
|
||||
% Syntax: [stewart] = computeJointsPose(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
% - FO_A [3x1] - Position of {A} with respect to {F}
|
||||
% - MO_B [3x1] - Position of {B} with respect to {M}
|
||||
% - FO_M [3x1] - Position of {M} with respect to {F}
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following added fields
|
||||
% - Aa [3x6] - The i'th column is the position of ai with respect to {A}
|
||||
% - Ab [3x6] - The i'th column is the position of bi with respect to {A}
|
||||
% - Ba [3x6] - The i'th column is the position of ai with respect to {B}
|
||||
% - Bb [3x6] - The i'th column is the position of bi with respect to {B}
|
||||
% - l [6x1] - The i'th element is the initial length of strut i
|
||||
% - As [3x6] - The i'th column is the unit vector of strut i expressed in {A}
|
||||
% - Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}
|
||||
% - FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
|
||||
% - MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
|
||||
#+end_src
|
||||
|
||||
** Documentation
|
||||
|
||||
#+name: fig:stewart-struts
|
||||
#+caption: Position and orientation of the struts
|
||||
[[file:figs/stewart-struts.png]]
|
||||
|
||||
** Compute the position of the Joints
|
||||
#+begin_src matlab
|
||||
stewart.Aa = stewart.Fa - repmat(stewart.FO_A, [1, 6]);
|
||||
stewart.Bb = stewart.Mb - repmat(stewart.MO_B, [1, 6]);
|
||||
|
||||
stewart.Ab = stewart.Bb - repmat(-stewart.MO_B-stewart.FO_M+stewart.FO_A, [1, 6]);
|
||||
stewart.Ba = stewart.Aa - repmat( stewart.MO_B+stewart.FO_M-stewart.FO_A, [1, 6]);
|
||||
#+end_src
|
||||
|
||||
** Compute the strut length and orientation
|
||||
#+begin_src matlab
|
||||
stewart.As = (stewart.Ab - stewart.Aa)./vecnorm(stewart.Ab - stewart.Aa); % As_i is the i'th vector of As
|
||||
|
||||
stewart.l = vecnorm(stewart.Ab - stewart.Aa)';
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
stewart.Bs = (stewart.Bb - stewart.Ba)./vecnorm(stewart.Bb - stewart.Ba);
|
||||
#+end_src
|
||||
|
||||
** Compute the orientation of the Joints
|
||||
#+begin_src matlab
|
||||
stewart.FRa = zeros(3,3,6);
|
||||
stewart.MRb = zeros(3,3,6);
|
||||
|
||||
for i = 1:6
|
||||
stewart.FRa(:,:,i) = [cross([0;1;0], stewart.As(:,i)) , cross(stewart.As(:,i), cross([0;1;0], stewart.As(:,i))) , stewart.As(:,i)];
|
||||
stewart.FRa(:,:,i) = stewart.FRa(:,:,i)./vecnorm(stewart.FRa(:,:,i));
|
||||
|
||||
stewart.MRb(:,:,i) = [cross([0;1;0], stewart.Bs(:,i)) , cross(stewart.Bs(:,i), cross([0;1;0], stewart.Bs(:,i))) , stewart.Bs(:,i)];
|
||||
stewart.MRb(:,:,i) = stewart.MRb(:,:,i)./vecnorm(stewart.MRb(:,:,i));
|
||||
end
|
||||
#+end_src
|
||||
|
||||
* =initializeStrutDynamics=: Add Stiffness and Damping properties of each strut
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/initializeStrutDynamics.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:initializeStrutDynamics>>
|
||||
|
||||
This Matlab function is accessible [[file:src/initializeStrutDynamics.m][here]].
|
||||
|
||||
** Function description
|
||||
#+begin_src matlab
|
||||
function [stewart] = initializeStrutDynamics(stewart, args)
|
||||
% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
|
||||
%
|
||||
% Syntax: [stewart] = initializeStrutDynamics(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Ki [6x1] - Stiffness of each strut [N/m]
|
||||
% - Ci [6x1] - Damping of each strut [N/(m/s)]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - Ki [6x1] - Stiffness of each strut [N/m]
|
||||
% - Ci [6x1] - Damping of each strut [N/(m/s)]
|
||||
#+end_src
|
||||
|
||||
** Optional Parameters
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1)
|
||||
args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e3*ones(6,1)
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** Add Stiffness and Damping properties of each strut
|
||||
#+begin_src matlab
|
||||
stewart.Ki = args.Ki;
|
||||
stewart.Ci = args.Ci;
|
||||
#+end_src
|
||||
|
||||
* =computeJacobian=: Compute the Jacobian Matrix
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/computeJacobian.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:computeJacobian>>
|
||||
|
||||
This Matlab function is accessible [[file:src/computeJacobian.m][here]].
|
||||
|
||||
** Function description
|
||||
#+begin_src matlab
|
||||
function [stewart] = computeJacobian(stewart)
|
||||
% computeJacobian -
|
||||
%
|
||||
% Syntax: [stewart] = computeJacobian(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - With at least the following fields:
|
||||
% - As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
||||
% - Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - With the 3 added field:
|
||||
% - J [6x6] - The Jacobian Matrix
|
||||
% - K [6x6] - The Stiffness Matrix
|
||||
% - C [6x6] - The Compliance Matrix
|
||||
#+end_src
|
||||
|
||||
** Compute Jacobian Matrix
|
||||
#+begin_src matlab
|
||||
stewart.J = [stewart.As' , cross(stewart.Ab, stewart.As)'];
|
||||
#+end_src
|
||||
|
||||
** Compute Stiffness Matrix
|
||||
#+begin_src matlab
|
||||
stewart.K = stewart.J'*diag(stewart.Ki)*stewart.J;
|
||||
#+end_src
|
||||
|
||||
** Compute Compliance Matrix
|
||||
#+begin_src matlab
|
||||
stewart.C = inv(stewart.K);
|
||||
#+end_src
|
||||
|
||||
* Initialize the Geometry of the Mechanical Elements
|
||||
** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/initializeCylindricalPlatforms.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:initializeCylindricalPlatforms>>
|
||||
|
||||
This Matlab function is accessible [[file:src/initializeCylindricalPlatforms.m][here]].
|
||||
|
||||
*** Function description
|
||||
#+begin_src matlab
|
||||
function [stewart] = initializeCylindricalPlatforms(stewart, args)
|
||||
% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
|
||||
%
|
||||
% Syntax: [stewart] = initializeCylindricalPlatforms(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Fpm [1x1] - Fixed Platform Mass [kg]
|
||||
% - Fph [1x1] - Fixed Platform Height [m]
|
||||
% - Fpr [1x1] - Fixed Platform Radius [m]
|
||||
% - Mpm [1x1] - Mobile Platform Mass [kg]
|
||||
% - Mph [1x1] - Mobile Platform Height [m]
|
||||
% - Mpr [1x1] - Mobile Platform Radius [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - platforms [struct] - structure with the following fields:
|
||||
% - Fpm [1x1] - Fixed Platform Mass [kg]
|
||||
% - Msi [3x3] - Mobile Platform Inertia matrix [kg*m^2]
|
||||
% - Fph [1x1] - Fixed Platform Height [m]
|
||||
% - Fpr [1x1] - Fixed Platform Radius [m]
|
||||
% - Mpm [1x1] - Mobile Platform Mass [kg]
|
||||
% - Fsi [3x3] - Fixed Platform Inertia matrix [kg*m^2]
|
||||
% - Mph [1x1] - Mobile Platform Height [m]
|
||||
% - Mpr [1x1] - Mobile Platform Radius [m]
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3
|
||||
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Create the =platforms= struct
|
||||
#+begin_src matlab
|
||||
platforms = struct();
|
||||
|
||||
platforms.Fpm = args.Fpm;
|
||||
platforms.Fph = args.Fph;
|
||||
platforms.Fpr = args.Fpr;
|
||||
platforms.Fpi = diag([1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ...
|
||||
1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ...
|
||||
1/2 * platforms.Fpm * platforms.Fpr^2]);
|
||||
|
||||
platforms.Mpm = args.Mpm;
|
||||
platforms.Mph = args.Mph;
|
||||
platforms.Mpr = args.Mpr;
|
||||
platforms.Mpi = diag([1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ...
|
||||
1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ...
|
||||
1/2 * platforms.Mpm * platforms.Mpr^2]);
|
||||
#+end_src
|
||||
|
||||
*** Save the =platforms= struct
|
||||
#+begin_src matlab
|
||||
stewart.platforms = platforms;
|
||||
#+end_src
|
||||
|
||||
** =initializeCylindricalStruts=: Define the mass and moment of inertia of cylindrical struts
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/initializeCylindricalStruts.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:initializeCylindricalStruts>>
|
||||
|
||||
This Matlab function is accessible [[file:src/initializeCylindricalStruts.m][here]].
|
||||
|
||||
*** Function description
|
||||
#+begin_src matlab
|
||||
function [stewart] = initializeCylindricalStruts(stewart, args)
|
||||
% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
|
||||
%
|
||||
% Syntax: [stewart] = initializeCylindricalStruts(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
|
||||
% - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
|
||||
% - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||||
% - Msm [1x1] - Mass of the Mobile part of the struts [kg]
|
||||
% - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
|
||||
% - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - struts [struct] - structure with the following fields:
|
||||
% - Fsm [6x1] - Mass of the Fixed part of the struts [kg]
|
||||
% - Fsi [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
|
||||
% - Msm [6x1] - Mass of the Mobile part of the struts [kg]
|
||||
% - Msi [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
|
||||
% - Fsh [6x1] - Height of cylinder for the Fixed part of the struts [m]
|
||||
% - Fsr [6x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||||
% - Msh [6x1] - Height of cylinder for the Mobile part of the struts [m]
|
||||
% - Msr [6x1] - Radius of cylinder for the Mobile part of the struts [m]
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
||||
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
||||
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
||||
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
||||
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
||||
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Create the =struts= structure
|
||||
#+begin_src matlab
|
||||
struts = struct();
|
||||
|
||||
struts.Fsm = ones(6,1).*args.Fsm;
|
||||
struts.Msm = ones(6,1).*args.Msm;
|
||||
|
||||
struts.Fsh = ones(6,1).*args.Fsh;
|
||||
struts.Fsr = ones(6,1).*args.Fsr;
|
||||
struts.Msh = ones(6,1).*args.Msh;
|
||||
struts.Msr = ones(6,1).*args.Msr;
|
||||
|
||||
struts.Fsi = zeros(3, 3, 6);
|
||||
struts.Msi = zeros(3, 3, 6);
|
||||
for i = 1:6
|
||||
struts.Fsi(:,:,i) = diag([1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ...
|
||||
1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ...
|
||||
1/2 * struts.Fsm(i) * struts.Fsr(i)^2]);
|
||||
struts.Msi(:,:,i) = diag([1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ...
|
||||
1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ...
|
||||
1/2 * struts.Msm(i) * struts.Msr(i)^2]);
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
stewart.struts = struts;
|
||||
#+end_src
|
||||
|
||||
* Utility Functions
|
||||
** =inverseKinematics=: Compute Inverse Kinematics
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/inverseKinematics.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:inverseKinematics>>
|
||||
|
||||
This Matlab function is accessible [[file:src/inverseKinematics.m][here]].
|
||||
|
||||
*** Function description
|
||||
#+begin_src matlab
|
||||
function [Li, dLi] = inverseKinematics(stewart, args)
|
||||
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
|
||||
%
|
||||
% Syntax: [stewart] = inverseKinematics(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - Aa [3x6] - The positions ai expressed in {A}
|
||||
% - Bb [3x6] - The positions bi expressed in {B}
|
||||
% - args - Can have the following fields:
|
||||
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
||||
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||||
%
|
||||
% Outputs:
|
||||
% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Theory
|
||||
For inverse kinematic analysis, it is assumed that the position ${}^A\bm{P}$ and orientation of the moving platform ${}^A\bm{R}_B$ are given and the problem is to obtain the joint variables, namely, $\bm{L} = [l_1, l_2, \dots, l_6]^T$.
|
||||
|
||||
From the geometry of the manipulator, the loop closure for each limb, $i = 1, 2, \dots, 6$ can be written as
|
||||
\begin{align*}
|
||||
l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\
|
||||
&= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i
|
||||
\end{align*}
|
||||
|
||||
To obtain the length of each actuator and eliminate $\hat{\bm{s}}_i$, it is sufficient to dot multiply each side by itself:
|
||||
\begin{equation}
|
||||
l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]
|
||||
\end{equation}
|
||||
|
||||
Hence, for $i = 1, 2, \dots, 6$, each limb length can be uniquely determined by:
|
||||
\begin{equation}
|
||||
l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i}
|
||||
\end{equation}
|
||||
|
||||
If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation.
|
||||
Otherwise, when the limbs' lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable.
|
||||
|
||||
*** Compute
|
||||
#+begin_src matlab
|
||||
Li = sqrt(args.AP'*args.AP + diag(stewart.Bb'*stewart.Bb) + diag(stewart.Aa'*stewart.Aa) - (2*args.AP'*stewart.Aa)' + (2*args.AP'*(args.ARB*stewart.Bb))' - diag(2*(args.ARB*stewart.Bb)'*stewart.Aa));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
dLi = Li-stewart.l;
|
||||
#+end_src
|
||||
|
||||
** =forwardKinematicsApprox=: Compute the Forward Kinematics
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/forwardKinematicsApprox.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:forwardKinematicsApprox>>
|
||||
|
||||
This Matlab function is accessible [[file:src/forwardKinematicsApprox.m][here]].
|
||||
|
||||
*** Function description
|
||||
#+begin_src matlab
|
||||
function [P, R] = forwardKinematicsApprox(stewart, args)
|
||||
% forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using
|
||||
% the Jacobian Matrix
|
||||
%
|
||||
% Syntax: [P, R] = forwardKinematicsApprox(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - J [6x6] - The Jacobian Matrix
|
||||
% - args - Can have the following fields:
|
||||
% - dL [6x1] - Displacement of each strut [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - P [3x1] - The estimated position of {B} with respect to {A}
|
||||
% - R [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Computation
|
||||
From a small displacement of each strut $d\bm{\mathcal{L}}$, we can compute the
|
||||
position and orientation of {B} with respect to {A} using the following formula:
|
||||
\[ d \bm{\mathcal{X}} = \bm{J}^{-1} d\bm{\mathcal{L}} \]
|
||||
#+begin_src matlab
|
||||
X = stewart.J\args.dL;
|
||||
#+end_src
|
||||
|
||||
The position vector corresponds to the first 3 elements.
|
||||
#+begin_src matlab
|
||||
P = X(1:3);
|
||||
#+end_src
|
||||
|
||||
The next 3 elements are the orientation of {B} with respect to {A} expressed
|
||||
using the screw axis.
|
||||
#+begin_src matlab
|
||||
theta = norm(X(4:6));
|
||||
s = X(4:6)/theta;
|
||||
#+end_src
|
||||
|
||||
We then compute the corresponding rotation matrix.
|
||||
#+begin_src matlab
|
||||
R = [s(1)^2*(1-cos(theta)) + cos(theta) , s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta);
|
||||
s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta), s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
|
||||
s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];
|
||||
#+end_src
|
||||
3214
org/uniaxial.org
Normal file
3214
org/uniaxial.org
Normal file
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user