Add analysis about compliance

org/control_voice_coil.org

@@ -980,6 +980,113 @@ And we simulate the system.
 [[file:figs/exp_tomography_voice_coil_time_domain.png]]
 
 
+** Compliance of the nano-hexapod
+*** Identification
+Let's identify the Compliance of the NASS:
+#+begin_src matlab
+  %% Name of the Simulink File
+  mdl = 'nass_model';
+
+  %% Input/Output definition
+  clear io; io_i = 1;
+  io(io_i) = linio([mdl, '/Disturbances/Fd'], 1, 'openinput');       io_i = io_i + 1; % Direct Forces/Torques applied on the sample
+  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
+#+end_src
+
+First in open-loop:
+#+begin_src matlab
+  Kp = tf(zeros(6));
+  Kl = tf(zeros(6));
+  Kiff = tf(zeros(6));
+#+end_src
+
+#+begin_src matlab
+  %% Run the linearization
+  Gc_ol = linearize(mdl, io, 0);
+  Gc_ol.InputName  = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
+  Gc_ol.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
+#+end_src
+
+Then with the IFF control.
+#+begin_src matlab
+  load('mat/hac_lac_cascade_vc_controllers.mat', 'Kiff')
+#+end_src
+
+#+begin_src matlab
+  %% Run the linearization
+  Gc_iff = linearize(mdl, io, 0);
+  Gc_iff.InputName  = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
+  Gc_iff.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
+#+end_src
+
+With the HAC control added
+#+begin_src matlab
+  load('mat/hac_lac_cascade_vc_controllers.mat', 'Kl')
+#+end_src
+
+#+begin_src matlab
+  %% Run the linearization
+  Gc_hac = linearize(mdl, io, 0);
+  Gc_hac.InputName  = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
+  Gc_hac.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
+#+end_src
+
+Finally with the primary controller
+#+begin_src matlab
+  load('mat/hac_lac_cascade_vc_controllers.mat', 'Kp')
+#+end_src
+
+#+begin_src matlab
+  %% Run the linearization
+  Gc_pri = linearize(mdl, io, 0);
+  Gc_pri.InputName  = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
+  Gc_pri.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
+#+end_src
+
+*** Obtained Compliance
+#+begin_src matlab :exports none
+  labels = {'$\epsilon_x/F_{xd}$', '$\epsilon_y/F_{yd}$', '$\epsilon_z/F_{zd}$', '$\epsilon_{R_x}/M_{xd}$', '$\epsilon_{R_y}/M_{yd}$', '$\epsilon_{R_z}/M_{zd}$'};
+
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(1, 2, 1);
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(Gc_ol( 1, 1), freqs, 'Hz'))), 'DisplayName', 'OL');
+  plot(freqs, abs(squeeze(freqresp(Gc_iff(1, 1), freqs, 'Hz'))), 'DisplayName', 'IFF');
+  plot(freqs, abs(squeeze(freqresp(Gc_hac(1, 1), freqs, 'Hz'))), 'DisplayName', 'HAC');
+  plot(freqs, abs(squeeze(freqresp(Gc_pri(1, 1), freqs, 'Hz'))), 'DisplayName', 'PRI');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Compliance [m/N]');
+  xlabel('Frequency [Hz]');
+  legend('location', 'northeast');
+
+  ax2 = subplot(1, 2, 2);
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(Gc_ol( 4, 4), freqs, 'Hz'))), 'DisplayName', 'OL');
+  plot(freqs, abs(squeeze(freqresp(Gc_iff(4, 4), freqs, 'Hz'))), 'DisplayName', 'IFF');
+  plot(freqs, abs(squeeze(freqresp(Gc_hac(4, 4), freqs, 'Hz'))), 'DisplayName', 'HAC');
+  plot(freqs, abs(squeeze(freqresp(Gc_pri(4, 4), freqs, 'Hz'))), 'DisplayName', 'PRI');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Compliance [$\frac{rad}{Nm}$]');
+  xlabel('Frequency [Hz]');
+  legend('location', 'northeast');
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+header: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/compliance_evolution_vc_cascade_control.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+name: fig:compliance_evolution_vc_cascade_control
+#+caption: Evolution of the NASS compliance with each control loop added ([[./figs/compliance_evolution_vc_cascade_control.png][png]], [[./figs/compliance_evolution_vc_cascade_control.pdf][pdf]])
+[[file:figs/compliance_evolution_vc_cascade_control.png]]
+
 ** Robustness to Payload Variability
 *** Initialization
 Let's change the payload mass, and see if the controller design for a payload mass of 1 still gives good performance.