#+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

* Introduction                                                        :ignore:
* Test-Bench Description

#+begin_note
Here are the documentation of the equipment used for this test bench:
- 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_nano_hexapod
#+caption: Nano-Hexapod
[[file:figs/IMG_20210608_152917.jpg]]

#+name: fig:picture_bench_granite_overview
#+caption: Nano-Hexapod and the control electronics
[[file:figs/IMG_20210608_154722.jpg]]

* Encoders fixed to the Struts
** Introduction

** 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 Data
#+begin_src matlab
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
% 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));
#+end_src

And we get the frequency vector.
#+begin_src matlab
[~, f] = tfestimate(meas_data_lf{1}.Va, meas_data_lf{1}.de, win, [], [], 1/Ts);
#+end_src

#+begin_src matlab
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_lf = zeros(length(f), 6, 6);
coh_dvf_hf = zeros(length(f), 6, 6);

for i = 1:6
    coh_dvf_lf(:, :, i) = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts);
    coh_dvf_hf(:, :, i) = mscohere(meas_data_hf{i}.Va, meas_data_hf{i}.de, win, [], [], 1/Ts);
end

#+end_src

#+begin_src matlab :exports none
figure;
hold on;
for i = 1:5
    for j = i+1:6
        plot(f(i_lf), coh_dvf_lf(i_lf, i, j), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
        plot(f(i_hf), coh_dvf_hf(i_hf, i, j), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f(i_lf), coh_dvf_lf(i_lf,i, i), ...
         'DisplayName', sprintf('$G_{dvf}(%i,%i)$', i, i));
    set(gca,'ColorOrderIndex',i)
    plot(f(i_hf), coh_dvf_hf(i_hf,i, i), ...
         'HandleVisibility', 'off');
end
plot(f(i_lf), coh_dvf_lf(i_lf, 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
G_dvf_lf = zeros(length(f), 6, 6);
G_dvf_hf = zeros(length(f), 6, 6);

for i = 1:6
    G_dvf_lf(:, :, i) = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts);
    G_dvf_hf(:, :, i) = tfestimate(meas_data_hf{i}.Va, meas_data_hf{i}.de, win, [], [], 1/Ts);
end
#+end_src

#+begin_src matlab :exports none
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(i_lf), abs(G_dvf_lf(i_lf, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
        plot(f(i_hf), abs(G_dvf_hf(i_hf, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f(i_lf), abs(G_dvf_lf(i_lf,i, i)), ...
         'DisplayName', sprintf('$G_{dvf}(%i,%i)$', i, i));
    set(gca,'ColorOrderIndex',i)
    plot(f(i_hf), abs(G_dvf_hf(i_hf,i, i)), ...
         'HandleVisibility', 'off');
end
plot(f(i_lf), abs(G_dvf_lf(i_lf, 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(i_lf), 180/pi*angle(G_dvf_lf(i_lf,i, i)));
    set(gca,'ColorOrderIndex',i)
    plot(f(i_hf), 180/pi*angle(G_dvf_hf(i_hf,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
coh_iff_lf = zeros(length(f), 6, 6);
coh_iff_hf = zeros(length(f), 6, 6);

for i = 1:6
    coh_iff_lf(:, :, i) = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts);
    coh_iff_hf(:, :, i) = mscohere(meas_data_hf{i}.Va, meas_data_hf{i}.Vs, win, [], [], 1/Ts);
end

#+end_src

#+begin_src matlab :exports none
figure;
hold on;
for i = 1:5
    for j = i+1:6
        plot(f(i_lf), coh_iff_lf(i_lf, i, j), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
        plot(f(i_hf), coh_iff_hf(i_hf, i, j), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f(i_lf), coh_iff_lf(i_lf,i, i), ...
         'DisplayName', sprintf('$G_{iff}(%i,%i)$', i, i));
    set(gca,'ColorOrderIndex',i)
    plot(f(i_hf), coh_iff_hf(i_hf,i, i), ...
         'HandleVisibility', 'off');
end
plot(f(i_lf), coh_iff_lf(i_lf, 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_lf = zeros(length(f), 6, 6);
G_iff_hf = zeros(length(f), 6, 6);

for i = 1:6
    G_iff_lf(:, :, i) = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts);
    G_iff_hf(:, :, i) = tfestimate(meas_data_hf{i}.Va, meas_data_hf{i}.Vs, win, [], [], 1/Ts);
end
#+end_src

#+begin_src matlab :exports none
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(i_lf), abs(G_iff_lf(i_lf, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
        plot(f(i_hf), abs(G_iff_hf(i_hf, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f(i_lf), abs(G_iff_lf(i_lf,i, i)), ...
         'DisplayName', sprintf('$G_{iff}(%i,%i)$', i, i));
    set(gca,'ColorOrderIndex',i)
    plot(f(i_hf), abs(G_iff_hf(i_hf,i, i)), ...
         'HandleVisibility', 'off');
end
plot(f(i_lf), abs(G_iff_lf(i_lf, 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(i_lf), 180/pi*angle(G_iff_lf(i_lf,i, i)));
    set(gca,'ColorOrderIndex',i)
    plot(f(i_hf), 180/pi*angle(G_iff_hf(i_hf,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]]

** Jacobian
#+begin_src matlab
load('jacobian.mat', 'J');
#+end_src
*** DVF Plant
#+begin_src matlab
G_dvf_J_lf = permute(pagemtimes(inv(J), pagemtimes(permute(G_dvf_lf, [2 3 1]), inv(J'))), [3 1 2]);
G_dvf_J_hf = permute(pagemtimes(inv(J), pagemtimes(permute(G_dvf_hf, [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(i_lf), abs(G_dvf_J_lf(i_lf, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
        plot(f(i_hf), abs(G_dvf_J_hf(i_hf, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f(i_lf), abs(G_dvf_J_lf(i_lf,i, i)), ...
         'DisplayName', labels{i});
    set(gca,'ColorOrderIndex',i)
    plot(f(i_hf), abs(G_dvf_J_hf(i_hf,i, i)), ...
         'HandleVisibility', 'off');
end
plot(f(i_lf), abs(G_dvf_J_lf(i_lf, 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(i_lf), 180/pi*angle(G_dvf_J_lf(i_lf,i, i)));
    set(gca,'ColorOrderIndex',i)
    plot(f(i_hf), 180/pi*angle(G_dvf_J_hf(i_hf,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

*** IFF Plant
#+begin_src matlab
G_iff_J_lf = permute(pagemtimes(inv(J), pagemtimes(permute(G_iff_lf, [2 3 1]), inv(J'))), [3 1 2]);
G_iff_J_hf = permute(pagemtimes(inv(J), pagemtimes(permute(G_iff_hf, [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(i_lf), abs(G_iff_J_lf(i_lf, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
        plot(f(i_hf), abs(G_iff_J_hf(i_hf, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f(i_lf), abs(G_iff_J_lf(i_lf,i, i)), ...
         'DisplayName', labels{i});
    set(gca,'ColorOrderIndex',i)
    plot(f(i_hf), abs(G_iff_J_hf(i_hf,i, i)), ...
         'HandleVisibility', 'off');
end
plot(f(i_lf), abs(G_iff_J_lf(i_lf, 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(i_lf), 180/pi*angle(G_iff_J_lf(i_lf,i, i)));
    set(gca,'ColorOrderIndex',i)
    plot(f(i_hf), 180/pi*angle(G_iff_J_hf(i_hf,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