Compare IFF/DVF plant with Simscape model

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "matlab/nass-simscape"]
+	path = matlab/nass-simscape
+	url = https://git.tdehaeze.xyz/tdehaeze/nass-simscape

matlab/nass-simscape

@@ -0,0 +1 @@
+Subproject commit ef7d7a54d507b329d9bf23f1a3495ed7d5618a2d

test-bench-nano-hexapod.org

@@ -46,7 +46,11 @@
   <hr>
 #+end_export
 
+\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]]
@@ -66,6 +70,73 @@ Here are the documentation of the equipment used for this test bench:
 #+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.
@@ -90,7 +161,8 @@ addpath('./mat/');
 addpath('./src/');
 #+end_src
 
-** Load Data
+** Identification of the dynamics
+*** Load Data
 #+begin_src matlab
 meas_data_lf = {};
 
@@ -100,7 +172,7 @@ for i = 1:6
 end
 #+end_src
 
-** Spectral Analysis - Setup
+*** 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);
@@ -122,7 +194,7 @@ i_lf = f < 250; % Points for low frequency excitation
 i_hf = f > 250; % Points for high frequency excitation
 #+end_src
 
-** DVF Plant
+*** 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
@@ -244,7 +316,7 @@ exportFig('figs/enc_struts_dvf_frf.pdf', 'width', 'wide', 'height', 'tall');
 [[file:figs/enc_struts_dvf_frf.png]]
 
 
-** IFF Plant
+*** 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
@@ -365,11 +437,11 @@ exportFig('figs/enc_struts_iff_frf.pdf', 'width', 'wide', 'height', 'tall');
 #+RESULTS:
 [[file:figs/enc_struts_iff_frf.png]]
 
-** Jacobian
+** 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:nano_hexapod_decentralized_schematic]] with:
+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
@@ -540,3 +612,337 @@ exportFig('figs/enc_struts_iff_cart_frf.pdf', 'width', 'wide', 'height', 'tall')
 #+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
+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
+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
+mdl = 'nano_hexapod_simscape';
+
+options = linearizeOptions;
+options.SampleTime = 0;
+
+open(mdl)
+#+end_src
+
+*** Dynamics from Actuator to Force Sensors
+#+begin_src matlab
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '3dof', ...
+                                       'flex_top_type', '2dof', ...
+                                       'motion_sensor_type', 'struts', ...
+                                       'actuator_type', '2dof');
+#+end_src
+
+#+begin_src matlab
+%% Input/Output definition
+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 = 20*exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+#+end_src
+
+#+begin_src matlab :exports none
+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(Giff(1,1), freqs, 'Hz'))), '-', ...
+     'DisplayName', '$\tau_{m,i}/u_i$ - Model')
+plot(f(i_lf), abs(G_iff_lf(i_lf,1, 1)), ...
+     'DisplayName', '$\tau_{m,i}/u_i$ - FRF')
+for i = 2:6
+    set(gca,'ColorOrderIndex',1);
+    plot(freqs, abs(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-', ...
+         'HandleVisibility', 'off');
+end
+for i = 2:6
+    set(gca,'ColorOrderIndex',2)
+    plot(f(i_lf), abs(G_iff_lf(i_lf,i, i)), ...
+         'HandleVisibility', 'off');
+    set(gca,'ColorOrderIndex',2)
+    plot(f(i_hf), abs(G_iff_hf(i_hf,i, i)), ...
+         '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
+    set(gca,'ColorOrderIndex',1);
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-');
+end
+for i = 1:6
+    set(gca,'ColorOrderIndex',2)
+    plot(f(i_lf), 180/pi*angle(G_iff_lf(i_lf,i, i)));
+    set(gca,'ColorOrderIndex',2)
+    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');
+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
+freqs = 2*logspace(1, 3, 1000);
+
+figure;
+hold on;
+% Off diagonal terms
+set(gca,'ColorOrderIndex',1);
+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',1);
+        plot(freqs, abs(squeeze(freqresp(Giff(i, j), freqs, 'Hz'))), ...
+             'HandleVisibility', 'off');
+    end
+end
+set(gca,'ColorOrderIndex',2);
+plot(f(i_lf), abs(G_iff_lf(i_lf, 1, 2)), ...
+     'DisplayName', '$\tau_{m,i}/u_j$ - FRF')
+for i = 1:5
+    for j = i+1:6
+        set(gca,'ColorOrderIndex',2);
+        plot(f(i_lf), abs(G_iff_lf(i_lf, i, j)), ...
+             'HandleVisibility', 'off');
+        set(gca,'ColorOrderIndex',2);
+        plot(f(i_hf), abs(G_iff_hf(i_hf, i, j)), ...
+             '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
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '3dof', ...
+                                       'flex_top_type', '2dof', ...
+                                       'motion_sensor_type', 'struts', ...
+                                       'actuator_type', '2dof');
+#+end_src
+
+#+begin_src matlab
+%% Input/Output definition
+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 = 20*exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+#+end_src
+
+#+begin_src matlab :exports none
+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(1,1), freqs, 'Hz'))), '-', ...
+     'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - Model')
+plot(f(i_lf), abs(G_dvf_lf(i_lf,1, 1)), ...
+     'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - FRF')
+for i = 2:6
+    set(gca,'ColorOrderIndex',1);
+    plot(freqs, abs(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-', ...
+         'HandleVisibility', 'off');
+end
+for i = 2:6
+    set(gca,'ColorOrderIndex',2)
+    plot(f(i_lf), abs(G_dvf_lf(i_lf,i, i)), ...
+         'HandleVisibility', 'off');
+    set(gca,'ColorOrderIndex',2)
+    plot(f(i_hf), abs(G_dvf_hf(i_hf,i, i)), ...
+         'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+legend('location', 'northeast');
+ylim([1e-8, 1e-3]);
+
+ax2 = nexttile;
+hold on;
+for i = 1:6
+    set(gca,'ColorOrderIndex',1);
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-');
+end
+for i = 1:6
+    set(gca,'ColorOrderIndex',2)
+    plot(f(i_lf), 180/pi*angle(G_dvf_lf(i_lf,i, i)));
+    set(gca,'ColorOrderIndex',2)
+    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');
+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
+freqs = 2*logspace(1, 3, 1000);
+
+figure;
+hold on;
+% Off diagonal terms
+set(gca,'ColorOrderIndex',1);
+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',1);
+        plot(freqs, abs(squeeze(freqresp(Gdvf(i, j), freqs, 'Hz'))), ...
+             'HandleVisibility', 'off');
+    end
+end
+set(gca,'ColorOrderIndex',2);
+plot(f(i_lf), abs(G_dvf_lf(i_lf, 1, 2)), ...
+     'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - FRF')
+for i = 1:5
+    for j = i+1:6
+        set(gca,'ColorOrderIndex',2);
+        plot(f(i_lf), abs(G_dvf_lf(i_lf, i, j)), ...
+             'HandleVisibility', 'off');
+        set(gca,'ColorOrderIndex',2);
+        plot(f(i_hf), abs(G_dvf_hf(i_hf, i, j)), ...
+             '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]]
+
+** TODO Integral Force Feedback
+*** Plant
+#+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
+
+*** Root Locus
+
+*** Gains
+
+*** Experimental Results