Analysis: effect of DVF on disturbance sensibility

org/optimal_stiffness_control.org

@@ -50,7 +50,6 @@
 #+end_src
 
 ** Initialization
-We initialize all the stages with the default parameters.
 #+begin_src matlab
   initializeGround();
   initializeGranite();
@@ -60,15 +59,11 @@ We initialize all the stages with the default parameters.
   initializeMicroHexapod();
   initializeAxisc();
   initializeMirror();
-#+end_src
 
-#+begin_src matlab
   initializeSimscapeConfiguration();
   initializeDisturbances('enable', false);
   initializeLoggingConfiguration('log', 'none');
-#+end_src
 
-#+begin_src matlab
   initializeController('type', 'hac-dvf');
 #+end_src
 
@@ -121,6 +116,12 @@ The nano-hexapod has the following leg's stiffness and damping.
 #+end_src
 
 ** Controller Design
+The obtain dynamics from actuators forces $\tau_i$ to the relative motion of the legs $d\mathcal{L}_i$ is shown in Figure [[fig:opt_stiff_dvf_plant]] for the three considered payload masses.
+
+The Root Locus is shown in Figure [[fig:opt_stiff_dvf_root_locus]] and wee see that we have unconditional stability.
+
+In order to choose the gain such that we obtain good damping for all the three payload masses, we plot the damping ration of the modes as a function of the gain for all three payload masses in Figure [[fig:opt_stiff_dvf_damping_gain]].
+
 #+begin_src matlab :exports none
   freqs = logspace(-1, 3, 1000);
 
@@ -151,11 +152,19 @@ The nano-hexapod has the following leg's stiffness and damping.
   linkaxes([ax1,ax2],'x');
 #+end_src
 
-Root Locus
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/opt_stiff_dvf_plant.pdf', 'width', 'full', 'height', 'full')
+#+end_src
+
+#+name: fig:opt_stiff_dvf_plant
+#+caption: Dynamics for the Direct Velocity Feedback active damping for three payload masses
+#+RESULTS:
+[[file:figs/opt_stiff_dvf_plant.png]]
+
 #+begin_src matlab :exports none :post
   figure;
 
-  gains = logspace(1, 4, 300);
+  gains = logspace(2, 5, 300);
 
   hold on;
   for i = 1:length(Ms)
@@ -181,13 +190,13 @@ Root Locus
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/opt_stdvf_dvf_root_locus.pdf', 'width', 'wide', 'height', 'tall');
+  exportFig('figs/opt_stiff_dvf_root_locus.pdf', 'width', 'wide', 'height', 'tall');
 #+end_src
 
-#+name: fig:opt_stdvf_dvf_root_locus
-#+caption: Root Locus for the
+#+name: fig:opt_stiff_dvf_root_locus
+#+caption: Root Locus for the DVF controll for three payload masses
 #+RESULTS:
-[[file:figs/opt_stdvf_dvf_root_locus.png]]
+[[file:figs/opt_stiff_dvf_root_locus.png]]
 
 Damping as function of the gain
 #+begin_src matlab :exports none
@@ -218,22 +227,35 @@ Damping as function of the gain
   ylim([0, 1]);
 #+end_src
 
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/opt_stiff_dvf_damping_gain.pdf', 'width', 'full', 'height', 'full')
+#+end_src
+
+#+name: fig:opt_stiff_dvf_damping_gain
+#+caption: Damping ratio of the poles as a function of the DVF gain
+#+RESULTS:
+[[file:figs/opt_stiff_dvf_damping_gain.png]]
+
+Finally, we use the following controller for the Decentralized Direct Velocity Feedback:
 #+begin_src matlab
   Kdvf = 5e3*s/(1+s/2/pi/1e3)*eye(6);
 #+end_src
 
-* Identification of the dynamics for the Primary controller
-** Introduction                                                      :ignore:
+** Effect of the Low Authority Control on the Primary Plant
+*** Introduction                                                    :ignore:
 Let's identify the dynamics from actuator forces $\bm{\tau}$ to displacement as measured by the metrology $\bm{\mathcal{X}}$:
 \[ \bm{G}(s) = \frac{\bm{\mathcal{X}}}{\bm{\tau}} \]
+We do so both when the DVF is applied and when it is not applied.
 
