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

This report is also available as a pdf.

#+end_export #+latex: \clearpage * Introduction :ignore: In this document, the dynamics of the nano-hexapod shown in Figure [[fig:picture_bench_granite_nano_hexapod]] is identified. #+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 #+attr_latex: :width \linewidth [[file:figs/IMG_20210608_152917.jpg]] #+name: fig:picture_bench_granite_overview #+caption: Nano-Hexapod and the control electronics #+attr_latex: :width \linewidth [[file:figs/IMG_20210608_154722.jpg]] #+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 #+RESULTS: [[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}$ | * Encoders fixed to the Struts ** Introduction In this section, the encoders are fixed to the struts. ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+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 ** Identification of the dynamics *** Load 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_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 %% Coherence for the transfer function from u to dLm 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 (transfer function from u to dLm) 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 %% 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(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 for the IFF plant 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 %% 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(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 %% 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(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 :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 *** 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_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 #+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_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-3, 1e4]); 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 #+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 *** Introduction :ignore: In this section, the measured dynamics is compared with the dynamics estimated from the Simscape model. *** Initialize :noexport: #+begin_src matlab :tangle no %% Add all useful folders to the path addpath('matlab/') 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) #+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, '/F'], 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 #+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(i_lf), abs(G_iff_lf(i_lf,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(i_lf), abs(G_iff_lf(i_lf,i, i)), 'color', [0,0,0,0.2], ... 'HandleVisibility', 'off'); set(gca,'ColorOrderIndex',2) plot(f(i_hf), abs(G_iff_hf(i_hf,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(i_lf), 180/pi*angle(G_iff_lf(i_lf,i, i)), 'color', [0,0,0,0.2]); plot(f(i_hf), 180/pi*angle(G_iff_hf(i_hf,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(i_lf), abs(G_iff_lf(i_lf, 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(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 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', '2dof'); #+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, '/F'], 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(i_lf), abs(G_dvf_lf(i_lf,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(i_lf), abs(G_dvf_lf(i_lf,i, i)), 'color', [0,0,0,0.2], ... 'HandleVisibility', 'off'); set(gca,'ColorOrderIndex',2) plot(f(i_hf), abs(G_dvf_hf(i_hf,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(i_lf), 180/pi*angle(G_dvf_lf(i_lf,i, i)), 'color', [0,0,0,0.2]); plot(f(i_hf), 180/pi*angle(G_dvf_hf(i_hf,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(i_lf), abs(G_dvf_lf(i_lf, 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(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 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]] ** Integral Force Feedback *** 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 :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(i_lf), abs(squeeze(freqresp(Kiff(1,1), f(i_lf), 'Hz')).*G_iff_lf(i_lf,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(i_lf), abs(squeeze(freqresp(Kiff(1,1), f(i_lf), 'Hz')).*G_iff_lf(i_lf,i, i)), 'color', [0,0,0,0.2], ... 'HandleVisibility', 'off'); set(gca,'ColorOrderIndex',2) plot(f(i_hf), abs(squeeze(freqresp(Kiff(1,1), f(i_hf), 'Hz')).*G_iff_hf(i_hf,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(i_lf), 180/pi*angle(squeeze(freqresp(Kiff(1,1), f(i_lf), 'Hz')).*G_iff_lf(i_lf,i, i)), 'color', [0,0,0,0.2]); plot(f(i_hf), 180/pi*angle(squeeze(freqresp(Kiff(1,1), f(i_hf), 'Hz')).*G_iff_hf(i_hf,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, 1000]; #+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, '/F'], 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 * Encoders fixed to the plates ** Introduction :ignore: