5484 lines
168 KiB
Org Mode
5484 lines
168 KiB
Org Mode
#+TITLE: Nano-Hexapod - Test Bench
|
|
:DRAWER:
|
|
#+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="https://research.tdehaeze.xyz/css/style.css"/>
|
|
#+HTML_HEAD: <script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>
|
|
|
|
#+BIND: org-latex-image-default-option "scale=1"
|
|
#+BIND: org-latex-image-default-width ""
|
|
|
|
#+LaTeX_CLASS: scrreprt
|
|
#+LaTeX_CLASS_OPTIONS: [a4paper, 10pt, DIV=12, parskip=full]
|
|
#+LaTeX_HEADER_EXTRA: \input{preamble.tex}
|
|
|
|
#+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
|
|
|
|
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/tikz/org/}{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 file raw replace
|
|
#+PROPERTY: header-args:latex+ :buffer no
|
|
#+PROPERTY: header-args:latex+ :tangle no
|
|
#+PROPERTY: header-args:latex+ :eval no-export
|
|
#+PROPERTY: header-args:latex+ :exports results
|
|
#+PROPERTY: header-args:latex+ :mkdirp yes
|
|
#+PROPERTY: header-args:latex+ :output-dir figs
|
|
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
|
|
:END:
|
|
|
|
#+begin_export html
|
|
<hr>
|
|
<p>This report is also available as a <a href="./test-bench-nano-hexapod.pdf">pdf</a>.</p>
|
|
<hr>
|
|
#+end_export
|
|
|
|
#+latex: \clearpage
|
|
|
|
* Introduction :ignore:
|
|
This document is dedicated to the experimental study of the nano-hexapod shown in Figure [[fig:picture_bench_granite_nano_hexapod]].
|
|
|
|
#+name: fig:picture_bench_granite_nano_hexapod
|
|
#+caption: Nano-Hexapod
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/IMG_20210608_152917.jpg]]
|
|
|
|
#+begin_note
|
|
Here are the documentation of the equipment used for this test bench (lots of them are shwon in Figure [[fig:picture_bench_granite_overview]]):
|
|
- Voltage Amplifier: PiezoDrive [[file:doc/PD200-V7-R1.pdf][PD200]]
|
|
- Amplified Piezoelectric Actuator: Cedrat [[file:doc/APA300ML.pdf][APA300ML]]
|
|
- DAC/ADC: Speedgoat [[file:doc/IO131-OEM-Datasheet.pdf][IO313]]
|
|
- Encoder: Renishaw [[file:doc/L-9517-9678-05-A_Data_sheet_VIONiC_series_en.pdf][Vionic]] and used [[file:doc/L-9517-9862-01-C_Data_sheet_RKLC_EN.pdf][Ruler]]
|
|
- Interferometers: Attocube
|
|
#+end_note
|
|
|
|
#+name: fig:picture_bench_granite_overview
|
|
#+caption: Nano-Hexapod and the control electronics
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/IMG_20210608_154722.jpg]]
|
|
|
|
In Figure [[fig:nano_hexapod_signals]] is shown a block diagram of the experimental setup.
|
|
When possible, the notations are consistent with this diagram and summarized in Table [[tab:list_signals]].
|
|
|
|
#+begin_src latex :file nano_hexapod_signals.pdf
|
|
\definecolor{instrumentation}{rgb}{0, 0.447, 0.741}
|
|
\definecolor{mechanics}{rgb}{0.8500, 0.325, 0.098}
|
|
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block={4.0cm}{3.0cm}, fill=mechanics!20!white] (nano_hexapod) {Mechanics};
|
|
\coordinate[] (inputF) at (nano_hexapod.west);
|
|
\coordinate[] (outputL) at ($(nano_hexapod.south east)!0.8!(nano_hexapod.north east)$);
|
|
\coordinate[] (outputF) at ($(nano_hexapod.south east)!0.2!(nano_hexapod.north east)$);
|
|
|
|
\node[block, left= 0.8 of inputF, fill=instrumentation!20!white, align=center] (F_stack) {\tiny Actuator \\ \tiny stacks};
|
|
\node[block, left= 0.8 of F_stack, fill=instrumentation!20!white] (PD200) {PD200};
|
|
\node[DAC, left= 0.8 of PD200, fill=instrumentation!20!white] (F_DAC) {DAC};
|
|
\node[block, right=0.8 of outputF, fill=instrumentation!20!white, align=center] (Fm_stack){\tiny Sensor \\ \tiny stack};
|
|
\node[ADC, right=0.8 of Fm_stack,fill=instrumentation!20!white] (Fm_ADC) {ADC};
|
|
\node[block, right=0.8 of outputL, fill=instrumentation!20!white] (encoder) {\tiny Encoder};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(F_DAC.west)+(-0.8,0)$) node[above right]{$\bm{u}$} node[below right]{$[V]$} -- node[sloped]{$/$} (F_DAC.west);
|
|
\draw[->] (F_DAC.east) -- node[midway, above]{$\tilde{\bm{u}}$}node[midway, below]{$[V]$} (PD200.west);
|
|
\draw[->] (PD200.east) -- node[midway, above]{$\bm{u}_a$}node[midway, below]{$[V]$} (F_stack.west);
|
|
\draw[->] (F_stack.east) -- (inputF) node[above left]{$\bm{\tau}$}node[below left]{$[N]$};
|
|
|
|
\draw[->] (outputF) -- (Fm_stack.west) node[above left]{$\bm{\epsilon}$} node[below left]{$[m]$};
|
|
\draw[->] (Fm_stack.east) -- node[midway, above]{$\tilde{\bm{\tau}}_m$}node[midway, below]{$[V]$} (Fm_ADC.west);
|
|
\draw[->] (Fm_ADC.east) -- node[sloped]{$/$} ++(0.8, 0)coordinate(end) node[above left]{$\bm{\tau}_m$}node[below left]{$[V]$};
|
|
|
|
\draw[->] (outputL) -- (encoder.west) node[above left]{$d\bm{\mathcal{L}}$} node[below left]{$[m]$};
|
|
\draw[->] (encoder.east) -- node[sloped]{$/$} (encoder-|end) node[above left]{$d\bm{\mathcal{L}}_m$}node[below left]{$[m]$};
|
|
|
|
% Nano-Hexapod
|
|
\begin{scope}[on background layer]
|
|
\node[fit={(F_stack.west|-nano_hexapod.south) (Fm_stack.east|-nano_hexapod.north)}, fill=black!20!white, draw, inner sep=2pt] (system) {};
|
|
\node[above] at (system.north) {Nano-Hexapod};
|
|
\end{scope}
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_signals
|
|
#+caption: Block diagram of the system with named signals
|
|
#+attr_latex: :scale 1
|
|
[[file:figs/nano_hexapod_signals.png]]
|
|
|
|
#+name: tab:list_signals
|
|
#+caption: List of signals
|
|
#+attr_latex: :environment tabularx :width \linewidth :align Xllll
|
|
#+attr_latex: :center t :booktabs t :float t
|
|
| | *Unit* | *Matlab* | *Vector* | *Elements* |
|
|
|------------------------------------+-----------+-----------+-----------------------+----------------------|
|
|
| Control Input (wanted DAC voltage) | =[V]= | =u= | $\bm{u}$ | $u_i$ |
|
|
| DAC Output Voltage | =[V]= | =u= | $\tilde{\bm{u}}$ | $\tilde{u}_i$ |
|
|
| PD200 Output Voltage | =[V]= | =ua= | $\bm{u}_a$ | $u_{a,i}$ |
|
|
| Actuator applied force | =[N]= | =tau= | $\bm{\tau}$ | $\tau_i$ |
|
|
|------------------------------------+-----------+-----------+-----------------------+----------------------|
|
|
| Strut motion | =[m]= | =dL= | $d\bm{\mathcal{L}}$ | $d\mathcal{L}_i$ |
|
|
| Encoder measured displacement | =[m]= | =dLm= | $d\bm{\mathcal{L}}_m$ | $d\mathcal{L}_{m,i}$ |
|
|
|------------------------------------+-----------+-----------+-----------------------+----------------------|
|
|
| Force Sensor strain | =[m]= | =epsilon= | $\bm{\epsilon}$ | $\epsilon_i$ |
|
|
| Force Sensor Generated Voltage | =[V]= | =taum= | $\tilde{\bm{\tau}}_m$ | $\tilde{\tau}_{m,i}$ |
|
|
| Measured Generated Voltage | =[V]= | =taum= | $\bm{\tau}_m$ | $\tau_{m,i}$ |
|
|
|------------------------------------+-----------+-----------+-----------------------+----------------------|
|
|
| Motion of the top platform | =[m,rad]= | =dX= | $d\bm{\mathcal{X}}$ | $d\mathcal{X}_i$ |
|
|
| Metrology measured displacement | =[m,rad]= | =dXm= | $d\bm{\mathcal{X}}_m$ | $d\mathcal{X}_{m,i}$ |
|
|
|
|
This document is divided in the following sections:
|
|
- Section [[sec:encoders_struts]]: the encoders are fixed to the struts
|
|
- Section [[sec:encoders_plates]]: the encoders are fixed to the plates
|
|
|
|
* Encoders fixed to the Struts
|
|
<<sec:encoders_struts>>
|
|
|
|
** Introduction
|
|
In this section, the encoders are fixed to the struts.
|
|
|
|
It is divided in the following sections:
|
|
- Section [[sec:enc_struts_plant_id]]: the transfer function matrix from the actuators to the force sensors and to the encoders is experimentally identified.
|
|
- Section [[sec:enc_struts_comp_simscape]]: the obtained FRF matrix is compared with the dynamics of the simscape model
|
|
- Section [[sec:enc_struts_iff]]: decentralized Integral Force Feedback (IFF) is applied and its performances are evaluated.
|
|
- Section [[sec:enc_struts_modal_analysis]]: a modal analysis of the nano-hexapod is performed
|
|
|
|
** Identification of the dynamics
|
|
<<sec:enc_struts_plant_id>>
|
|
*** 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
|
|
addpath('./matlab/mat/');
|
|
addpath('./matlab/src/');
|
|
addpath('./matlab/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
addpath('./mat/');
|
|
addpath('./src/');
|
|
#+end_src
|
|
|
|
*** Load Measurement Data
|
|
#+begin_src matlab
|
|
%% Load Identification Data
|
|
meas_data_lf = {};
|
|
|
|
for i = 1:6
|
|
meas_data_lf(i) = {load(sprintf('mat/frf_data_exc_strut_%i_noise_lf.mat', i), 't', 'Va', 'Vs', 'de')};
|
|
meas_data_hf(i) = {load(sprintf('mat/frf_data_exc_strut_%i_noise_hf.mat', i), 't', 'Va', 'Vs', 'de')};
|
|
end
|
|
#+end_src
|
|
|
|
*** Spectral Analysis - Setup
|
|
#+begin_src matlab
|
|
%% Setup useful variables
|
|
% Sampling Time [s]
|
|
Ts = (meas_data_lf{1}.t(end) - (meas_data_lf{1}.t(1)))/(length(meas_data_lf{1}.t)-1);
|
|
|
|
% Sampling Frequency [Hz]
|
|
Fs = 1/Ts;
|
|
|
|
% Hannning Windows
|
|
win = hanning(ceil(1*Fs));
|
|
|
|
% And we get the frequency vector
|
|
[~, f] = tfestimate(meas_data_lf{1}.Va, meas_data_lf{1}.de, win, [], [], 1/Ts);
|
|
|
|
i_lf = f < 250; % Points for low frequency excitation
|
|
i_hf = f > 250; % Points for high frequency excitation
|
|
#+end_src
|
|
|
|
*** DVF Plant
|
|
First, let's compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure [[fig:enc_struts_dvf_coh]]).
|
|
|
|
#+begin_src matlab
|
|
%% Coherence
|
|
coh_dvf = zeros(length(f), 6, 6);
|
|
|
|
for i = 1:6
|
|
coh_dvf_lf = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts);
|
|
coh_dvf_hf = mscohere(meas_data_hf{i}.Va, meas_data_hf{i}.de, win, [], [], 1/Ts);
|
|
coh_dvf(:,:,i) = [coh_dvf_lf(i_lf, :); coh_dvf_hf(i_hf, :)];
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Coherence for the transfer function from u to dLm
|
|
figure;
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, coh_dvf(:, i, j), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, coh_dvf(:, i, i), ...
|
|
'DisplayName', sprintf('$G_{dvf}(%i,%i)$', i, i));
|
|
end
|
|
plot(f, coh_dvf(:, 1, 2), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$G_{dvf}(i,j)$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
|
|
xlim([20, 2e3]); ylim([0, 1]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_dvf_coh.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_dvf_coh
|
|
#+caption: Obtained coherence for the DVF plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_dvf_coh.png]]
|
|
|
|
Then the 6x6 transfer function matrix is estimated (Figure [[fig:enc_struts_dvf_frf]]).
|
|
#+begin_src matlab
|
|
%% DVF Plant (transfer function from u to dLm)
|
|
G_dvf = zeros(length(f), 6, 6);
|
|
|
|
for i = 1:6
|
|
G_dvf_lf = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts);
|
|
G_dvf_hf = tfestimate(meas_data_hf{i}.Va, meas_data_hf{i}.de, win, [], [], 1/Ts);
|
|
G_dvf(:,:,i) = [G_dvf_lf(i_lf, :); G_dvf_hf(i_hf, :)];
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the transfer function from u to dLm
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_dvf(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, abs(G_dvf(:,i, i)), ...
|
|
'DisplayName', sprintf('$G_{dvf}(%i,%i)$', i, i));
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, abs(G_dvf(:,i, i)), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(f, abs(G_dvf(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$G_{dvf}(i,j)$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-3]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, 180/pi*angle(G_dvf(:,i, i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_dvf_frf.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_dvf_frf
|
|
#+caption: Measured FRF for the DVF plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_dvf_frf.png]]
|
|
|
|
*** IFF Plant
|
|
First, let's compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure [[fig:enc_struts_iff_coh]]).
|
|
|
|
#+begin_src matlab
|
|
%% Coherence for the IFF plant
|
|
coh_iff = zeros(length(f), 6, 6);
|
|
|
|
for i = 1:6
|
|
coh_iff_lf = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts);
|
|
coh_iff_hf = mscohere(meas_data_hf{i}.Va, meas_data_hf{i}.Vs, win, [], [], 1/Ts);
|
|
coh_iff(:,:,i) = [coh_iff_lf(i_lf, :); coh_iff_hf(i_hf, :)];
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Coherence of the IFF Plant (transfer function from u to taum)
|
|
figure;
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, coh_iff(:, i, j), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, coh_iff(:,i, i), ...
|
|
'DisplayName', sprintf('$G_{iff}(%i,%i)$', i, i));
|
|
end
|
|
plot(f, coh_iff(:, 1, 2), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$G_{iff}(i,j)$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
|
|
xlim([20, 2e3]); ylim([0, 1]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_iff_coh.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_iff_coh
|
|
#+caption: Obtained coherence for the IFF plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_iff_coh.png]]
|
|
|
|
Then the 6x6 transfer function matrix is estimated (Figure [[fig:enc_struts_iff_frf]]).
|
|
#+begin_src matlab
|
|
%% IFF Plant
|
|
G_iff = zeros(length(f), 6, 6);
|
|
|
|
for i = 1:6
|
|
G_iff_lf = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts);
|
|
G_iff_hf = tfestimate(meas_data_hf{i}.Va, meas_data_hf{i}.Vs, win, [], [], 1/Ts);
|
|
G_iff(:,:,i) = [G_iff_lf(i_lf, :); G_iff_hf(i_hf, :)];
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the IFF Plant (transfer function from u to taum)
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_iff(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, abs(G_iff(:,i , i)), ...
|
|
'DisplayName', sprintf('$G_{iff}(%i,%i)$', i, i));
|
|
end
|
|
plot(f, abs(G_iff(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$G_{iff}(i,j)$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
ylim([1e-3, 1e2]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, 180/pi*angle(G_iff(:,i, i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_iff_frf.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_iff_frf
|
|
#+caption: Measured FRF for the IFF plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_iff_frf.png]]
|
|
|
|
*** Save Identified Plants
|
|
#+begin_src matlab :tangle no
|
|
save('matlab/mat/identified_plants_enc_struts.mat', 'f', 'Ts', 'G_iff', 'G_dvf')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :eval no
|
|
save('mat/identified_plants_enc_struts.mat', 'f', 'Ts', 'G_iff', 'G_dvf')
|
|
#+end_src
|
|
|
|
** Jacobian :noexport:
|
|
*** Introduction :ignore:
|
|
The Jacobian is used to transform the excitation force in the cartesian frame as well as the displacements.
|
|
|
|
Consider the plant shown in Figure [[fig:schematic_jacobian_in_out]] with:
|
|
- $\tau$ the 6 input voltages (going to the PD200 amplifier and then to the APA)
|
|
- $d\mathcal{L}$ the relative motion sensor outputs (encoders)
|
|
- $\bm{\tau}_m$ the generated voltage of the force sensor stacks
|
|
- $J_a$ and $J_s$ the Jacobians for the actuators and sensors
|
|
|
|
#+begin_src latex :file schematic_jacobian_in_out.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block={2.0cm}{2.0cm}] (P) {Plant};
|
|
\coordinate[] (inputF) at (P.west);
|
|
\coordinate[] (outputL) at ($(P.south east)!0.8!(P.north east)$);
|
|
\coordinate[] (outputF) at ($(P.south east)!0.2!(P.north east)$);
|
|
|
|
\node[block, left= of inputF] (Ja) {$\bm{J}^{-T}_a$};
|
|
\node[block, right= of outputL] (Js) {$\bm{J}^{-1}_s$};
|
|
\node[block, right= of outputF] (Jf) {$\bm{J}^{-1}_s$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Ja.west)+(-1,0)$) -- (Ja.west) node[above left]{$\bm{\mathcal{F}}$};
|
|
\draw[->] (Ja.east) -- (inputF) node[above left]{$\bm{\tau}$};
|
|
\draw[->] (outputL) -- (Js.west) node[above left]{$d\bm{\mathcal{L}}$};
|
|
\draw[->] (Js.east) -- ++(1, 0) node[above left]{$d\bm{\mathcal{X}}$};
|
|
\draw[->] (outputF) -- (Jf.west) node[above left]{$\bm{\tau}_m$};
|
|
\draw[->] (Jf.east) -- ++(1, 0) node[above left]{$\bm{\mathcal{F}}_m$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_jacobian_in_out
|
|
#+caption: Plant in the cartesian Frame
|
|
#+RESULTS:
|
|
[[file:figs/schematic_jacobian_in_out.png]]
|
|
|
|
First, we load the Jacobian matrix (same for the actuators and sensors).
|
|
#+begin_src matlab
|
|
load('jacobian.mat', 'J');
|
|
#+end_src
|
|
|
|
*** 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
|
|
addpath('./matlab/mat/');
|
|
addpath('./matlab/src/');
|
|
addpath('./matlab/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
addpath('./mat/');
|
|
addpath('./src/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
load('identified_plants_enc_struts.mat', 'f', 'Ts', 'G_iff', 'G_dvf')
|
|
load('jacobian.mat', 'J');
|
|
#+end_src
|
|
|
|
*** DVF Plant
|
|
The transfer function from $\bm{\mathcal{F}}$ to $d\bm{\mathcal{X}}$ is computed and shown in Figure [[fig:enc_struts_dvf_cart_frf]].
|
|
|
|
#+begin_src matlab
|
|
G_dvf_J = permute(pagemtimes(inv(J), pagemtimes(permute(G_dvf, [2 3 1]), inv(J'))), [3 1 2]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
labels = {'$D_x/F_{x}$', '$D_y/F_{y}$', '$D_z/F_{z}$', '$R_{x}/M_{x}$', '$R_{y}/M_{y}$', '$R_{R}/M_{z}$'};
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_dvf_J(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, abs(G_dvf_J(:,i , i)), ...
|
|
'DisplayName', labels{i});
|
|
end
|
|
plot(f, abs(G_dvf_J(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$D_i/F_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-7, 1e-1]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, 180/pi*angle(G_dvf_J(:,i , i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_dvf_cart_frf.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_dvf_cart_frf
|
|
#+caption: Measured FRF for the DVF plant in the cartesian frame
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_dvf_cart_frf.png]]
|
|
|
|
*** IFF Plant
|
|
The transfer function from $\bm{\mathcal{F}}$ to $\bm{\mathcal{F}}_m$ is computed and shown in Figure [[fig:enc_struts_iff_cart_frf]].
|
|
|
|
#+begin_src matlab
|
|
G_iff_J = permute(pagemtimes(inv(J), pagemtimes(permute(G_iff, [2 3 1]), inv(J'))), [3 1 2]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
labels = {'$F_{m,x}/F_{x}$', '$F_{m,y}/F_{y}$', '$F_{m,z}/F_{z}$', '$M_{m,x}/M_{x}$', '$M_{m,y}/M_{y}$', '$M_{m,z}/M_{z}$'};
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_iff_J(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, abs(G_iff_J(:,i, i)), ...
|
|
'DisplayName', labels{i});
|
|
end
|
|
plot(f, abs(G_iff_J(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$D_i/F_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-3, 1e4]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, 180/pi*angle(G_iff_J(:,i, i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_iff_cart_frf.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_iff_cart_frf
|
|
#+caption: Measured FRF for the IFF plant in the cartesian frame
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_iff_cart_frf.png]]
|
|
|
|
** Comparison with the Simscape Model
|
|
<<sec:enc_struts_comp_simscape>>
|
|
*** Introduction :ignore:
|
|
In this section, the measured dynamics is compared with the dynamics estimated from the Simscape model.
|
|
|
|
*** 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
|
|
addpath('./matlab/mat/');
|
|
addpath('./matlab/src/');
|
|
addpath('./matlab/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
addpath('./mat/');
|
|
addpath('./src/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
%% Add all useful folders to the path
|
|
addpath('matlab/nass-simscape/matlab/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/png/')
|
|
addpath('matlab/nass-simscape/src/')
|
|
addpath('matlab/nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
%% Add all useful folders to the path
|
|
addpath('nass-simscape/matlab/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/png/')
|
|
addpath('nass-simscape/src/')
|
|
addpath('nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Open Simulink Model
|
|
mdl = 'nano_hexapod_simscape';
|
|
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
Rx = zeros(1, 7);
|
|
|
|
open(mdl)
|
|
#+end_src
|
|
|
|
*** Load measured FRF
|
|
|
|
#+begin_src matlab
|
|
%% Load data
|
|
load('identified_plants_enc_struts.mat', 'f', 'Ts', 'G_iff', 'G_dvf')
|
|
#+end_src
|
|
|
|
*** Dynamics from Actuator to Force Sensors
|
|
#+begin_src matlab
|
|
%% Initialize Nano-Hexapod
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the IFF Plant (transfer function from u to taum)
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/dum'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors
|
|
|
|
Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the identified IFF Plant (Simscape) and measured FRF data
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(G_iff(:,1, 1)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - FRF')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f, abs(G_iff(:,i, i)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - Model')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(f, 180/pi*angle(G_iff(:,i, i)), 'color', [0,0,0,0.2]);
|
|
end
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(i,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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_iff_comp_simscape.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_iff_comp_simscape
|
|
#+caption: Diagonal elements of the IFF Plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_iff_comp_simscape.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the identified IFF Plant (Simscape) and measured FRF data (off-diagonal elements)
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
% Off diagonal terms
|
|
plot(f, abs(G_iff(:, 1, 2)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$\tau_{m,i}/u_j$ - FRF')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_iff(:, i, j)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(1, 2), freqs, 'Hz'))), ...
|
|
'DisplayName', '$\tau_{m,i}/u_j$ - Model')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(i, j), freqs, 'Hz'))), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [V/V]');
|
|
xlim([freqs(1), freqs(end)]); ylim([1e-3, 1e2]);
|
|
legend('location', 'northeast');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_iff_comp_offdiag_simscape.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_iff_comp_offdiag_simscape
|
|
#+caption: Off diagonal elements of the IFF Plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_iff_comp_offdiag_simscape.png]]
|
|
|
|
*** Dynamics from Actuator to Encoder
|
|
#+begin_src matlab
|
|
%% Initialization of the Nano-Hexapod
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', 'flexible');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the DVF Plant (transfer function from u to dLm)
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
|
|
|
|
Gdvf = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Diagonal elements of the DVF plant
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(G_dvf(:,1, 1)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - FRF')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f, abs(G_dvf(:,i, i)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - Model')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'northeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(f, 180/pi*angle(G_dvf(:,i, i)), 'color', [0,0,0,0.2]);
|
|
end
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(i,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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_dvf_comp_simscape.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_dvf_comp_simscape
|
|
#+caption: Diagonal elements of the DVF Plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_dvf_comp_simscape.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Off-diagonal elements of the DVF plant
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
% Off diagonal terms
|
|
plot(f, abs(G_dvf(:, 1, 2)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - FRF')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_dvf(:, i, j)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1, 2), freqs, 'Hz'))), ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - Model')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i, j), freqs, 'Hz'))), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
|
|
xlim([freqs(1), freqs(end)]); ylim([1e-8, 1e-3]);
|
|
legend('location', 'northeast');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_dvf_comp_offdiag_simscape.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_dvf_comp_offdiag_simscape
|
|
#+caption: Off diagonal elements of the DVF Plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_dvf_comp_offdiag_simscape.png]]
|
|
|
|
*** Effect of a change in bending damping of the joints
|
|
#+begin_src matlab
|
|
%% Tested bending dampings [Nm/(rad/s)]
|
|
cRs = [1e-3, 5e-3, 1e-2, 5e-2, 1e-1];
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the DVF Plant (transfer function from u to dLm)
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
|
|
#+end_src
|
|
|
|
Then the identification is performed for all the values of the bending damping.
|
|
#+begin_src matlab
|
|
%% Idenfity the transfer function from actuator to encoder for all bending dampins
|
|
Gs = {zeros(length(cRs), 1)};
|
|
|
|
for i = 1:length(cRs)
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', 'flexible', ...
|
|
'flex_bot_cRx', cRs(i), ...
|
|
'flex_bot_cRy', cRs(i), ...
|
|
'flex_top_cRx', cRs(i), ...
|
|
'flex_top_cRy', cRs(i));
|
|
|
|
G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
G.InputName = {'Va1', 'Va2', 'Va3', 'Va4', 'Va5', 'Va6'};
|
|
G.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6'};
|
|
|
|
Gs(i) = {G};
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Plot the obtained direct transfer functions for all the bending stiffnesses
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:length(cRs)
|
|
plot(freqs, abs(squeeze(freqresp(Gs{i}('dL1', 'Va1'), freqs, 'Hz'))), ...
|
|
'DisplayName', sprintf('$c_R = %.3f\\,[\\frac{Nm}{rad/s}]$', cRs(i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'southwest');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:length(cRs)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('dL1', 'Va1'), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Plot the obtained coupling transfer functions for all the bending stiffnesses
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
for i = 1:length(cRs)
|
|
plot(freqs, abs(squeeze(freqresp(Gs{i}('dL2', 'Va1'), freqs, 'Hz'))), ...
|
|
'DisplayName', sprintf('$c_R = %.3f\\,[\\frac{Nm}{rad/s}]$', cRs(i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'southwest');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
- Could be nice
|
|
- Actual damping is very small
|
|
|
|
*** Effect of a change in damping factor of the APA
|
|
#+begin_src matlab
|
|
%% Tested bending dampings [Nm/(rad/s)]
|
|
xis = [1e-3, 5e-3, 1e-2, 5e-2, 1e-1];
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the DVF Plant (transfer function from u to dLm)
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Idenfity the transfer function from actuator to encoder for all bending dampins
|
|
Gs = {zeros(length(xis), 1)};
|
|
|
|
for i = 1:length(xis)
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', 'flexible', ...
|
|
'actuator_xi', xis(i));
|
|
|
|
G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
G.InputName = {'Va1', 'Va2', 'Va3', 'Va4', 'Va5', 'Va6'};
|
|
G.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6'};
|
|
|
|
Gs(i) = {G};
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Plot the obtained direct transfer functions for all the bending stiffnesses
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:length(xis)
|
|
plot(freqs, abs(squeeze(freqresp(Gs{i}('dL1', 'Va1'), freqs, 'Hz'))), ...
|
|
'DisplayName', sprintf('$\\xi = %.3f$', xis(i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'southwest');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:length(xis)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('dL1', 'Va1'), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/bode_Va_dL_effect_xi_damp.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:bode_Va_dL_effect_xi_damp
|
|
#+caption: Effect of the APA damping factor $\xi$ on the dynamics from $u$ to $d\mathcal{L}$
|
|
#+RESULTS:
|
|
[[file:figs/bode_Va_dL_effect_xi_damp.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Plot the obtained coupling transfer functions for all the bending stiffnesses
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
for i = 1:length(xis)
|
|
plot(freqs, abs(squeeze(freqresp(Gs{i}('dL2', 'Va1'), freqs, 'Hz'))), ...
|
|
'DisplayName', sprintf('$c_R = %.3f\\,[\\frac{Nm}{rad/s}]$', xis(i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'southwest');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_important
|
|
Damping factor $\xi$ has a large impact on the damping of the "spurious resonances" at 200Hz and 300Hz.
|
|
#+end_important
|
|
|
|
#+begin_question
|
|
Why is the damping factor does not change the damping of the first peak?
|
|
#+end_question
|
|
|
|
*** Effect of a change in stiffness damping coef of the APA
|
|
#+begin_src matlab
|
|
m_coef = 1e1;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Tested bending dampings [Nm/(rad/s)]
|
|
k_coefs = [1e-6, 5e-6, 1e-5, 5e-5, 1e-4];
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the DVF Plant (transfer function from u to dLm)
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Idenfity the transfer function from actuator to encoder for all bending dampins
|
|
Gs = {zeros(length(k_coefs), 1)};
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', 'flexible');
|
|
|
|
for i = 1:length(k_coefs)
|
|
k_coef = k_coefs(i);
|
|
|
|
G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
G.InputName = {'Va1', 'Va2', 'Va3', 'Va4', 'Va5', 'Va6'};
|
|
G.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6'};
|
|
|
|
Gs(i) = {G};
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Plot the obtained direct transfer functions for all the bending stiffnesses
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:length(k_coefs)
|
|
plot(freqs, abs(squeeze(freqresp(Gs{i}('dL1', 'Va1'), freqs, 'Hz'))), ...
|
|
'DisplayName', sprintf('kcoef = %.0e', k_coefs(i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'southwest');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:length(k_coefs)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('dL1', 'Va1'), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/bode_Va_dL_effect_k_coef.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:bode_Va_dL_effect_k_coef
|
|
#+caption: Effect of a change of the damping "stiffness coeficient" on the transfer function from $u$ to $d\mathcal{L}$
|
|
#+RESULTS:
|
|
[[file:figs/bode_Va_dL_effect_k_coef.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Plot the obtained coupling transfer functions for all the bending stiffnesses
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
for i = 1:length(xis)
|
|
plot(freqs, abs(squeeze(freqresp(Gs{i}('dL2', 'Va1'), freqs, 'Hz'))), ...
|
|
'DisplayName', sprintf('$c_R = %.3f\\,[\\frac{Nm}{rad/s}]$', xis(i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'southwest');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
*** Effect of a change in mass damping coef of the APA
|
|
#+begin_src matlab
|
|
k_coef = 1e-6;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Tested bending dampings [Nm/(rad/s)]
|
|
m_coefs = [1e1, 5e1, 1e2, 5e2, 1e3];
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the DVF Plant (transfer function from u to dLm)
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Idenfity the transfer function from actuator to encoder for all bending dampins
|
|
Gs = {zeros(length(m_coefs), 1)};
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', 'flexible');
|
|
|
|
for i = 1:length(m_coefs)
|
|
m_coef = m_coefs(i);
|
|
|
|
G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
G.InputName = {'Va1', 'Va2', 'Va3', 'Va4', 'Va5', 'Va6'};
|
|
G.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6'};
|
|
|
|
Gs(i) = {G};
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Plot the obtained direct transfer functions for all the bending stiffnesses
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:length(m_coefs)
|
|
plot(freqs, abs(squeeze(freqresp(Gs{i}('dL1', 'Va1'), freqs, 'Hz'))), ...
|
|
'DisplayName', sprintf('mcoef = %.0e', m_coefs(i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'southwest');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:length(m_coefs)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('dL1', 'Va1'), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/bode_Va_dL_effect_m_coef.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:bode_Va_dL_effect_m_coef
|
|
#+caption: Effect of a change of the damping "mass coeficient" on the transfer function from $u$ to $d\mathcal{L}$
|
|
#+RESULTS:
|
|
[[file:figs/bode_Va_dL_effect_m_coef.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Plot the obtained coupling transfer functions for all the bending stiffnesses
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
for i = 1:length(xis)
|
|
plot(freqs, abs(squeeze(freqresp(Gs{i}('dL2', 'Va1'), freqs, 'Hz'))), ...
|
|
'DisplayName', sprintf('$c_R = %.3f\\,[\\frac{Nm}{rad/s}]$', xis(i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'southwest');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
*** TODO Using Flexible model
|
|
#+begin_src matlab
|
|
d_aligns = [[-0.05, -0.3, 0];
|
|
[ 0, 0.5, 0];
|
|
[-0.1, -0.3, 0];
|
|
[ 0, 0.3, 0];
|
|
[-0.05, 0.05, 0];
|
|
[0, 0, 0]]*1e-3;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
d_aligns = zeros(6,3);
|
|
% d_aligns(1,:) = [-0.05, -0.3, 0]*1e-3;
|
|
d_aligns(2,:) = [ 0, 0.3, 0]*1e-3;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Initialize Nano-Hexapod
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', 'flexible', ...
|
|
'actuator_d_align', d_aligns);
|
|
#+end_src
|
|
|
|
#+begin_question
|
|
Why do we have smaller resonances when using flexible APA?
|
|
On the test bench we have the same resonance as the 2DoF model.
|
|
Could it be due to the compliance in other dof of the flexible model?
|
|
#+end_question
|
|
|
|
#+begin_src matlab
|
|
%% Identify the DVF Plant (transfer function from u to dLm)
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
|
|
|
|
Gdvf = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Comparison of the plants (encoder output) when tuning the misalignment
|
|
freqs = 2*logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(2, 3, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
ax1 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_dvf(:, 1, 1)));
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');
|
|
|
|
ax2 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_dvf(:, 2, 2)));
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(2,2), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
|
|
ax3 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_dvf(:, 3, 3)), 'DisplayName', 'Meas.');
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(3,3), freqs, 'Hz'))), ...
|
|
'DisplayName', 'Model');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
legend('location', 'southwest', 'FontSize', 8);
|
|
|
|
ax4 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_dvf(:, 4, 4)));
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(4,4), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
|
|
|
|
ax5 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_dvf(:, 5, 5)));
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(5,5), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
|
|
|
ax6 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_dvf(:, 6, 6)));
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(6,6), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
|
|
|
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
|
|
% xlim([20, 2e3]); ylim([1e-8, 1e-3]);
|
|
xlim([50, 5e2]); ylim([1e-6, 1e-3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Diagonal elements of the DVF plant
|
|
freqs = 6*logspace(1, 2, 2000);
|
|
|
|
i_strut = 1;
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(G_dvf(:,i_strut, 2)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - FRF')
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(2,2), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - Model')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'northeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(G_dvf(:,2, 2)), 'color', [0,0,0,0.2]);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(2,2), freqs, 'Hz'))), '-');
|
|
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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Diagonal elements of the DVF plant
|
|
freqs = 6*logspace(1, 2, 2000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(G_dvf(:,1, 1)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - FRF')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f, abs(G_dvf(:,i, i)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - Model')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'northeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(f, 180/pi*angle(G_dvf(:,i, i)), 'color', [0,0,0,0.2]);
|
|
end
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(i,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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the IFF Plant (transfer function from u to taum)
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/dum'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors
|
|
|
|
Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the identified IFF Plant (Simscape) and measured FRF data
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(G_iff(:,1, 1)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - FRF')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f, abs(G_iff(:,i, i)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - Model')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(f, 180/pi*angle(G_iff(:,i, i)), 'color', [0,0,0,0.2]);
|
|
end
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(i,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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
#+end_src
|
|
|
|
*** Flexible model + encoders fixed to the plates
|
|
#+begin_src matlab
|
|
%% Identify the IFF Plant (transfer function from u to taum)
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
d_aligns = [[-0.05, -0.3, 0];
|
|
[ 0, 0.5, 0];
|
|
[-0.1, -0.3, 0];
|
|
[ 0, 0.3, 0];
|
|
[-0.05, 0.05, 0];
|
|
[0, 0, 0]]*1e-3;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Initialize Nano-Hexapod
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', 'flexible', ...
|
|
'actuator_d_align', d_aligns);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
Gdvf_struts = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Initialize Nano-Hexapod
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', 'flexible', ...
|
|
'actuator_d_align', d_aligns);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
Gdvf_plates = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Plot the obtained direct transfer functions for all the bending stiffnesses
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf_struts(1, 1), freqs, 'Hz'))), ...
|
|
'DisplayName', 'Struts');
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf_plates(1, 1), freqs, 'Hz'))), ...
|
|
'DisplayName', 'Plates');
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf_struts(i, i), freqs, 'Hz'))), ...
|
|
'HandleVisibility', 'off');
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf_plates(i, i), freqs, 'Hz'))), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'southwest');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf_struts(i, i), freqs, 'Hz'))));
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf_plates(i, i), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/dvf_plant_comp_struts_plates.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:dvf_plant_comp_struts_plates
|
|
#+caption: Comparison of the dynamics from $V_a$ to $d_L$ when the encoders are fixed to the struts (blue) and to the plates (red). APA are modeled as a flexible element.
|
|
#+RESULTS:
|
|
[[file:figs/dvf_plant_comp_struts_plates.png]]
|
|
|
|
** Integral Force Feedback
|
|
<<sec:enc_struts_iff>>
|
|
*** 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
|
|
addpath('./matlab/mat/');
|
|
addpath('./matlab/src/');
|
|
addpath('./matlab/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
addpath('./mat/');
|
|
addpath('./src/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
load('identified_plants_enc_struts.mat', 'f', 'Ts', 'G_iff', 'G_dvf')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
%% Add all useful folders to the path
|
|
addpath('matlab/nass-simscape/matlab/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/png/')
|
|
addpath('matlab/nass-simscape/src/')
|
|
addpath('matlab/nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
%% Add all useful folders to the path
|
|
addpath('nass-simscape/matlab/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/png/')
|
|
addpath('nass-simscape/src/')
|
|
addpath('nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Open Simulink Model
|
|
mdl = 'nano_hexapod_simscape';
|
|
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
Rx = zeros(1, 7);
|
|
|
|
open(mdl)
|
|
#+end_src
|
|
|
|
*** Identification of the IFF Plant
|
|
#+begin_src matlab
|
|
%% Initialize Nano-Hexapod
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the IFF Plant (transfer function from u to taum)
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/dum'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors
|
|
|
|
Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
*** Root Locus and Decentralized Loop gain
|
|
#+begin_src matlab
|
|
%% IFF Controller
|
|
Kiff_g1 = -(1/(s + 2*pi*40))*... % Low pass filter (provides integral action above 40Hz)
|
|
(s/(s + 2*pi*30))*... % High pass filter to limit low frequency gain
|
|
(1/(1 + s/2/pi/500))*... % Low pass filter to be more robust to high frequency resonances
|
|
eye(6); % Diagonal 6x6 controller
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Root Locus for IFF
|
|
gains = logspace(1, 4, 100);
|
|
|
|
figure;
|
|
|
|
hold on;
|
|
% Pure Integrator
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(pole(Giff)), imag(pole(Giff)), 'x', 'DisplayName', '$g = 0$');
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', 'HandleVisibility', 'off');
|
|
|
|
for g = gains
|
|
clpoles = pole(feedback(Giff, g*Kiff_g1*eye(6)));
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off');
|
|
end
|
|
|
|
g = 4e2;
|
|
clpoles = pole(feedback(Giff, g*Kiff_g1*eye(6)));
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(real(clpoles), imag(clpoles), 'x', 'DisplayName', sprintf('$g=%.0f$', g));
|
|
hold off;
|
|
axis square;
|
|
xlim([-1250, 0]); ylim([0, 1250]);
|
|
xlabel('Real Part'); ylabel('Imaginary Part');
|
|
legend('location', 'northwest');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_iff_root_locus.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_iff_root_locus
|
|
#+caption: Root Locus for the IFF control strategy
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_iff_root_locus.png]]
|
|
|
|
Then the "optimal" IFF controller is:
|
|
#+begin_src matlab
|
|
%% IFF controller with Optimal gain
|
|
Kiff = g*Kiff_g1;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
save('matlab/mat/Kiff.mat', 'Kiff')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :eval no
|
|
save('mat/Kiff.mat', 'Kiff')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the "decentralized loop gain"
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(squeeze(freqresp(Kiff(1,1), f, 'Hz')).*G_iff(:, 1, 1)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$\tau_{m,i}/u_i \cdot K_{iff}$ - FRF')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f, abs(squeeze(freqresp(Kiff(1,1), f, 'Hz')).*G_iff(:, i, i)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*Giff(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\tau_{m,i}/u_i \cdot K_{iff}$ - Model')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*Giff(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'northeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Kiff(1,1), f, 'Hz')).*G_iff(:, i, i)), 'color', [0,0,0,0.2]);
|
|
end
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Kiff(1,1)*Giff(i,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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_iff_opt_loop_gain.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_iff_opt_loop_gain
|
|
#+caption: Bode plot of the "decentralized loop gain" $G_\text{iff}(i,i) \times K_\text{iff}(i,i)$
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_iff_opt_loop_gain.png]]
|
|
|
|
*** Multiple Gains - Simulation
|
|
#+begin_src matlab
|
|
%% Tested IFF gains
|
|
iff_gains = [4, 10, 20, 40, 100, 200, 400];
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Initialize the Simscape model in closed loop
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'iff');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain
|
|
Gd_iff = {zeros(1, length(iff_gains))};
|
|
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Strut Displacement (encoder)
|
|
|
|
for i = 1:length(iff_gains)
|
|
Kiff = iff_gains(i)*Kiff_g1*eye(6); % IFF Controller
|
|
Gd_iff(i) = {exp(-s*Ts)*linearize(mdl, io, 0.0, options)};
|
|
|
|
isstable(Gd_iff{i})
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the transfer function from u to dLm for tested values of the IFF gain
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:length(iff_gains)
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff{i}(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', sprintf('$g = %.0f$', iff_gains(i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:length(iff_gains)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff{i}(1,1), 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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_iff_gains_effect_dvf_plant.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_iff_gains_effect_dvf_plant
|
|
#+caption: Effect of the IFF gain $g$ on the transfer function from $\bm{\tau}$ to $d\bm{\mathcal{L}}_m$
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_iff_gains_effect_dvf_plant.png]]
|
|
|
|
*** Experimental Results - Gains
|
|
**** Introduction :ignore:
|
|
Let's look at the damping introduced by IFF as a function of the IFF gain and compare that with the results obtained using the Simscape model.
|
|
|
|
**** Load Data
|
|
#+begin_src matlab
|
|
%% Load Identification Data
|
|
meas_iff_gains = {};
|
|
|
|
for i = 1:length(iff_gains)
|
|
meas_iff_gains(i) = {load(sprintf('mat/iff_strut_1_noise_g_%i.mat', iff_gains(i)), 't', 'Vexc', 'Vs', 'de', 'u')};
|
|
end
|
|
#+end_src
|
|
|
|
**** Spectral Analysis - Setup
|
|
#+begin_src matlab
|
|
%% Setup useful variables
|
|
% Sampling Time [s]
|
|
Ts = (meas_iff_gains{1}.t(end) - (meas_iff_gains{1}.t(1)))/(length(meas_iff_gains{1}.t)-1);
|
|
|
|
% Sampling Frequency [Hz]
|
|
Fs = 1/Ts;
|
|
|
|
% Hannning Windows
|
|
win = hanning(ceil(1*Fs));
|
|
|
|
% And we get the frequency vector
|
|
[~, f] = tfestimate(meas_iff_gains{1}.Vexc, meas_iff_gains{1}.de, win, [], [], 1/Ts);
|
|
#+end_src
|
|
|
|
**** DVF Plant
|
|
#+begin_src matlab
|
|
%% DVF Plant (transfer function from u to dLm)
|
|
G_iff_gains = {};
|
|
|
|
for i = 1:length(iff_gains)
|
|
G_iff_gains{i} = tfestimate(meas_iff_gains{i}.Vexc, meas_iff_gains{i}.de(:,1), win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the transfer function from u to dLm for tested values of the IFF gain
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:length(iff_gains)
|
|
plot(f, abs(G_iff_gains{i}), '-', ...
|
|
'DisplayName', sprintf('$g_{iff} = %.0f$', iff_gains(i)));
|
|
end
|
|
set(gca,'ColorOrderIndex',1)
|
|
for i = 1:length(iff_gains)
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff{i}(1,1), freqs, 'Hz'))), '--', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:length(iff_gains)
|
|
plot(f, 180/pi*angle(G_iff_gains{i}), '-');
|
|
end
|
|
set(gca,'ColorOrderIndex',1)
|
|
for i = 1:length(iff_gains)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff{i}(1,1), 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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/comp_iff_gains_dvf_plant.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:comp_iff_gains_dvf_plant
|
|
#+caption: Transfer function from $u$ to $d\mathcal{L}_m$ for multiple values of the IFF gain
|
|
#+RESULTS:
|
|
[[file:figs/comp_iff_gains_dvf_plant.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
xlim([20, 200]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/comp_iff_gains_dvf_plant_zoom.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:comp_iff_gains_dvf_plant_zoom
|
|
#+caption: Transfer function from $u$ to $d\mathcal{L}_m$ for multiple values of the IFF gain (Zoom)
|
|
#+RESULTS:
|
|
[[file:figs/comp_iff_gains_dvf_plant_zoom.png]]
|
|
|
|
#+begin_important
|
|
The IFF control strategy is very effective for the damping of the suspension modes.
|
|
It however does not damp the modes at 200Hz, 300Hz and 400Hz (flexible modes of the APA).
|
|
This is very logical.
|
|
|
|
Also, the experimental results and the models obtained from the Simscape model are in agreement.
|
|
#+end_important
|
|
|
|
**** Experimental Results - Comparison of the un-damped and fully damped system
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the transfer function from u to dLm
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
% Un Damped measurement
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(f, abs(G_dvf(:, 1, 1)), ...
|
|
'DisplayName', 'Un-Damped')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(f, abs(G_dvf(:,i , i)), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
% IFF Damped measurement
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f, abs(G_iff_opt{1}(:,1)), ...
|
|
'DisplayName', 'Optimal gain')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f, abs(G_iff_opt{i}(:,i)), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/V_{exc}$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-3]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(f, 180/pi*angle(G_dvf(i,i, i)));
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f, 180/pi*angle(G_iff_opt{i}(:,i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/comp_undamped_opt_iff_gain_diagonal.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:comp_undamped_opt_iff_gain_diagonal
|
|
#+caption: Comparison of the diagonal elements of the tranfer function from $\bm{u}$ to $d\bm{\mathcal{L}}_m$ without active damping and with optimal IFF gain
|
|
#+RESULTS:
|
|
[[file:figs/comp_undamped_opt_iff_gain_diagonal.png]]
|
|
|
|
#+begin_question
|
|
A series of modes at around 205Hz are also damped.
|
|
|
|
Are these damped modes at 205Hz additional "suspension" modes or flexible modes of the struts?
|
|
#+end_question
|
|
|
|
*** Experimental Results - Damped Plant with Optimal gain
|
|
**** Introduction :ignore:
|
|
Let's now look at the $6 \times 6$ damped plant with the optimal gain $g = 400$.
|
|
|
|
**** Load Data
|
|
#+begin_src matlab
|
|
%% Load Identification Data
|
|
meas_iff_struts = {};
|
|
|
|
for i = 1:6
|
|
meas_iff_struts(i) = {load(sprintf('mat/iff_strut_%i_noise_g_400.mat', i), 't', 'Vexc', 'Vs', 'de', 'u')};
|
|
end
|
|
#+end_src
|
|
|
|
**** Spectral Analysis - Setup
|
|
#+begin_src matlab
|
|
%% Setup useful variables
|
|
% Sampling Time [s]
|
|
Ts = (meas_iff_struts{1}.t(end) - (meas_iff_struts{1}.t(1)))/(length(meas_iff_struts{1}.t)-1);
|
|
|
|
% Sampling Frequency [Hz]
|
|
Fs = 1/Ts;
|
|
|
|
% Hannning Windows
|
|
win = hanning(ceil(1*Fs));
|
|
|
|
% And we get the frequency vector
|
|
[~, f] = tfestimate(meas_iff_struts{1}.Vexc, meas_iff_struts{1}.de, win, [], [], 1/Ts);
|
|
#+end_src
|
|
|
|
**** DVF Plant
|
|
#+begin_src matlab
|
|
%% DVF Plant (transfer function from u to dLm)
|
|
G_iff_opt = {};
|
|
|
|
for i = 1:6
|
|
G_iff_opt{i} = tfestimate(meas_iff_struts{i}.Vexc, meas_iff_struts{i}.de, win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the transfer function from u to dLm
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
% Diagonal Elements FRF
|
|
plot(f, abs(G_iff_opt{1}(:,1)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - FRF')
|
|
for i = 2:6
|
|
plot(f, abs(G_iff_opt{i}(:,i)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
% Diagonal Elements Model
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff{end}(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - Model')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff{end}(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/V_{exc}$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-3]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(f, 180/pi*angle(G_iff_opt{i}(:,i)), 'color', [0,0,0,0.2]);
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff{end}(i,i), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/damped_iff_plant_comp_diagonal.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:damped_iff_plant_comp_diagonal
|
|
#+caption: Comparison of the diagonal elements of the transfer functions from $\bm{u}$ to $d\bm{\mathcal{L}}_m$ with active damping (IFF) applied with an optimal gain $g = 400$
|
|
#+RESULTS:
|
|
[[file:figs/damped_iff_plant_comp_diagonal.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the transfer function from u to dLm
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
% Off diagonal FRF
|
|
plot(f, abs(G_iff_opt{1}(:,2)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - FRF')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_iff_opt{i}(:,j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
|
|
% Off diagonal Model
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff{end}(1,2), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - Model')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff{end}(i,j), freqs, 'Hz'))), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/V_{exc}$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-3]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
% Off diagonal FRF
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, 180/pi*angle(G_iff_opt{i}(:,j)), 'color', [0, 0, 0, 0.2]);
|
|
end
|
|
end
|
|
|
|
% Off diagonal Model
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff{end}(i,j), freqs, 'Hz'))));
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/damped_iff_plant_comp_off_diagonal.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:damped_iff_plant_comp_off_diagonal
|
|
#+caption: Comparison of the off-diagonal elements of the transfer functions from $\bm{u}$ to $d\bm{\mathcal{L}}_m$ with active damping (IFF) applied with an optimal gain $g = 400$
|
|
#+RESULTS:
|
|
[[file:figs/damped_iff_plant_comp_off_diagonal.png]]
|
|
|
|
#+begin_important
|
|
With the IFF control strategy applied and the optimal gain used, the suspension modes are very well damped.
|
|
Remains the undamped flexible modes of the APA (200Hz, 300Hz, 400Hz), and the modes of the plates (700Hz).
|
|
|
|
The Simscape model and the experimental results are in very good agreement.
|
|
#+end_important
|
|
|
|
** Modal Analysis
|
|
<<sec:enc_struts_modal_analysis>>
|
|
*** Introduction :ignore:
|
|
Several 3-axis accelerometers are fixed on the top platform of the nano-hexapod as shown in Figure [[fig:compliance_vertical_comp_iff]].
|
|
|
|
#+name: fig:accelerometers_nano_hexapod
|
|
#+caption: Location of the accelerometers on top of the nano-hexapod
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/accelerometers_nano_hexapod.jpg]]
|
|
|
|
The top platform is then excited using an instrumented hammer as shown in Figure [[fig:hammer_excitation_compliance_meas]].
|
|
|
|
#+name: fig:hammer_excitation_compliance_meas
|
|
#+caption: Example of an excitation using an instrumented hammer
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/hammer_excitation_compliance_meas.jpg]]
|
|
|
|
*** 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
|
|
addpath('./matlab/mat/');
|
|
addpath('./matlab/src/');
|
|
addpath('./matlab/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
addpath('./mat/');
|
|
addpath('./src/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
%% Add all useful folders to the path
|
|
addpath('matlab/nass-simscape/matlab/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/png/')
|
|
addpath('matlab/nass-simscape/src/')
|
|
addpath('matlab/nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
%% Add all useful folders to the path
|
|
addpath('nass-simscape/matlab/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/png/')
|
|
addpath('nass-simscape/src/')
|
|
addpath('nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Open Simulink Model
|
|
mdl = 'nano_hexapod_simscape';
|
|
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
Rx = zeros(1, 7);
|
|
|
|
open(mdl)
|
|
#+end_src
|
|
|
|
*** Effectiveness of the IFF Strategy - Compliance
|
|
In this section, we wish to estimated the effectiveness of the IFF strategy concerning the compliance.
|
|
|
|
The top plate is excited vertically using the instrumented hammer two times:
|
|
1. no control loop is used
|
|
2. decentralized IFF is used
|
|
|
|
The data is loaded.
|
|
#+begin_src matlab
|
|
frf_ol = load('Measurement_Z_axis.mat'); % Open-Loop
|
|
frf_iff = load('Measurement_Z_axis_damped.mat'); % IFF
|
|
#+end_src
|
|
|
|
The mean vertical motion of the top platform is computed by averaging all 5 accelerometers.
|
|
#+begin_src matlab
|
|
%% Multiply by 10 (gain in m/s^2/V) and divide by 5 (number of accelerometers)
|
|
d_frf_ol = 10/5*(frf_ol.FFT1_H1_4_1_RMS_Y_Mod + frf_ol.FFT1_H1_7_1_RMS_Y_Mod + frf_ol.FFT1_H1_10_1_RMS_Y_Mod + frf_ol.FFT1_H1_13_1_RMS_Y_Mod + frf_ol.FFT1_H1_16_1_RMS_Y_Mod)./(2*pi*frf_ol.FFT1_H1_16_1_RMS_X_Val).^2;
|
|
d_frf_iff = 10/5*(frf_iff.FFT1_H1_4_1_RMS_Y_Mod + frf_iff.FFT1_H1_7_1_RMS_Y_Mod + frf_iff.FFT1_H1_10_1_RMS_Y_Mod + frf_iff.FFT1_H1_13_1_RMS_Y_Mod + frf_iff.FFT1_H1_16_1_RMS_Y_Mod)./(2*pi*frf_iff.FFT1_H1_16_1_RMS_X_Val).^2;
|
|
#+end_src
|
|
|
|
The vertical compliance (magnitude of the transfer function from a vertical force applied on the top plate to the vertical motion of the top plate) is shown in Figure [[fig:compliance_vertical_comp_iff]].
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
hold on;
|
|
plot(frf_ol.FFT1_H1_16_1_RMS_X_Val, d_frf_ol, 'DisplayName', 'OL');
|
|
plot(frf_iff.FFT1_H1_16_1_RMS_X_Val, d_frf_iff, 'DisplayName', 'IFF');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Vertical Compliance [$m/N$]');
|
|
xlim([20, 2e3]); ylim([2e-9, 2e-5]);
|
|
legend('location', 'northeast');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/compliance_vertical_comp_iff.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:compliance_vertical_comp_iff
|
|
#+caption: Measured vertical compliance with and without IFF
|
|
#+RESULTS:
|
|
[[file:figs/compliance_vertical_comp_iff.png]]
|
|
|
|
#+begin_important
|
|
From Figure [[fig:compliance_vertical_comp_iff]], it is clear that the IFF control strategy is very effective in damping the suspensions modes of the nano-hexapode.
|
|
It also has the effect of degrading (slightly) the vertical compliance at low frequency.
|
|
|
|
It also seems some damping can be added to the modes at around 205Hz which are flexible modes of the struts.
|
|
#+end_important
|
|
|
|
*** Comparison with the Simscape Model
|
|
Let's now compare the measured vertical compliance with the vertical compliance as estimated from the Simscape model.
|
|
|
|
The transfer function from a vertical external force to the absolute motion of the top platform is identified (with and without IFF) using the Simscape model.
|
|
#+begin_src matlab :exports none
|
|
%% Identify the IFF Plant (transfer function from u to taum)
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/duz_ext'], 1, 'openinput'); io_i = io_i + 1; % External - Vertical force
|
|
io(io_i) = linio([mdl, '/Z_top_plat'], 1, 'openoutput'); io_i = io_i + 1; % Absolute vertical motion of top platform
|
|
|
|
%% Initialize Nano-Hexapod in Open Loop
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
|
|
G_compl_z_ol = linearize(mdl, io, 0.0, options);
|
|
|
|
%% Initialize Nano-Hexapod with IFF
|
|
Kiff = 400*(1/(s + 2*pi*40))*... % Low pass filter (provides integral action above 40Hz)
|
|
(s/(s + 2*pi*30))*... % High pass filter to limit low frequency gain
|
|
(1/(1 + s/2/pi/500))*... % Low pass filter to be more robust to high frequency resonances
|
|
eye(6); % Diagonal 6x6 controller
|
|
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'iff');
|
|
|
|
G_compl_z_iff = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
The comparison is done in Figure [[fig:compliance_vertical_comp_model_iff]].
|
|
Again, the model is quite accurate!
|
|
#+begin_src matlab :exports none
|
|
%% Comparison of the measured compliance and the one obtained from the model
|
|
freqs = 2*logspace(1,3,1000);
|
|
|
|
figure;
|
|
hold on;
|
|
plot(frf_ol.FFT1_H1_16_1_RMS_X_Val, d_frf_ol, '-', 'DisplayName', 'OL - Meas.');
|
|
plot(frf_iff.FFT1_H1_16_1_RMS_X_Val, d_frf_iff, '-', 'DisplayName', 'IFF - Meas.');
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(freqs, abs(squeeze(freqresp(G_compl_z_ol, freqs, 'Hz'))), '--', 'DisplayName', 'OL - Model')
|
|
plot(freqs, abs(squeeze(freqresp(G_compl_z_iff, freqs, 'Hz'))), '--', 'DisplayName', 'IFF - Model')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Vertical Compliance [$m/N$]');
|
|
xlim([20, 2e3]); ylim([2e-9, 2e-5]);
|
|
legend('location', 'northeast', 'FontSize', 8);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/compliance_vertical_comp_model_iff.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:compliance_vertical_comp_model_iff
|
|
#+caption: Measured vertical compliance with and without IFF
|
|
#+RESULTS:
|
|
[[file:figs/compliance_vertical_comp_model_iff.png]]
|
|
|
|
*** Obtained Mode Shapes
|
|
Then, several excitation are performed using the instrumented Hammer and the mode shapes are extracted.
|
|
|
|
We can observe the mode shapes of the first 6 modes that are the suspension modes (the plate is behaving as a solid body) in Figure [[fig:mode_shapes_annotated]].
|
|
|
|
#+name: fig:mode_shapes_annotated
|
|
#+caption: Measured mode shapes for the first six modes
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/mode_shapes_annotated.gif]]
|
|
|
|
Then, there is a mode at 692Hz which corresponds to a flexible mode of the top plate (Figure [[fig:mode_shapes_flexible_annotated]]).
|
|
|
|
#+name: fig:mode_shapes_flexible_annotated
|
|
#+caption: First flexible mode at 692Hz
|
|
#+attr_latex: :width 0.3\linewidth
|
|
[[file:figs/ModeShapeFlex1_crop.gif]]
|
|
|
|
The obtained modes are summarized in Table [[tab:description_modes]].
|
|
|
|
#+name: tab:description_modes
|
|
#+caption: Description of the identified modes
|
|
#+attr_latex: :environment tabularx :width 0.7\linewidth :align ccX
|
|
#+attr_latex: :center t :booktabs t :float t
|
|
| Mode | Freq. [Hz] | Description |
|
|
|------+------------+----------------------------------------------|
|
|
| 1 | 105 | Suspension Mode: Y-translation |
|
|
| 2 | 107 | Suspension Mode: X-translation |
|
|
| 3 | 131 | Suspension Mode: Z-translation |
|
|
| 4 | 161 | Suspension Mode: Y-tilt |
|
|
| 5 | 162 | Suspension Mode: X-tilt |
|
|
| 6 | 180 | Suspension Mode: Z-rotation |
|
|
| 7 | 692 | (flexible) Membrane mode of the top platform |
|
|
|
|
** Accelerometers fixed on the top platform
|
|
*** Introduction :ignore:
|
|
|
|
#+name: fig:acc_top_plat_top_view
|
|
#+caption: Accelerometers fixed on the top platform
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/acc_top_plat_top_view.jpg]]
|
|
|
|
*** 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
|
|
addpath('./matlab/mat/');
|
|
addpath('./matlab/src/');
|
|
addpath('./matlab/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
addpath('./mat/');
|
|
addpath('./src/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
%% Add all useful folders to the path
|
|
addpath('matlab/nass-simscape/matlab/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/png/')
|
|
addpath('matlab/nass-simscape/src/')
|
|
addpath('matlab/nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
%% Add all useful folders to the path
|
|
addpath('nass-simscape/matlab/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/png/')
|
|
addpath('nass-simscape/src/')
|
|
addpath('nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Open Simulink Model
|
|
mdl = 'nano_hexapod_simscape';
|
|
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
Rx = zeros(1, 7);
|
|
|
|
open(mdl)
|
|
#+end_src
|
|
|
|
*** Experimental Identification
|
|
#+begin_src matlab
|
|
%% Load Identification Data
|
|
meas_acc = {};
|
|
|
|
for i = 1:6
|
|
meas_acc(i) = {load(sprintf('mat/meas_acc_top_plat_strut_%i.mat', i), 't', 'Va', 'de', 'Am')};
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Setup useful variables
|
|
% Sampling Time [s]
|
|
Ts = (meas_acc{1}.t(end) - (meas_acc{1}.t(1)))/(length(meas_acc{1}.t)-1);
|
|
|
|
% Sampling Frequency [Hz]
|
|
Fs = 1/Ts;
|
|
|
|
% Hannning Windows
|
|
win = hanning(ceil(1*Fs));
|
|
|
|
% And we get the frequency vector
|
|
[~, f] = tfestimate(meas_acc{1}.Va, meas_acc{1}.de, win, [], [], 1/Ts);
|
|
#+end_src
|
|
|
|
The sensibility of the accelerometers are $0.1 V/g \approx 0.01 V/(m/s^2)$.
|
|
#+begin_src matlab
|
|
%% Compute the 6x6 transfer function matrix
|
|
G_acc = zeros(length(f), 6, 6);
|
|
|
|
for i = 1:6
|
|
G_acc(:,:,i) = tfestimate(meas_acc{i}.Va, 1/0.01*meas_acc{i}.Am, win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
*** Location and orientation of accelerometers
|
|
#+begin_src matlab
|
|
Opm = [ 0.047, -0.112, 10e-3;
|
|
0.047, -0.112, 10e-3;
|
|
-0.113, 0.011, 10e-3;
|
|
-0.113, 0.011, 10e-3;
|
|
0.040, 0.113, 10e-3;
|
|
0.040, 0.113, 10e-3]';
|
|
|
|
Osm = [-1, 0, 0;
|
|
0, 0, 1;
|
|
0, -1, 0;
|
|
0, 0, 1;
|
|
-1, 0, 0;
|
|
0, 0, 1]';
|
|
|
|
#+end_src
|
|
|
|
*** COM
|
|
#+begin_src matlab
|
|
Hbm = -15e-3;
|
|
|
|
M = getTransformationMatrixAcc(Opm-[0;0;Hbm], Osm);
|
|
J = getJacobianNanoHexapod(Hbm);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
G_acc_CoM = zeros(size(G_acc));
|
|
|
|
for i = 1:length(f)
|
|
G_acc_CoM(i, :, :) = inv(M)*squeeze(G_acc(i, :, :))*inv(J');
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
labels = {'$D_x/F_{x}$', '$D_y/F_{y}$', '$D_z/F_{z}$', '$R_{x}/M_{x}$', '$R_{y}/M_{y}$', '$R_{R}/M_{z}$'};
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(f, abs(G_acc_CoM(:, i, j)./(-(2*pi*f).^2)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:3
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, abs(G_acc_CoM(:,i , i)./(-(2*pi*f).^2)), ...
|
|
'DisplayName', labels{i});
|
|
end
|
|
plot(f, abs(G_acc_CoM(:, 1, 2)./(-(2*pi*f).^2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$D_i/F_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $A_m/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-5]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:3
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, 180/pi*angle(G_acc_CoM(:,i , i)./(-(2*pi*f).^2)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
labels = {'$D_x/F_{x}$', '$D_y/F_{y}$', '$D_z/F_{z}$', '$R_{x}/M_{x}$', '$R_{y}/M_{y}$', '$R_{R}/M_{z}$'};
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, abs(G_acc_CoM(:,i , i)./(-(2*pi*f).^2)), ...
|
|
'DisplayName', labels{i});
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $A_m/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-3]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, 180/pi*angle(G_acc_CoM(:,i , i)./(-(2*pi*f).^2)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([50, 5e2]);
|
|
#+end_src
|
|
|
|
*** COK
|
|
#+begin_src matlab
|
|
Hbm = -42.3e-3;
|
|
|
|
M = getTransformationMatrixAcc(Opm-[0;0;Hbm], Osm);
|
|
J = getJacobianNanoHexapod(Hbm);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
G_acc_CoK = zeros(size(G_acc));
|
|
|
|
for i = 1:length(f)
|
|
G_acc_CoK(i, :, :) = inv(M)*squeeze(G_acc(i, :, :))*inv(J');
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
labels = {'$D_x/F_{x}$', '$D_y/F_{y}$', '$D_z/F_{z}$', '$R_{x}/M_{x}$', '$R_{y}/M_{y}$', '$R_{R}/M_{z}$'};
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(f, abs(G_acc_CoK(:, i, j)./(-(2*pi*f).^2)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:3
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, abs(G_acc_CoK(:,i , i)./(-(2*pi*f).^2)), ...
|
|
'DisplayName', labels{i});
|
|
end
|
|
plot(f, abs(G_acc_CoK(:, 1, 2)./(-(2*pi*f).^2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$D_i/F_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $A_m/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-5]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:3
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, 180/pi*angle(G_acc_CoK(:,i , i)./(-(2*pi*f).^2)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', ...
|
|
'$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'};
|
|
|
|
figure;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, abs(G_acc_CoK(:,i,i)./(-(2*pi*f).^2)), ...
|
|
'DisplayName', labels{i});
|
|
end
|
|
plot(f, abs(G_acc_CoK(:,1,2)./(-(2*pi*f).^2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', 'Off-Diagonal');
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_acc_CoK(:,i,j)./(-(2*pi*f).^2)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude $X_m/V_a$ [m/V]');
|
|
xlim([50, 5e2]); ylim([1e-7, 1e-1]);
|
|
legend('location', 'southwest');
|
|
#+end_src
|
|
|
|
*** Comp with the Simscape Model
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', 'flexible', ...
|
|
'MO_B', -42.3e-3);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs
|
|
|
|
G = linearize(mdl, io, 0.0, options);
|
|
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
G.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
|
|
#+end_src
|
|
|
|
Then use the Jacobian matrices to obtain the "cartesian" centralized plant.
|
|
#+begin_src matlab
|
|
Gc = inv(n_hexapod.geometry.J)*...
|
|
G*...
|
|
inv(n_hexapod.geometry.J');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', ...
|
|
'$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'};
|
|
|
|
figure;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gc(i,i), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', labels{i});
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(Gc(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', 'Off-Diagonal');
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N,rad/N/m]');
|
|
xlim([50, 5e2]); ylim([1e-7, 1e-1]);
|
|
legend('location', 'southwest');
|
|
#+end_src
|
|
|
|
* Encoders fixed to the plates
|
|
<<sec:encoders_plates>>
|
|
|
|
** Introduction :ignore:
|
|
In this section, the encoders are fixed to the plates rather than to the struts as shown in Figure [[fig:enc_fixed_to_struts]].
|
|
|
|
#+name: fig:enc_fixed_to_struts
|
|
#+caption: Nano-Hexapod with encoders fixed to the struts
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/IMG_20210625_083801.jpg]]
|
|
|
|
It is structured as follow:
|
|
- Section [[sec:enc_plates_plant_id]]: The dynamics of the nano-hexapod is identified
|
|
- Section [[sec:enc_plates_comp_simscape]]: The identified dynamics is compared with the Simscape model
|
|
- Section [[sec:enc_plates_iff]]: The Integral Force Feedback (IFF) control strategy is applied and the dynamics of the damped nano-hexapod is identified and compare with the Simscape model
|
|
- Section [[sec:hac_iff_struts]]: The High Authority Control (HAC) in the frame of the struts is developed
|
|
- Section [[sec:hac_iff_struts_ref_track]]: Some reference tracking tests are performed in order to experimentally validate the HAC-LAC control strategy.
|
|
|
|
** Identification of the dynamics
|
|
<<sec:enc_plates_plant_id>>
|
|
*** 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
|
|
addpath('./matlab/mat/');
|
|
addpath('./matlab/src/');
|
|
addpath('./matlab/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
addpath('./mat/');
|
|
addpath('./src/');
|
|
#+end_src
|
|
|
|
*** Load Measurement Data
|
|
#+begin_src matlab
|
|
%% Load Identification Data
|
|
meas_data_lf = {};
|
|
|
|
for i = 1:6
|
|
meas_data_lf(i) = {load(sprintf('mat/frf_exc_strut_%i_enc_plates_noise.mat', i), 't', 'Va', 'Vs', 'de')};
|
|
end
|
|
#+end_src
|
|
|
|
*** Spectral Analysis - Setup
|
|
#+begin_src matlab
|
|
%% Setup useful variables
|
|
% Sampling Time [s]
|
|
Ts = (meas_data_lf{1}.t(end) - (meas_data_lf{1}.t(1)))/(length(meas_data_lf{1}.t)-1);
|
|
|
|
% Sampling Frequency [Hz]
|
|
Fs = 1/Ts;
|
|
|
|
% Hannning Windows
|
|
win = hanning(ceil(1*Fs));
|
|
|
|
% And we get the frequency vector
|
|
[~, f] = tfestimate(meas_data_lf{1}.Va, meas_data_lf{1}.de, win, [], [], 1/Ts);
|
|
#+end_src
|
|
|
|
*** DVF Plant
|
|
First, let's compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure [[fig:enc_plates_dvf_coh]]).
|
|
|
|
#+begin_src matlab
|
|
%% Coherence
|
|
coh_dvf = zeros(length(f), 6, 6);
|
|
|
|
for i = 1:6
|
|
coh_dvf(:, :, i) = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Coherence for the transfer function from u to dLm
|
|
figure;
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, coh_dvf(:, i, j), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, coh_dvf(:, i, i), ...
|
|
'DisplayName', sprintf('$G_{dvf}(%i,%i)$', i, i));
|
|
end
|
|
plot(f, coh_dvf(:, 1, 2), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$G_{dvf}(i,j)$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
|
|
xlim([20, 2e3]); ylim([0, 1]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_plates_dvf_coh.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_plates_dvf_coh
|
|
#+caption: Obtained coherence for the DVF plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_plates_dvf_coh.png]]
|
|
|
|
Then the 6x6 transfer function matrix is estimated (Figure [[fig:enc_plates_dvf_frf]]).
|
|
#+begin_src matlab
|
|
%% DVF Plant (transfer function from u to dLm)
|
|
G_dvf = zeros(length(f), 6, 6);
|
|
|
|
for i = 1:6
|
|
G_dvf(:,:,i) = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the transfer function from u to dLm
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_dvf(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, abs(G_dvf(:,i, i)), ...
|
|
'DisplayName', sprintf('$G_{dvf}(%i,%i)$', i, i));
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, abs(G_dvf(:,i, i)), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(f, abs(G_dvf(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$G_{dvf}(i,j)$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-3]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, 180/pi*angle(G_dvf(:,i, i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_plates_dvf_frf.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_plates_dvf_frf
|
|
#+caption: Measured FRF for the DVF plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_plates_dvf_frf.png]]
|
|
|
|
*** IFF Plant
|
|
First, let's compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure [[fig:enc_plates_iff_coh]]).
|
|
|
|
#+begin_src matlab
|
|
%% Coherence for the IFF plant
|
|
coh_iff = zeros(length(f), 6, 6);
|
|
|
|
for i = 1:6
|
|
coh_iff(:,:,i) = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Coherence of the IFF Plant (transfer function from u to taum)
|
|
figure;
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, coh_iff(:, i, j), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, coh_iff(:,i, i), ...
|
|
'DisplayName', sprintf('$G_{iff}(%i,%i)$', i, i));
|
|
end
|
|
plot(f, coh_iff(:, 1, 2), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$G_{iff}(i,j)$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
|
|
xlim([20, 2e3]); ylim([0, 1]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_plates_iff_coh.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_plates_iff_coh
|
|
#+caption: Obtained coherence for the IFF plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_plates_iff_coh.png]]
|
|
|
|
Then the 6x6 transfer function matrix is estimated (Figure [[fig:enc_plates_iff_frf]]).
|
|
#+begin_src matlab
|
|
%% IFF Plant
|
|
G_iff = zeros(length(f), 6, 6);
|
|
|
|
for i = 1:6
|
|
G_iff(:,:,i) = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the IFF Plant (transfer function from u to taum)
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_iff(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, abs(G_iff(:,i , i)), ...
|
|
'DisplayName', sprintf('$G_{iff}(%i,%i)$', i, i));
|
|
end
|
|
plot(f, abs(G_iff(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$G_{iff}(i,j)$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
ylim([1e-3, 1e2]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f, 180/pi*angle(G_iff(:,i, i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_plates_iff_frf.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_plates_iff_frf
|
|
#+caption: Measured FRF for the IFF plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_plates_iff_frf.png]]
|
|
|
|
*** Save Identified Plants
|
|
#+begin_src matlab :tangle no
|
|
save('matlab/mat/identified_plants_enc_plates.mat', 'f', 'Ts', 'G_iff', 'G_dvf')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :eval no
|
|
save('mat/identified_plants_enc_plates.mat', 'f', 'Ts', 'G_iff', 'G_dvf')
|
|
#+end_src
|
|
|
|
** Comparison with the Simscape Model
|
|
<<sec:enc_plates_comp_simscape>>
|
|
*** Introduction :ignore:
|
|
In this section, the measured dynamics is compared with the dynamics estimated from the Simscape model.
|
|
|
|
*** 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
|
|
addpath('./matlab/mat/');
|
|
addpath('./matlab/src/');
|
|
addpath('./matlab/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
addpath('./mat/');
|
|
addpath('./src/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
%% Add all useful folders to the path
|
|
addpath('matlab/nass-simscape/matlab/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/png/')
|
|
addpath('matlab/nass-simscape/src/')
|
|
addpath('matlab/nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
%% Add all useful folders to the path
|
|
addpath('nass-simscape/matlab/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/png/')
|
|
addpath('nass-simscape/src/')
|
|
addpath('nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Open Simulink Model
|
|
mdl = 'nano_hexapod_simscape';
|
|
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
Rx = zeros(1, 7);
|
|
|
|
open(mdl)
|
|
#+end_src
|
|
|
|
*** Load measured FRF
|
|
#+begin_src matlab
|
|
%% Load data
|
|
load('identified_plants_enc_plates.mat', 'f', 'Ts', 'G_iff', 'G_dvf')
|
|
#+end_src
|
|
|
|
*** Dynamics from Actuator to Force Sensors
|
|
#+begin_src matlab
|
|
%% Initialize Nano-Hexapod
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', 'flexible');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the IFF Plant (transfer function from u to taum)
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/dum'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors
|
|
|
|
Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Comparison of the plants (encoder output) when tuning the misalignment
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
i_input = 1;
|
|
|
|
figure;
|
|
tiledlayout(2, 3, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_iff(:, 1, i_input)));
|
|
plot(freqs, abs(squeeze(freqresp(Giff(1, i_input), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');
|
|
title(sprintf('$d\\tau_{m1}/u_{%i}$', i_input));
|
|
|
|
ax2 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_iff(:, 2, i_input)));
|
|
plot(freqs, abs(squeeze(freqresp(Giff(2, i_input), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
title(sprintf('$d\\tau_{m2}/u_{%i}$', i_input));
|
|
|
|
ax3 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_iff(:, 3, i_input)), ...
|
|
'DisplayName', 'Meas.');
|
|
plot(freqs, abs(squeeze(freqresp(Giff(3, i_input), freqs, 'Hz'))), ...
|
|
'DisplayName', 'Model');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8);
|
|
title(sprintf('$d\\tau_{m3}/u_{%i}$', i_input));
|
|
|
|
ax4 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_iff(:, 4, i_input)));
|
|
plot(freqs, abs(squeeze(freqresp(Giff(4, i_input), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
|
|
title(sprintf('$d\\tau_{m4}/u_{%i}$', i_input));
|
|
|
|
ax5 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_iff(:, 5, i_input)));
|
|
plot(freqs, abs(squeeze(freqresp(Giff(5, i_input), freqs, 'Hz'))));
|
|
hold off;
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
title(sprintf('$d\\tau_{m5}/u_{%i}$', i_input));
|
|
|
|
ax6 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_iff(:, 6, i_input)));
|
|
plot(freqs, abs(squeeze(freqresp(Giff(6, i_input), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
|
title(sprintf('$d\\tau_{m6}/u_{%i}$', i_input));
|
|
|
|
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
|
|
xlim([20, 2e3]); ylim([1e-2, 1e2]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_plates_iff_comp_simscape_all.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_plates_iff_comp_simscape_all
|
|
#+caption: IFF Plant for the first actuator input and all the force senosrs
|
|
#+RESULTS:
|
|
[[file:figs/enc_plates_iff_comp_simscape_all.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the identified IFF Plant (Simscape) and measured FRF data
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(G_iff(:,1, 1)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - FRF')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f, abs(G_iff(:,i, i)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - Model')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(f, 180/pi*angle(G_iff(:,i, i)), 'color', [0,0,0,0.2]);
|
|
end
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(i,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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_plates_iff_comp_simscape.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_plates_iff_comp_simscape
|
|
#+caption: Diagonal elements of the IFF Plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_plates_iff_comp_simscape.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the identified IFF Plant (Simscape) and measured FRF data (off-diagonal elements)
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
% Off diagonal terms
|
|
plot(f, abs(G_iff(:, 1, 2)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$\tau_{m,i}/u_j$ - FRF')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_iff(:, i, j)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(1, 2), freqs, 'Hz'))), ...
|
|
'DisplayName', '$\tau_{m,i}/u_j$ - Model')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(i, j), freqs, 'Hz'))), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [V/V]');
|
|
xlim([freqs(1), freqs(end)]); ylim([1e-3, 1e2]);
|
|
legend('location', 'northeast');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_plates_iff_comp_offdiag_simscape.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_plates_iff_comp_offdiag_simscape
|
|
#+caption: Off diagonal elements of the IFF Plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_plates_iff_comp_offdiag_simscape.png]]
|
|
|
|
*** Dynamics from Actuator to Encoder
|
|
#+begin_src matlab
|
|
%% Initialization of the Nano-Hexapod
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', 'flexible');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the DVF Plant (transfer function from u to dLm)
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
|
|
|
|
Gdvf = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Comparison of the plants (encoder output) when tuning the misalignment
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
i_input = 3;
|
|
|
|
figure;
|
|
tiledlayout(2, 3, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_dvf(:, 1, i_input)));
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1, i_input), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');
|
|
title(sprintf('$d\\mathcal{L}_{m1}/u_{%i}$', i_input));
|
|
|
|
ax2 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_dvf(:, 2, i_input)));
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(2, i_input), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
title(sprintf('$d\\mathcal{L}_{m2}/u_{%i}$', i_input));
|
|
|
|
ax3 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_dvf(:, 3, i_input)), ...
|
|
'DisplayName', 'Meas.');
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(3, i_input), freqs, 'Hz'))), ...
|
|
'DisplayName', 'Model');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8);
|
|
title(sprintf('$d\\mathcal{L}_{m3}/u_{%i}$', i_input));
|
|
|
|
ax4 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_dvf(:, 4, i_input)));
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(4, i_input), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
|
|
title(sprintf('$d\\mathcal{L}_{m4}/u_{%i}$', i_input));
|
|
|
|
ax5 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_dvf(:, 5, i_input)));
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(5, i_input), freqs, 'Hz'))));
|
|
hold off;
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
title(sprintf('$d\\mathcal{L}_{m5}/u_{%i}$', i_input));
|
|
|
|
ax6 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_dvf(:, 6, i_input)));
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(6, i_input), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
|
title(sprintf('$d\\mathcal{L}_{m6}/u_{%i}$', i_input));
|
|
|
|
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
|
|
xlim([40, 4e2]); ylim([1e-8, 1e-2]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_plates_dvf_comp_simscape_all.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_plates_dvf_comp_simscape_all
|
|
#+caption: DVF Plant for the first actuator input and all the encoders
|
|
#+RESULTS:
|
|
[[file:figs/enc_plates_dvf_comp_simscape_all.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Diagonal elements of the DVF plant
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(G_dvf(:,1, 1)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - FRF')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f, abs(G_dvf(:,i, i)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - Model')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'northeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(f, 180/pi*angle(G_dvf(:,i, i)), 'color', [0,0,0,0.2]);
|
|
end
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(i,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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_plates_dvf_comp_simscape.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_plates_dvf_comp_simscape
|
|
#+caption: Diagonal elements of the DVF Plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_plates_dvf_comp_simscape.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Off-diagonal elements of the DVF plant
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
% Off diagonal terms
|
|
plot(f, abs(G_dvf(:, 1, 2)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - FRF')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_dvf(:, i, j)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1, 2), freqs, 'Hz'))), ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - Model')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i, j), freqs, 'Hz'))), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
|
|
xlim([freqs(1), freqs(end)]); ylim([1e-8, 1e-3]);
|
|
legend('location', 'northeast');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_plates_dvf_comp_offdiag_simscape.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_plates_dvf_comp_offdiag_simscape
|
|
#+caption: Off diagonal elements of the DVF Plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_plates_dvf_comp_offdiag_simscape.png]]
|
|
|
|
** Integral Force Feedback
|
|
<<sec:enc_plates_iff>>
|
|
*** Introduction :ignore:
|
|
|
|
|
|
#+begin_src latex :file control_architecture_iff.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block={3.0cm}{3.0cm}] (P) {Plant};
|
|
\coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
|
|
\coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
|
|
\coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
|
|
\coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);
|
|
|
|
\node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
|
|
\node[addb={+}{}{-}{}{}, left= of inputF] (addF) {};
|
|
|
|
% Connections and labels
|
|
\draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
|
|
\draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
|
|
\draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$};
|
|
|
|
\draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
|
|
\draw[->] (Kiff.west) -| (addF.north);
|
|
\draw[->] (addF.east) -- (inputF) node[above left]{$\bm{u}$};
|
|
\draw[<-] (addF.west) -- ++(-1, 0) node[above right]{$\bm{u}^\prime$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:control_architecture_iff
|
|
#+caption: Integral Force Feedback Strategy
|
|
#+RESULTS:
|
|
[[file:figs/control_architecture_iff.png]]
|
|
|
|
*** 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
|
|
addpath('./matlab/mat/');
|
|
addpath('./matlab/src/');
|
|
addpath('./matlab/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
addpath('./mat/');
|
|
addpath('./src/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
load('identified_plants_enc_plates.mat', 'f', 'Ts', 'G_iff', 'G_dvf')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
%% Add all useful folders to the path
|
|
addpath('matlab/nass-simscape/matlab/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/png/')
|
|
addpath('matlab/nass-simscape/src/')
|
|
addpath('matlab/nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
%% Add all useful folders to the path
|
|
addpath('nass-simscape/matlab/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/png/')
|
|
addpath('nass-simscape/src/')
|
|
addpath('nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Open Simulink Model
|
|
mdl = 'nano_hexapod_simscape';
|
|
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
open(mdl)
|
|
|
|
Rx = zeros(1, 7);
|
|
|
|
colors = colororder;
|
|
#+end_src
|
|
|
|
*** Identification of the IFF Plant
|
|
#+begin_src matlab
|
|
%% Initialize Nano-Hexapod
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', '2dof');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the IFF Plant (transfer function from u to taum)
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors
|
|
|
|
Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
*** Effect of IFF on the plant - Simscape Model
|
|
#+begin_src matlab
|
|
load('Kiff.mat', 'Kiff')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Plate Displacement (encoder)
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Initialize the Simscape model in closed loop
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', 'flexible');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
Gd_ol = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Initialize the Simscape model in closed loop
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', 'flexible', ...
|
|
'controller_type', 'iff');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
Gd_iff = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
isstable(Gd_iff)
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: 1
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the transfer function from u to dLm for tested values of the IFF gain
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gd_ol(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', 'OL - Diag');
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', 'IFF - Diag');
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gd_ol(1,1), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gd_ol(1,2), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
|
|
'DisplayName', 'OL - Off-diag')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gd_ol(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff(1,2), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
|
'DisplayName', 'IFF - Off-diag')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_ol(1,1), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_plates_iff_gains_effect_dvf_plant.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_plates_iff_gains_effect_dvf_plant
|
|
#+caption: Effect of the IFF control strategy on the transfer function from $\bm{\tau}$ to $d\bm{\mathcal{L}}_m$
|
|
#+RESULTS:
|
|
[[file:figs/enc_plates_iff_gains_effect_dvf_plant.png]]
|
|
|
|
*** Experimental Results - Damped Plant with Optimal gain
|
|
**** Introduction :ignore:
|
|
Let's now look at the $6 \times 6$ damped plant with the optimal gain $g = 400$.
|
|
|
|
**** Load Data
|
|
#+begin_src matlab
|
|
%% Load Identification Data
|
|
meas_iff_plates = {};
|
|
|
|
for i = 1:6
|
|
meas_iff_plates(i) = {load(sprintf('mat/frf_exc_iff_strut_%i_enc_plates_noise.mat', i), 't', 'Va', 'Vs', 'de', 'u')};
|
|
end
|
|
#+end_src
|
|
|
|
**** Spectral Analysis - Setup
|
|
#+begin_src matlab
|
|
%% Setup useful variables
|
|
% Sampling Time [s]
|
|
Ts = (meas_iff_plates{1}.t(end) - (meas_iff_plates{1}.t(1)))/(length(meas_iff_plates{1}.t)-1);
|
|
|
|
% Sampling Frequency [Hz]
|
|
Fs = 1/Ts;
|
|
|
|
% Hannning Windows
|
|
win = hanning(ceil(1*Fs));
|
|
|
|
% And we get the frequency vector
|
|
[~, f] = tfestimate(meas_iff_plates{1}.Va, meas_iff_plates{1}.de, win, [], [], 1/Ts);
|
|
#+end_src
|
|
|
|
**** Simscape Model
|
|
#+begin_src matlab
|
|
load('Kiff.mat', 'Kiff')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Initialize the Simscape model in closed loop
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', 'flexible', ...
|
|
'controller_type', 'iff');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Plate Displacement (encoder)
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
Gd_iff_opt = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
**** DVF Plant
|
|
#+begin_src matlab
|
|
%% IFF Plant
|
|
G_enc_iff_opt = zeros(length(f), 6, 6);
|
|
|
|
for i = 1:6
|
|
G_enc_iff_opt(:,:,i) = tfestimate(meas_iff_plates{i}.Va, meas_iff_plates{i}.de, win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Comparison of the plants (encoder output) when tuning the misalignment
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
i_input = 1;
|
|
|
|
figure;
|
|
tiledlayout(2, 3, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_enc_iff_opt(:, 1, i_input)));
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(1, i_input), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');
|
|
title(sprintf('$d\\tau_{m1}/u_{%i}$', i_input));
|
|
|
|
ax2 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_enc_iff_opt(:, 2, i_input)));
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(2, i_input), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
title(sprintf('$d\\tau_{m2}/u_{%i}$', i_input));
|
|
|
|
ax3 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_enc_iff_opt(:, 3, i_input)), ...
|
|
'DisplayName', 'Meas.');
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(3, i_input), freqs, 'Hz'))), ...
|
|
'DisplayName', 'Model');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8);
|
|
title(sprintf('$d\\tau_{m3}/u_{%i}$', i_input));
|
|
|
|
ax4 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_enc_iff_opt(:, 4, i_input)));
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(4, i_input), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
|
|
title(sprintf('$d\\tau_{m4}/u_{%i}$', i_input));
|
|
|
|
ax5 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_enc_iff_opt(:, 5, i_input)));
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(5, i_input), freqs, 'Hz'))));
|
|
hold off;
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
title(sprintf('$d\\tau_{m5}/u_{%i}$', i_input));
|
|
|
|
ax6 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_enc_iff_opt(:, 6, i_input)));
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(6, i_input), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
|
title(sprintf('$d\\tau_{m6}/u_{%i}$', i_input));
|
|
|
|
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
|
|
xlim([20, 2e3]); ylim([1e-8, 1e-4]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_plates_opt_iff_comp_simscape_all.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_plates_opt_iff_comp_simscape_all
|
|
#+caption: FRF from one actuator to all the encoders when the plant is damped using IFF
|
|
#+RESULTS:
|
|
[[file:figs/enc_plates_opt_iff_comp_simscape_all.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the transfer function from u to dLm
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
% Diagonal Elements FRF
|
|
plot(f, abs(G_enc_iff_opt(:,1,1)), 'color', [colors(1,:), 0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - FRF')
|
|
for i = 2:6
|
|
plot(f, abs(G_enc_iff_opt(:,i,i)), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
% Diagonal Elements Model
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - Model')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/V_{exc}$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-8, 1e-4]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(f, 180/pi*angle(G_enc_iff_opt(:,i,i)), 'color', [colors(1,:), 0.2]);
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff_opt(i,i), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/damped_iff_plates_plant_comp_diagonal.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:damped_iff_plates_plant_comp_diagonal
|
|
#+caption: Comparison of the diagonal elements of the transfer functions from $\bm{u}$ to $d\bm{\mathcal{L}}_m$ with active damping (IFF) applied with an optimal gain $g = 400$
|
|
#+RESULTS:
|
|
[[file:figs/damped_iff_plates_plant_comp_diagonal.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the transfer function from u to dLm
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
% Off diagonal FRF
|
|
plot(f, abs(G_enc_iff_opt(:,1,2)), 'color', [colors(1,:), 0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - FRF')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_enc_iff_opt(:,i,j)), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
|
|
% Off diagonal Model
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(1,2), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - Model')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(i,j), freqs, 'Hz'))), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/V_{exc}$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-8, 1e-4]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
% Off diagonal FRF
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, 180/pi*angle(G_enc_iff_opt(:,i,j)), 'color', [colors(1,:), 0.2]);
|
|
end
|
|
end
|
|
|
|
% Off diagonal Model
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff_opt(i,j), freqs, 'Hz'))));
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/damped_iff_plates_plant_comp_off_diagonal.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:damped_iff_plates_plant_comp_off_diagonal
|
|
#+caption: Comparison of the off-diagonal elements of the transfer functions from $\bm{u}$ to $d\bm{\mathcal{L}}_m$ with active damping (IFF) applied with an optimal gain $g = 400$
|
|
#+RESULTS:
|
|
[[file:figs/damped_iff_plates_plant_comp_off_diagonal.png]]
|
|
|
|
*** Effect of IFF on the plant - FRF
|
|
#+begin_src matlab :tangle no
|
|
load('identified_plants_enc_plates.mat', 'f', 'G_dvf');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the transfer function from u to dLm for tested values of the IFF gain
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(G_dvf(:,1,1)), '-', ...
|
|
'DisplayName', 'OL - Diag');
|
|
plot(f, abs(G_enc_iff_opt(:,1,1)), '-', ...
|
|
'DisplayName', 'IFF - Diag');
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(f, abs(G_dvf(:,1,1)), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(f, abs(G_enc_iff_opt(:,i,i)), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
plot(f, abs(G_dvf(:,1,2)), 'color', [colors(1,:), 0.2], ...
|
|
'DisplayName', 'OL - Off-diag')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_dvf(:,i,j)), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(f, abs(G_enc_iff_opt(:,1,2)), 'color', [colors(2,:), 0.2], ...
|
|
'DisplayName', 'IFF - Off-diag')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_enc_iff_opt(:,i,j)), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(f, 180/pi*angle(G_dvf(:,1,1)), '-')
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(f, 180/pi*angle(G_enc_iff_opt(:,i,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]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_plant_plates_effect_iff.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_plant_plates_effect_iff
|
|
#+caption: Effect of the IFF control strategy on the transfer function from $\bm{\tau}$ to $d\bm{\mathcal{L}}_m$
|
|
#+RESULTS:
|
|
[[file:figs/enc_plant_plates_effect_iff.png]]
|
|
|
|
*** Save Damped Plant
|
|
#+begin_src matlab :tangle no
|
|
save('matlab/mat/damped_plant_enc_plates.mat', 'f', 'Ts', 'G_enc_iff_opt')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :eval no
|
|
save('mat/damped_plant_enc_plates.mat', 'f', 'Ts', 'G_enc_iff_opt')
|
|
#+end_src
|
|
|
|
** HAC Control - Frame of the struts
|
|
<<sec:hac_iff_struts>>
|
|
*** Introduction :ignore:
|
|
|
|
In a first approximation, the Jacobian matrix can be used instead of using the inverse kinematic equations.
|
|
|
|
#+begin_src latex :file control_architecture_hac_iff_L.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block={3.0cm}{3.0cm}] (P) {Plant};
|
|
\coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
|
|
\coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
|
|
\coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
|
|
\coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);
|
|
|
|
\node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
|
|
\node[addb, left= of inputF] (addF) {};
|
|
\node[block, left= of addF] (K) {$\bm{K}_\mathcal{L}$};
|
|
\node[addb, left= of K] (subr) {};
|
|
\node[block, align=center, left= of subr] (J) {Inverse\\Kinematics};
|
|
|
|
% Connections and labels
|
|
\draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
|
|
\draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
|
|
\draw[->] (Kiff.west) -| (addF.north);
|
|
\draw[->] (addF.east) -- (inputF) node[above left]{$\bm{u}$};
|
|
|
|
\draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
|
|
\draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, -1) -| (subr.south);
|
|
\draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{d\mathcal{L}}$};
|
|
\draw[->] (K.east) -- (addF.west) node[above left]{$\bm{u}^\prime$};
|
|
|
|
\draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$};
|
|
|
|
\draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{d\mathcal{L}}$};
|
|
\draw[<-] (J.west)node[above left]{$\bm{r}_{\mathcal{X}_n}$} -- ++(-1, 0);
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:control_architecture_hac_iff_L
|
|
#+caption: HAC-LAC: IFF + Control in the frame of the legs
|
|
#+RESULTS:
|
|
[[file:figs/control_architecture_hac_iff_L.png]]
|
|
|
|
*** 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
|
|
addpath('./matlab/mat/');
|
|
addpath('./matlab/src/');
|
|
addpath('./matlab/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
addpath('./mat/');
|
|
addpath('./src/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
load('damped_plant_enc_plates.mat', 'f', 'Ts', 'G_enc_iff_opt')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
%% Add all useful folders to the path
|
|
addpath('matlab/nass-simscape/matlab/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/png/')
|
|
addpath('matlab/nass-simscape/src/')
|
|
addpath('matlab/nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
%% Add all useful folders to the path
|
|
addpath('nass-simscape/matlab/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/png/')
|
|
addpath('nass-simscape/src/')
|
|
addpath('nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Open Simulink Model
|
|
mdl = 'nano_hexapod_simscape';
|
|
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
open(mdl)
|
|
|
|
Rx = zeros(1, 7);
|
|
|
|
colors = colororder;
|
|
#+end_src
|
|
|
|
*** Simscape Model
|
|
Let's start with the Simscape model and the damped plant.
|
|
|
|
Apply HAC control and verify the system is stable.
|
|
|
|
Then, try the control strategy on the real plant.
|
|
#+begin_src matlab
|
|
load('Kiff.mat', 'Kiff')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Initialize the Simscape model in closed loop
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', 'flexible', ...
|
|
'controller_type', 'iff');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Plate Displacement (encoder)
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
Gd_iff_opt = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports both :tangle no
|
|
isstable(Gd_iff_opt)
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: 1
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the transfer function from u to dLm
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
% Diagonal Elements Model
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff_opt(i,i), freqs, 'Hz'))), 'k-');
|
|
end
|
|
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d\mathcal{L}_m/u$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-8, 1e-4]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff_opt(i,i), freqs, 'Hz'))), 'k-');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/hac_iff_struts_enc_plates_plant_bode.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:hac_iff_struts_enc_plates_plant_bode
|
|
#+caption: Transfer function from $u$ to $d\mathcal{L}_m$ with IFF (diagonal elements)
|
|
#+RESULTS:
|
|
[[file:figs/hac_iff_struts_enc_plates_plant_bode.png]]
|
|
|
|
*** HAC Controller
|
|
Let's try to have 100Hz bandwidth:
|
|
#+begin_src matlab
|
|
%% Lead
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
|
wc = 2*pi*100; % Frequency with the maximum phase lead [rad/s]
|
|
|
|
H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
|
|
|
%% Low Pass filter
|
|
H_lpf = 1/(1 + s/2/pi/200);
|
|
|
|
%% Notch
|
|
gm = 0.02;
|
|
xi = 0.3;
|
|
wn = 2*pi*700;
|
|
|
|
H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
Khac_iff_struts = -(1/(2.87e-5)) * ... % Gain
|
|
H_lead * ... % Lead
|
|
H_notch * ... % Notch
|
|
(2*pi*100/s) * ... % Integrator
|
|
eye(6); % 6x6 Diagonal
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
save('matlab/mat/Khac_iff_struts.mat', 'Khac_iff_struts')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :eval no
|
|
save('mat/Khac_iff_struts.mat', 'Khac_iff_struts')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
Lhac_iff_struts = Khac_iff_struts*Gd_iff_opt;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the transfer function from u to dLm
|
|
freqs = 2*logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
% Diagonal Elements Model
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(Lhac_iff_struts(i,i), freqs, 'Hz'))), 'k-');
|
|
end
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(Lhac_iff_struts(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2]);
|
|
end
|
|
end
|
|
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/V_{exc}$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-3, 1e2]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Lhac_iff_struts(i,i), freqs, 'Hz'))), 'k-');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([2, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/loop_gain_hac_iff_struts.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:loop_gain_hac_iff_struts
|
|
#+caption: Diagonal and off-diagonal elements of the Loop gain for "HAC-IFF-Struts"
|
|
#+RESULTS:
|
|
[[file:figs/loop_gain_hac_iff_struts.png]]
|
|
|
|
|
|
*** Experimental Loop Gain
|
|
- [ ] Find a more clever way to do the multiplication
|
|
|
|
#+begin_src matlab
|
|
L_frf = zeros(size(G_enc_iff_opt));
|
|
|
|
for i = 1:size(G_enc_iff_opt, 1)
|
|
L_frf(i, :, :) = squeeze(G_enc_iff_opt(i,:,:))*freqresp(Khac_iff_struts, f(i), 'Hz');
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the transfer function from u to dLm
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
% Diagonal Elements FRF
|
|
plot(f, abs(L_frf(:,1,1)), 'color', colors(1,:), ...
|
|
'DisplayName', 'Diagonal');
|
|
for i = 2:6
|
|
plot(f, abs(L_frf(:,i,i)), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(f, abs(L_frf(:,1,2)), 'color', [colors(2,:), 0.2], ...
|
|
'DisplayName', 'Off-Diag');
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(L_frf(:,i,j)), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain [-]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-3, 1e2]);
|
|
legend('location', 'northeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(f, 180/pi*angle(L_frf(:,i,i)), 'color', colors(1,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([1, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/hac_iff_plates_exp_loop_gain_diag.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:hac_iff_plates_exp_loop_gain_diag
|
|
#+caption: Diagonal and Off-diagonal elements of the Loop gain (experimental data)
|
|
#+RESULTS:
|
|
[[file:figs/hac_iff_plates_exp_loop_gain_diag.png]]
|
|
|
|
*** Verification of the Stability using the Simscape model
|
|
#+begin_src matlab
|
|
%% Initialize the Simscape model in closed loop
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', 'flexible', ...
|
|
'controller_type', 'hac-iff-struts');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
Gd_iff_hac_opt = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports both
|
|
isstable(Gd_iff_hac_opt)
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: 1
|
|
|
|
** Reference Tracking
|
|
<<sec:hac_iff_struts_ref_track>>
|
|
*** 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
|
|
addpath('./matlab/mat/');
|
|
addpath('./matlab/src/');
|
|
addpath('./matlab/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
addpath('./mat/');
|
|
addpath('./src/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
%% Add all useful folders to the path
|
|
addpath('matlab/nass-simscape/matlab/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/png/')
|
|
addpath('matlab/nass-simscape/src/')
|
|
addpath('matlab/nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
%% Add all useful folders to the path
|
|
addpath('nass-simscape/matlab/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/png/')
|
|
addpath('nass-simscape/src/')
|
|
addpath('nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Open Simulink Model
|
|
mdl = 'nano_hexapod_simscape';
|
|
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
open(mdl)
|
|
|
|
colors = colororder;
|
|
#+end_src
|
|
|
|
*** Load
|
|
#+begin_src matlab
|
|
load('Khac_iff_struts.mat', 'Khac_iff_struts')
|
|
#+end_src
|
|
|
|
*** Y-Z Scans
|
|
**** Generate the Scan
|
|
#+begin_src matlab
|
|
Rx_yz = generateYZScanTrajectory(...
|
|
'y_tot', 4e-6, ...
|
|
'z_tot', 8e-6, ...
|
|
'n', 5, ...
|
|
'Ts', 1e-3, ...
|
|
'ti', 2, ...
|
|
'tw', 0.5, ...
|
|
'ty', 2, ...
|
|
'tz', 1);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
figure;
|
|
hold on;
|
|
plot(Rx_yz(:,1), Rx_yz(:,3), ...
|
|
'DisplayName', 'Y motion')
|
|
plot(Rx_yz(:,1), Rx_yz(:,4), ...
|
|
'DisplayName', 'Z motion')
|
|
hold off;
|
|
xlabel('Time [s]');
|
|
ylabel('Displacement [m]');
|
|
legend('location', 'northeast');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
figure;
|
|
plot(Rx_yz(:,3), Rx_yz(:,4));
|
|
xlabel('y [m]'); ylabel('z [m]');
|
|
#+end_src
|
|
|
|
**** Reference Signal for the Strut lengths
|
|
Let's use the Jacobian to estimate the wanted strut length as a function of time.
|
|
#+begin_src matlab
|
|
dL_ref = [n_hexapod.geometry.J*Rx_yz(:, 2:7)']';
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
figure;
|
|
hold on;
|
|
for i=1:6
|
|
plot(Rx_yz(:,1), dL_ref(:, i))
|
|
end
|
|
xlabel('Time [s]'); ylabel('Displacement [m]');
|
|
#+end_src
|
|
|
|
**** Time domain simulation with 2DoF model
|
|
#+begin_src matlab
|
|
%% Initialize the Simscape model in closed loop
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '2dof', ...
|
|
'flex_top_type', '3dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'hac-iff-struts');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
set_param(mdl,'StopTime', num2str(Rx_yz(end,1)))
|
|
set_param(mdl,'SimulationCommand','start')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
out.X.Data = out.X.Data - out.X.Data(1,:);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
hold on;
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(1e6*out.X.Data(:,2), 1e6*out.X.Data(:,3), '-', ...
|
|
'DisplayName', 'Meas. Motion')
|
|
plot(1e6*Rx_yz(:,3), 1e6*Rx_yz(:,4), 'k--', ...
|
|
'DisplayName', 'Reference Path')
|
|
hold off;
|
|
xlabel('X displacement [$\mu m$]'); ylabel('Y displacement [$\mu m$]');
|
|
legend('location', 'southwest');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/ref_track_hac_iff_struts_yz_plane.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:ref_track_hac_iff_struts_yz_plane
|
|
#+caption: Simulated Y-Z motion
|
|
#+RESULTS:
|
|
[[file:figs/ref_track_hac_iff_struts_yz_plane.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
hold on;
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(out.X.Time, out.X.Data(:,2), '-', ...
|
|
'DisplayName', 'Meas. - Y')
|
|
plot(Rx_yz(:,1), Rx_yz(:,3), 'k--', ...
|
|
'DisplayName', 'Ref - Y')
|
|
plot(out.X.Time, out.X.Data(:,3), '-', ...
|
|
'DisplayName', 'Meas - Z')
|
|
plot(Rx_yz(:,1), Rx_yz(:,4), 'k-.', ...
|
|
'DisplayName', 'Ref - Z')
|
|
hold off;
|
|
xlabel('Time [s]'); ylabel('Y Displacement [$\mu m$]');
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/ref_track_hac_iff_struts_yz_time.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:ref_track_hac_iff_struts_yz_time
|
|
#+caption: Y and Z motion as a function of time as well as the reference signals
|
|
#+RESULTS:
|
|
[[file:figs/ref_track_hac_iff_struts_yz_time.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
tiledlayout(1, 2, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile;
|
|
hold on;
|
|
plot(out.X.Time, 1e9*(out.X.Data(:,1) - Rx_yz(:,2)), 'DisplayName', '$\epsilon_x$')
|
|
plot(out.X.Time, 1e9*(out.X.Data(:,2) - Rx_yz(:,3)), 'DisplayName', '$\epsilon_y$')
|
|
plot(out.X.Time, 1e9*(out.X.Data(:,3) - Rx_yz(:,4)), 'DisplayName', '$\epsilon_z$')
|
|
hold off;
|
|
xlabel('Time [s]'); ylabel('Position Errors [nm]');
|
|
legend('location', 'northeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(out.X.Time, 1e6*(out.X.Data(:,4) - Rx_yz(:,5)), 'DisplayName', '$\epsilon_{R_x}$')
|
|
plot(out.X.Time, 1e6*(out.X.Data(:,5) - Rx_yz(:,6)), 'DisplayName', '$\epsilon_{R_y}$')
|
|
plot(out.X.Time, 1e6*(out.X.Data(:,6) - Rx_yz(:,7)), 'DisplayName', '$\epsilon_{R_z}$')
|
|
hold off;
|
|
xlabel('Time [s]'); ylabel('Orientation Errors [$\mu rad$]');
|
|
legend('location', 'northeast');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/ref_track_hac_iff_struts_pos_error.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:ref_track_hac_iff_struts_pos_error
|
|
#+caption: Positioning errors as a function of time
|
|
#+RESULTS:
|
|
[[file:figs/ref_track_hac_iff_struts_pos_error.png]]
|
|
|
|
*** "NASS" reference path
|
|
**** Generate Path
|
|
#+begin_src matlab
|
|
ref_path = [ ...
|
|
0, 0, 0;
|
|
0, 0, 1; % N
|
|
0, 4, 1;
|
|
3, 0, 1;
|
|
3, 4, 1;
|
|
3, 4, 0;
|
|
4, 0, 0;
|
|
4, 0, 1; % A
|
|
4, 3, 1;
|
|
5, 4, 1;
|
|
6, 4, 1;
|
|
7, 3, 1;
|
|
7, 2, 1;
|
|
4, 2, 1;
|
|
4, 3, 1;
|
|
5, 4, 1;
|
|
6, 4, 1;
|
|
7, 3, 1;
|
|
7, 0, 1;
|
|
7, 0, 0;
|
|
8, 0, 0;
|
|
8, 0, 1; % S
|
|
11, 0, 1;
|
|
11, 2, 1;
|
|
8, 2, 1;
|
|
8, 4, 1;
|
|
11, 4, 1;
|
|
11, 4, 0;
|
|
12, 0, 0;
|
|
12, 0, 1; % S
|
|
15, 0, 1;
|
|
15, 2, 1;
|
|
12, 2, 1;
|
|
12, 4, 1;
|
|
15, 4, 1;
|
|
15, 4, 0;
|
|
];
|
|
|
|
% Center the trajectory arround zero
|
|
ref_path = ref_path - (max(ref_path) - min(ref_path))/2;
|
|
|
|
% Define the X-Y-Z cuboid dimensions containing the trajectory
|
|
X_max = 10e-6;
|
|
Y_max = 4e-6;
|
|
Z_max = 2e-6;
|
|
|
|
ref_path = ([X_max, Y_max, Z_max]./max(ref_path)).*ref_path; % [m]
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
Rx_nass = generateXYZTrajectory('points', ref_path);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
figure;
|
|
plot(1e6*Rx_nass(Rx_nass(:,4)>0, 2), 1e6*Rx_nass(Rx_nass(:,4)>0, 3), 'k.')
|
|
xlabel('X [$\mu m$]');
|
|
ylabel('Y [$\mu m$]');
|
|
axis equal;
|
|
xlim(1e6*[min(Rx_nass(:,2)), max(Rx_nass(:,2))]);
|
|
ylim(1e6*[min(Rx_nass(:,3)), max(Rx_nass(:,3))]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/ref_track_test_nass.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:ref_track_test_nass
|
|
#+caption: Reference path corresponding to the "NASS" acronym
|
|
#+RESULTS:
|
|
[[file:figs/ref_track_test_nass.png]]
|
|
|
|
#+begin_src matlab
|
|
figure;
|
|
plot3(Rx_nass(:,2), Rx_nass(:,3), Rx_nass(:,4), 'k-');
|
|
xlabel('x');
|
|
ylabel('y');
|
|
zlabel('z');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
figure;
|
|
hold on;
|
|
plot(Rx_nass(:,1), Rx_nass(:,2));
|
|
plot(Rx_nass(:,1), Rx_nass(:,3));
|
|
plot(Rx_nass(:,1), Rx_nass(:,4));
|
|
hold off;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
figure;
|
|
scatter(Rx_nass(:,2), Rx_nass(:,3), 1, Rx_nass(:,4), 'filled')
|
|
colormap winter
|
|
#+end_src
|
|
**** Simscape Simulations
|
|
#+begin_src matlab
|
|
%% Initialize the Simscape model in closed loop
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '2dof', ...
|
|
'flex_top_type', '3dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'hac-iff-struts');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
set_param(mdl,'StopTime', num2str(Rx_nass(end,1)))
|
|
set_param(mdl,'SimulationCommand','start')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
out.X.Data = out.X.Data - out.X.Data(1,:);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
hold on;
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(1e6*out.X.Data(out.X.Data(:,3)>0, 1), 1e6*out.X.Data(out.X.Data(:,3)>0, 2), '-', ...
|
|
'DisplayName', 'Meas. Motion')
|
|
plot(1e6*Rx_nass(Rx_nass(:,4)>0, 2), 1e6*Rx_nass(Rx_nass(:,4)>0, 3), 'k--', ...
|
|
'DisplayName', 'Reference Path')
|
|
hold off;
|
|
xlabel('X displacement [$\mu m$]'); ylabel('Y displacement [$\mu m$]');
|
|
legend('location', 'southwest');
|
|
#+end_src
|
|
|
|
*** Save Reference paths
|
|
#+begin_src matlab :tangle no
|
|
save('matlab/mat/reference_path.mat', 'Rx_yz', 'Rx_nass')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :eval no
|
|
save('mat/reference_path.mat', 'Rx_yz', 'Rx_nass')
|
|
#+end_src
|
|
|
|
*** Experimental Results
|
|
|
|
** Feedforward (Open-Loop) Control
|
|
*** Introduction
|
|
|
|
#+begin_src latex :file control_architecture_iff_feedforward.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block={3.0cm}{3.0cm}] (P) {Plant};
|
|
\coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
|
|
\coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
|
|
\coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
|
|
\coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);
|
|
|
|
\node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
|
|
\node[addb, left= of inputF] (addF) {};
|
|
\node[block, left= of addF] (Kff) {$\bm{K}_{\mathcal{L},\text{ff}}$};
|
|
\node[block, align=center, left= of Kff] (J) {Inverse\\Kinematics};
|
|
|
|
% Connections and labels
|
|
\draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
|
|
\draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
|
|
\draw[->] (Kiff.west) -| (addF.north);
|
|
\draw[->] (addF.east) -- (inputF) node[above left]{$\bm{u}$};
|
|
|
|
\draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
|
|
\draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$};
|
|
|
|
\draw[->] (Kff.east) -- (addF.west) node[above left]{$\bm{u}_{\text{ff}}$};
|
|
\draw[->] (J.east) -- (Kff.west) node[above left]{$\bm{r}_{d\mathcal{L}}$};
|
|
\draw[<-] (J.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:control_architecture_iff_feedforward
|
|
#+caption: Feedforward control in the frame of the legs
|
|
#+RESULTS:
|
|
[[file:figs/control_architecture_iff_feedforward.png]]
|
|
|
|
*** 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
|
|
addpath('./matlab/mat/');
|
|
addpath('./matlab/src/');
|
|
addpath('./matlab/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
addpath('./mat/');
|
|
addpath('./src/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
load('damped_plant_enc_plates.mat', 'f', 'Ts', 'G_enc_iff_opt')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
%% Add all useful folders to the path
|
|
addpath('matlab/nass-simscape/matlab/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/png/')
|
|
addpath('matlab/nass-simscape/src/')
|
|
addpath('matlab/nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
%% Add all useful folders to the path
|
|
addpath('nass-simscape/matlab/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/png/')
|
|
addpath('nass-simscape/src/')
|
|
addpath('nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Open Simulink Model
|
|
mdl = 'nano_hexapod_simscape';
|
|
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
open(mdl)
|
|
|
|
Rx = zeros(1, 7);
|
|
|
|
colors = colororder;
|
|
#+end_src
|
|
|
|
*** Simple Feedforward Controller
|
|
Let's estimate the mean DC gain for the damped plant (diagonal elements:)
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
mean(diag(abs(squeeze(mean(G_enc_iff_opt(f>2 & f<4,:,:))))))
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: 1.773e-05
|
|
|
|
The feedforward controller is then taken as the inverse of this gain (the minus sign is there manually added as it is "removed" by the =abs= function):
|
|
#+begin_src matlab
|
|
Kff_iff_L = -1/mean(diag(abs(squeeze(mean(G_enc_iff_opt(f>2 & f<4,:,:))))));
|
|
#+end_src
|
|
|
|
The open-loop gain (feedforward controller times the damped plant) is shown in Figure [[fig:open_loop_gain_feedforward_iff_struts]].
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the transfer function from u to dLm for tested values of the IFF gain
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(f, abs(Kff_iff_L*G_enc_iff_opt(:,i,i)), 'k-');
|
|
end
|
|
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [-]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-2, 1e1]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(f, 180/pi*angle(Kff_iff_L*G_enc_iff_opt(:,i,i)), 'k-')
|
|
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');
|
|
xlim([1, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/open_loop_gain_feedforward_iff_struts.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:open_loop_gain_feedforward_iff_struts
|
|
#+caption: Diagonal elements of the "open loop gain"
|
|
#+RESULTS:
|
|
[[file:figs/open_loop_gain_feedforward_iff_struts.png]]
|
|
|
|
And save the feedforward controller for further use:
|
|
#+begin_src matlab
|
|
Kff_iff_L = zpk(Kff_iff_L)*eye(6);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
save('matlab/mat/feedforward_iff.mat', 'Kff_iff_L')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :eval no
|
|
save('mat/feedforward_iff.mat', 'Kff_iff_L')
|
|
#+end_src
|
|
|
|
*** Test with Simscape Model
|
|
#+begin_src matlab
|
|
load('reference_path.mat', 'Rx_yz');
|
|
#+end_src
|
|
|
|
** Feedback/Feedforward control in the frame of the struts
|
|
*** Introduction :ignore:
|
|
|
|
#+begin_src latex :file control_architecture_hac_iff_L_feedforward.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block={3.0cm}{3.0cm}] (P) {Plant};
|
|
\coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
|
|
\coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
|
|
\coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
|
|
\coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);
|
|
|
|
\node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
|
|
\node[addb, left= of inputF] (addF) {};
|
|
\node[block, left= of addF] (K) {$\bm{K}_\mathcal{L}$};
|
|
\node[block, above= of K] (Kff) {$\bm{K}_{\mathcal{L},\text{ff}}$};
|
|
\node[addb, left= of K] (subr) {};
|
|
\node[block, align=center, left= of subr] (J) {Inverse\\Kinematics};
|
|
|
|
% Connections and labels
|
|
\draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
|
|
\draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
|
|
\draw[->] (Kiff.west) -| (addF.north);
|
|
\draw[->] (addF.east) -- (inputF) node[above left]{$\bm{u}$};
|
|
|
|
\draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
|
|
\draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, -1) -| (subr.south);
|
|
\draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{d\mathcal{L}}$};
|
|
\draw[->] (K.east) -- (addF.west) node[above left]{$\bm{u}^\prime$};
|
|
|
|
\draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}_n$};
|
|
|
|
\draw[->] (J.east) -- (subr.west);
|
|
\draw[->] ($(J.east) + (0.4, 0)$)node[branch]{} node[below]{$\bm{r}_{d\mathcal{L}}$} |- (Kff.west);
|
|
\draw[->] (Kff.east) -- ++(0.5, 0) -- (addF.north west);
|
|
|
|
\draw[<-] (J.west)node[above left]{$\bm{r}_{\mathcal{X}_n}$} -- ++(-1, 0);
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:control_architecture_hac_iff_L_feedforward
|
|
#+caption: Feedback/Feedforward control in the frame of the legs
|
|
#+RESULTS:
|
|
[[file:figs/control_architecture_hac_iff_L_feedforward.png]]
|
|
|
|
|
|
* Functions
|
|
** =generateXYZTrajectory=
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/generateXYZTrajectory.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:generateXYZTrajectory>>
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [ref] = generateXYZTrajectory(args)
|
|
% generateXYZTrajectory -
|
|
%
|
|
% Syntax: [ref] = generateXYZTrajectory(args)
|
|
%
|
|
% Inputs:
|
|
% - args
|
|
%
|
|
% Outputs:
|
|
% - ref - Reference Signal
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
arguments
|
|
args.points double {mustBeNumeric} = zeros(2, 3) % [m]
|
|
|
|
args.ti (1,1) double {mustBeNumeric, mustBePositive} = 1 % Time to go to first point and after last point [s]
|
|
args.tw (1,1) double {mustBeNumeric, mustBePositive} = 0.5 % Time wait between each point [s]
|
|
args.tm (1,1) double {mustBeNumeric, mustBePositive} = 1 % Motion time between points [s]
|
|
|
|
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Sampling Time [s]
|
|
end
|
|
#+end_src
|
|
|
|
*** Initialize Time Vectors
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
time_i = 0:args.Ts:args.ti;
|
|
time_w = 0:args.Ts:args.tw;
|
|
time_m = 0:args.Ts:args.tm;
|
|
#+end_src
|
|
|
|
*** XYZ Trajectory
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
% Go to initial position
|
|
xyz = (args.points(1,:))'*(time_i/args.ti);
|
|
|
|
% Wait
|
|
xyz = [xyz, xyz(:,end).*ones(size(time_w))];
|
|
|
|
% Scans
|
|
for i = 2:size(args.points, 1)
|
|
% Go to next point
|
|
xyz = [xyz, xyz(:,end) + (args.points(i,:)' - xyz(:,end))*(time_m/args.tm)];
|
|
% Wait a litle bit
|
|
xyz = [xyz, xyz(:,end).*ones(size(time_w))];
|
|
end
|
|
|
|
% End motion
|
|
xyz = [xyz, xyz(:,end) - xyz(:,end)*(time_i/args.ti)];
|
|
#+end_src
|
|
|
|
*** Reference Signal
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
t = 0:args.Ts:args.Ts*(length(xyz) - 1);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
ref = zeros(length(xyz), 7);
|
|
|
|
ref(:, 1) = t;
|
|
ref(:, 2:4) = xyz';
|
|
#+end_src
|
|
|
|
** =generateYZScanTrajectory=
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/generateYZScanTrajectory.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:generateYZScanTrajectory>>
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [ref] = generateYZScanTrajectory(args)
|
|
% generateYZScanTrajectory -
|
|
%
|
|
% Syntax: [ref] = generateYZScanTrajectory(args)
|
|
%
|
|
% Inputs:
|
|
% - args
|
|
%
|
|
% Outputs:
|
|
% - ref - Reference Signal
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
arguments
|
|
args.y_tot (1,1) double {mustBeNumeric} = 10e-6 % [m]
|
|
args.z_tot (1,1) double {mustBeNumeric} = 10e-6 % [m]
|
|
|
|
args.n (1,1) double {mustBeInteger, mustBePositive} = 10 % [-]
|
|
|
|
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-4 % [s]
|
|
|
|
args.ti (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s]
|
|
args.tw (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s]
|
|
args.ty (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s]
|
|
args.tz (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s]
|
|
end
|
|
#+end_src
|
|
|
|
*** Initialize Time Vectors
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
time_i = 0:args.Ts:args.ti;
|
|
time_w = 0:args.Ts:args.tw;
|
|
time_y = 0:args.Ts:args.ty;
|
|
time_z = 0:args.Ts:args.tz;
|
|
#+end_src
|
|
|
|
*** Y and Z vectors
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
% Go to initial position
|
|
y = (time_i/args.ti)*(args.y_tot/2);
|
|
|
|
% Wait
|
|
y = [y, y(end)*ones(size(time_w))];
|
|
|
|
% Scans
|
|
for i = 1:args.n
|
|
if mod(i,2) == 0
|
|
y = [y, -(args.y_tot/2) + (time_y/args.ty)*args.y_tot];
|
|
else
|
|
y = [y, (args.y_tot/2) - (time_y/args.ty)*args.y_tot];
|
|
end
|
|
|
|
if i < args.n
|
|
y = [y, y(end)*ones(size(time_z))];
|
|
end
|
|
end
|
|
|
|
% Wait a litle bit
|
|
y = [y, y(end)*ones(size(time_w))];
|
|
|
|
% End motion
|
|
y = [y, y(end) - y(end)*time_i/args.ti];
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
% Go to initial position
|
|
z = (time_i/args.ti)*(args.z_tot/2);
|
|
|
|
% Wait
|
|
z = [z, z(end)*ones(size(time_w))];
|
|
|
|
% Scans
|
|
for i = 1:args.n
|
|
z = [z, z(end)*ones(size(time_y))];
|
|
|
|
if i < args.n
|
|
z = [z, z(end) - (time_z/args.tz)*args.z_tot/(args.n-1)];
|
|
end
|
|
end
|
|
|
|
% Wait a litle bit
|
|
z = [z, z(end)*ones(size(time_w))];
|
|
|
|
% End motion
|
|
z = [z, z(end) - z(end)*time_i/args.ti];
|
|
#+end_src
|
|
|
|
*** Reference Signal
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
t = 0:args.Ts:args.Ts*(length(y) - 1);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
ref = zeros(length(y), 7);
|
|
|
|
ref(:, 1) = t;
|
|
ref(:, 3) = y;
|
|
ref(:, 4) = z;
|
|
#+end_src
|
|
|
|
** =getTransformationMatrixAcc=
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/getTransformationMatrixAcc.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:getTransformationMatrixAcc>>
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [M] = getTransformationMatrixAcc(Opm, Osm)
|
|
% getTransformationMatrixAcc -
|
|
%
|
|
% Syntax: [M] = getTransformationMatrixAcc(Opm, Osm)
|
|
%
|
|
% Inputs:
|
|
% - Opm - Nx3 (N = number of accelerometer measurements) X,Y,Z position of accelerometers
|
|
% - Opm - Nx3 (N = number of accelerometer measurements) Unit vectors representing the accelerometer orientation
|
|
%
|
|
% Outputs:
|
|
% - M - Transformation Matrix
|
|
#+end_src
|
|
|
|
*** Transformation matrix from motion of the solid body to accelerometer measurements
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
Let's try to estimate the x-y-z acceleration of any point of the solid body from the acceleration/angular acceleration of the solid body expressed in $\{O\}$.
|
|
For any point $p_i$ of the solid body (corresponding to an accelerometer), we can write:
|
|
\begin{equation}
|
|
\begin{bmatrix}
|
|
a_{i,x} \\ a_{i,y} \\ a_{i,z}
|
|
\end{bmatrix} = \begin{bmatrix}
|
|
\dot{v}_x \\ \dot{v}_y \\ \dot{v}_z
|
|
\end{bmatrix} + p_i \times \begin{bmatrix}
|
|
\dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
We can write the cross product as a matrix product using the skew-symmetric transformation:
|
|
\begin{equation}
|
|
\begin{bmatrix}
|
|
a_{i,x} \\ a_{i,y} \\ a_{i,z}
|
|
\end{bmatrix} = \begin{bmatrix}
|
|
\dot{v}_x \\ \dot{v}_y \\ \dot{v}_z
|
|
\end{bmatrix} + \underbrace{\begin{bmatrix}
|
|
0 & p_{i,z} & -p_{i,y} \\
|
|
-p_{i,z} & 0 & p_{i,x} \\
|
|
p_{i,y} & -p_{i,x} & 0
|
|
\end{bmatrix}}_{P_{i,[\times]}} \cdot \begin{bmatrix}
|
|
\dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
If we now want to know the (scalar) acceleration $a_i$ of the point $p_i$ in the direction of the accelerometer direction $\hat{s}_i$, we can just project the 3d acceleration on $\hat{s}_i$:
|
|
\begin{equation}
|
|
a_i = \hat{s}_i^T \cdot \begin{bmatrix}
|
|
a_{i,x} \\ a_{i,y} \\ a_{i,z}
|
|
\end{bmatrix} = \hat{s}_i^T \cdot \begin{bmatrix}
|
|
\dot{v}_x \\ \dot{v}_y \\ \dot{v}_z
|
|
\end{bmatrix} + \left( \hat{s}_i^T \cdot P_{i,[\times]} \right) \cdot \begin{bmatrix}
|
|
\dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
Which is equivalent as a simple vector multiplication:
|
|
\begin{equation}
|
|
a_i = \begin{bmatrix}
|
|
\hat{s}_i^T & \hat{s}_i^T \cdot P_{i,[\times]}
|
|
\end{bmatrix}
|
|
\begin{bmatrix}
|
|
\dot{v}_x \\ \dot{v}_y \\ \dot{v}_z \\ \dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z
|
|
\end{bmatrix} = \begin{bmatrix}
|
|
\hat{s}_i^T & \hat{s}_i^T \cdot P_{i,[\times]}
|
|
\end{bmatrix} {}^O\vec{x}
|
|
\end{equation}
|
|
|
|
And finally we can combine the 6 (line) vectors for the 6 accelerometers to write that in a matrix form.
|
|
We obtain Eq. eqref:eq:M_matrix.
|
|
#+begin_important
|
|
The transformation from solid body acceleration ${}^O\vec{x}$ from sensor measured acceleration $\vec{a}$ is:
|
|
\begin{equation} \label{eq:M_matrix}
|
|
\vec{a} = \underbrace{\begin{bmatrix}
|
|
\hat{s}_1^T & \hat{s}_1^T \cdot P_{1,[\times]} \\
|
|
\vdots & \vdots \\
|
|
\hat{s}_6^T & \hat{s}_6^T \cdot P_{6,[\times]}
|
|
\end{bmatrix}}_{M} {}^O\vec{x}
|
|
\end{equation}
|
|
|
|
with $\hat{s}_i$ the unit vector representing the measured direction of the i'th accelerometer expressed in frame $\{O\}$ and $P_{i,[\times]}$ the skew-symmetric matrix representing the cross product of the position of the i'th accelerometer expressed in frame $\{O\}$.
|
|
#+end_important
|
|
|
|
Let's define such matrix using matlab:
|
|
#+begin_src matlab
|
|
M = zeros(length(Opm), 6);
|
|
|
|
for i = 1:length(Opm)
|
|
Ri = [0, Opm(3,i), -Opm(2,i);
|
|
-Opm(3,i), 0, Opm(1,i);
|
|
Opm(2,i), -Opm(1,i), 0];
|
|
M(i, 1:3) = Osm(:,i)';
|
|
M(i, 4:6) = Osm(:,i)'*Ri;
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
end
|
|
#+end_src
|
|
|
|
|
|
** =getJacobianNanoHexapod=
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/getJacobianNanoHexapod.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:getJacobianNanoHexapod>>
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [J] = getJacobianNanoHexapod(Hbm)
|
|
% getJacobianNanoHexapod -
|
|
%
|
|
% Syntax: [J] = getJacobianNanoHexapod(Hbm)
|
|
%
|
|
% Inputs:
|
|
% - Hbm - Height of {B} w.r.t. {M} [m]
|
|
%
|
|
% Outputs:
|
|
% - J - Jacobian Matrix
|
|
#+end_src
|
|
|
|
*** Transformation matrix from motion of the solid body to accelerometer measurements
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
Fa = [[-86.05, -74.78, 22.49],
|
|
[ 86.05, -74.78, 22.49],
|
|
[ 107.79, -37.13, 22.49],
|
|
[ 21.74, 111.91, 22.49],
|
|
[-21.74, 111.91, 22.49],
|
|
[-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m]
|
|
|
|
Mb = [[-28.47, -106.25, -22.50],
|
|
[ 28.47, -106.25, -22.50],
|
|
[ 106.25, 28.47, -22.50],
|
|
[ 77.78, 77.78, -22.50],
|
|
[-77.78, 77.78, -22.50],
|
|
[-106.25, 28.47, -22.50]]'*1e-3; % Bi w.r.t. {M} [m]
|
|
|
|
H = 95e-3; % Stewart platform height [m]
|
|
Fb = Mb + [0; 0; H]; % Bi w.r.t. {F} [m]
|
|
|
|
si = Fb - Fa;
|
|
si = si./vecnorm(si); % Normalize
|
|
|
|
Bb = Mb - [0; 0; Hbm];
|
|
|
|
J = [si', cross(Bb, si)'];
|
|
#+end_src
|