-Then, we compute both the transfer function from forces applied by the actuators $\bm{\mathcal{F}}$ to the measured position error in the frame of the nano-hexapod $\bm{\epsilon}_{\mathcal{X}_n}$:
+
+Then, we compute the transfer function from forces applied by the actuators $\bm{\mathcal{F}}$ to the measured position error in the frame of the nano-hexapod $\bm{\epsilon}_{\mathcal{X}_n}$:
 \[ \bm{G}_\mathcal{X}(s) = \frac{\bm{\epsilon}_{\mathcal{X}_n}}{\bm{\mathcal{F}}} = \bm{G}(s) \bm{J}^{-T} \]
+The obtained dynamics is shown in Figure [[fig:opt_stiff_primary_plant_damped_X]].
 
-and from actuator forces $\bm{\tau}$ to position error of each leg $\bm{\epsilon}_\mathcal{L}$:
+
+And we compute the transfer function from actuator forces $\bm{\tau}$ to position error of each leg $\bm{\epsilon}_\mathcal{L}$:
 \[ \bm{G}_\mathcal{L} = \frac{\bm{\epsilon}_\mathcal{L}}{\bm{\tau}} = \bm{J} \bm{G}(s) \]
-
-We identify these dynamics with and without using the DVF controller.
+The obtained dynamics is shown in Figure [[fig:opt_stiff_primary_plant_damped_L]].
 
 #+begin_src matlab :exports none
   %% Name of the Simulink File
@@ -249,7 +271,7 @@ We identify these dynamics with and without using the DVF controller.
   load('mat/stages.mat', 'nano_hexapod');
 #+end_src
 
-** Identification of the undamped plant                              :ignore:
+*** Identification of the undamped plant                            :ignore:
 #+begin_src matlab :exports none
   Kdvf_backup = Kdvf;
   Kdvf = tf(zeros(6));
@@ -284,7 +306,7 @@ We identify these dynamics with and without using the DVF controller.
   Kdvf = Kdvf_backup;
 #+end_src
 
-** Identification of the damped plant                                :ignore:
+*** Identification of the damped plant                              :ignore:
 #+begin_src matlab :exports none
   Gm_x = {zeros(length(Ms), 1)};
   Gm_l = {zeros(length(Ms), 1)};
@@ -310,7 +332,7 @@ We identify these dynamics with and without using the DVF controller.
   end
 #+end_src
 
-** Obtained dynamics for the Undamped plant                          :ignore:
+*** Effect of the Damping on the plant diagonal dynamics            :ignore:
 #+begin_src matlab :exports none
   freqs = logspace(0, 3, 5000);
 
@@ -323,6 +345,10 @@ We identify these dynamics with and without using the DVF controller.
       plot(freqs, abs(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz'))));
       set(gca,'ColorOrderIndex',i);
       plot(freqs, abs(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz'))));
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(1, 1), freqs, 'Hz'))), '--');
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(2, 2), freqs, 'Hz'))), '--');
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -334,6 +360,8 @@ We identify these dynamics with and without using the DVF controller.
   for i = 1:length(Ms)
       set(gca,'ColorOrderIndex',i);
       plot(freqs, abs(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz'))));
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(3, 3), freqs, 'Hz'))), '--');
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -347,6 +375,10 @@ We identify these dynamics with and without using the DVF controller.
       plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz')))));
       set(gca,'ColorOrderIndex',i);
       plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz')))));
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(1, 1), freqs, 'Hz')))), '--');
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(2, 2), freqs, 'Hz')))), '--');
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
@@ -360,6 +392,9 @@ We identify these dynamics with and without using the DVF controller.
       set(gca,'ColorOrderIndex',i);
       plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz')))), ...
            'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(3, 3), freqs, 'Hz')))), '--', ...
+           'HandleVisibility', 'off');
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
@@ -371,6 +406,15 @@ We identify these dynamics with and without using the DVF controller.
   linkaxes([ax1,ax2,ax3,ax4],'x');
 #+end_src
 
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/opt_stiff_primary_plant_damped_X.pdf', 'width', 'full', 'height', 'full')
+#+end_src
+
+#+name: fig:opt_stiff_primary_plant_damped_X
+#+caption: Primary plant in the task space with (dashed) and without (solid) Direct Velocity Feedback
+#+RESULTS:
+[[file:figs/opt_stiff_primary_plant_damped_X.png]]
+
 #+begin_src matlab :exports none
   freqs = logspace(0, 3, 5000);
 
@@ -381,6 +425,8 @@ We identify these dynamics with and without using the DVF controller.
   for i = 1:length(Ms)
       set(gca,'ColorOrderIndex',i);
       plot(freqs, abs(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz'))));
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz'))), '--');
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -392,6 +438,9 @@ We identify these dynamics with and without using the DVF controller.
       set(gca,'ColorOrderIndex',i);
       plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz')))), ...
            'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz')))), '--', ...
+           'HandleVisibility', 'off');
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
@@ -403,6 +452,226 @@ We identify these dynamics with and without using the DVF controller.
   linkaxes([ax1,ax2],'x');
 #+end_src
 
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/opt_stiff_primary_plant_damped_L.pdf', 'width', 'full', 'height', 'full')
+#+end_src
+
+#+name: fig:opt_stiff_primary_plant_damped_L
+#+caption: Primary plant in the space of the legs with (dashed) and without (solid) Direct Velocity Feedback
+#+RESULTS:
+[[file:figs/opt_stiff_primary_plant_damped_L.png]]
+
+*** Effect of the Damping on the coupling dynamics                  :ignore:
+The coupling (off diagonal elements) of $\bm{G}_\mathcal{X}$ are shown in Figure [[fig:opt_stiff_primary_plant_damped_coupling_X]] both when DVF is applied and when it is not.
+
+The coupling does not change a lot with DVF.
+
+
+The coupling in the space of the legs $\bm{G}_\mathcal{L}$ are shown in Figure [[fig:opt_stiff_primary_plant_damped_coupling_L]].
+The magnitude of the coupling around the resonance of the nano-hexapod (where the coupling is the highest) is considerably reduced when DVF is applied.
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+  hold on;
+  for i = 1:5
+    for j = i+1:6
+      plot(freqs, abs(squeeze(freqresp(G_x{1}(i, j), freqs, 'Hz'))),        'color', [0, 0, 0, 0.2]);
+      plot(freqs, abs(squeeze(freqresp(Gm_x{1}(i, j), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 0.2]);
+    end
+  end
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(G_x{1}(1, 1), freqs, 'Hz'))));
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(Gm_x{1}(1, 1), freqs, 'Hz'))), '--');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  ylim([1e-12, inf]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/opt_stiff_primary_plant_damped_coupling_X.pdf', 'width', 'full', 'height', 'tall')
+#+end_src
+
+#+name: fig:opt_stiff_primary_plant_damped_coupling_X
+#+caption: Coupling in the primary plant in the task with (dashed) and without (solid) Direct Velocity Feedback
+#+RESULTS:
+[[file:figs/opt_stiff_primary_plant_damped_coupling_X.png]]
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+  hold on;
+  for i = 1:5
+    for j = i+1:6
+      plot(freqs, abs(squeeze(freqresp(G_l{1}(i, j), freqs, 'Hz'))),        'color', [0, 0, 0, 0.2]);
+      plot(freqs, abs(squeeze(freqresp(Gm_l{1}(i, j), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 0.2]);
+    end
+  end
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(G_l{1}(1, 1), freqs, 'Hz'))));
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(Gm_l{1}(1, 1), freqs, 'Hz'))), '--');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  ylim([1e-9, inf]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/opt_stiff_primary_plant_damped_coupling_L.pdf', 'width', 'full', 'height', 'tall')
+#+end_src
+
+#+name: fig:opt_stiff_primary_plant_damped_coupling_L
+#+caption: Coupling in the primary plant in the space of the legs with (dashed) and without (solid) Direct Velocity Feedback
+#+RESULTS:
+[[file:figs/opt_stiff_primary_plant_damped_coupling_L.png]]
+
+** Effect of the Low Authority Control on the Sensibility to Disturbances
+*** Introduction                                                    :ignore:
+We may now see how Decentralized Direct Velocity Feedback changes the sensibility to disturbances, namely:
+- Ground motion
+- Spindle and Translation stage vibrations
+- Direct forces applied to the sample
+
+To simplify the analysis, we here only consider the vertical direction, thus, we will look at the transfer functions:
+- from vertical ground motion $D_{w,z}$ to the vertical position error of the sample $E_z$
+- from vertical vibration forces of the spindle $F_{R_z,z}$ to $E_z$
+- from vertical vibration forces of the translation stage $F_{T_y,z}$ to $E_z$
+- from vertical direct forces (such as cable forces) $F_{d,z}$ to $E_z$
+
+The norm of these transfer functions are shown in Figure [[fig:opt_stiff_sensibility_dist_dvf]].
+
+*** Identification                                                  :ignore:
+#+begin_src matlab :exports none
+  %% Name of the Simulink File
+  mdl = 'nass_model';
+
+  %% Micro-Hexapod
+  clear io; io_i = 1;
+  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz');   io_i = io_i + 1; % Z Ground motion
+  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fty_z'); io_i = io_i + 1; % Parasitic force Ty - Z
+  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Frz_z'); io_i = io_i + 1; % Parasitic force Rz - Z
+  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fd');    io_i = io_i + 1; % Direct forces
+
+  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
+#+end_src
+
+#+begin_src matlab :exports none
+  Kdvf_backup = Kdvf;
+  Kdvf = tf(zeros(6));
+#+end_src
+
+#+begin_src matlab :exports none
+  Gd = {zeros(length(Ms), 1)};
+
+  for i = 1:length(Ms)
+      initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
+      initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
+
+      %% Run the linearization
+      G = linearize(mdl, io);
+      G.InputName  = {'Dwz', 'Fty_z', 'Frz_z', 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
+      G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
+
+      Gd(i) = {G};
+  end
+#+end_src
+
+#+begin_src matlab :exports none
+  Kdvf = Kdvf_backup;
+#+end_src
+
+#+begin_src matlab :exports none
+  Gd_dvf = {zeros(length(Ms), 1)};
+
+  for i = 1:length(Ms)
+      initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
+      initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
+
+      %% Run the linearization
+      G = linearize(mdl, io);
+      G.InputName  = {'Dwz', 'Fty_z', 'Frz_z', 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
+      G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
+
+      Gd_dvf(i) = {G};
+  end
+#+end_src
+
+*** Results                                                         :ignore:
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 5000);
+
+  figure;
+
+  subplot(2, 2, 1);
+  title('$D_{w,z}$ to $E_z$');
+  hold on;
+  for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Dwz'), freqs, 'Hz'))), ...
+           'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gd_dvf{i}('Ez', 'Dwz'), freqs, 'Hz'))), '--', ...
+           'HandleVisibility', 'off');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/m]'); set(gca, 'XTickLabel',[]);
+  legend('location', 'southeast');
+
+  subplot(2, 2, 2);
+  title('$F_{dz}$ to $E_z$');
+  hold on;
+  for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fdz'), freqs, 'Hz'))));
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gd_dvf{i}('Ez', 'Fdz'), freqs, 'Hz'))), '--');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  subplot(2, 2, 3);
+  title('$F_{T_y,z}$ to $E_z$');
+  hold on;
+  for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fty_z'), freqs, 'Hz'))));
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gd_dvf{i}('Ez', 'Fty_z'), freqs, 'Hz'))), '--');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  subplot(2, 2, 4);
+  title('$F_{R_z,z}$ to $E_z$');
+  hold on;
+  for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Frz_z'), freqs, 'Hz'))));
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gd_dvf{i}('Ez', 'Frz_z'), freqs, 'Hz'))), '--');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/opt_stiff_sensibility_dist_dvf.pdf', 'width', 'full', 'height', 'full')
+#+end_src
+
+#+name: fig:opt_stiff_sensibility_dist_dvf
+#+caption: Norm of the transfer function from vertical disturbances to vertical position error with (dashed) and without (solid) Direct Velocity Feedback applied
+#+RESULTS:
+[[file:figs/opt_stiff_sensibility_dist_dvf.png]]
+
 * Primary Control in the task space
 ** Introduction                                                      :ignore:
 
@@ -690,6 +959,7 @@ Let's look $\bm{G}_\mathcal{X}(s)$.
 ** Simulation
 
 * Primary Control in the leg space
+** Introduction                                                      :ignore:
 ** Plant in the task space
 #+begin_src matlab :exports none
   freqs = logspace(0, 3, 1000);