nass-simscape/org/optimal_stiffness_control.org

#+TITLE: Control of the NASS with optimal stiffness
#+SETUPFILE: ./setup/org-setup-file.org

* Introduction                                                        :ignore:
* Low Authority Control - Decentralized Direct Velocity Feedback
<<sec:lac_dvf>>
** Introduction                                                      :ignore:

#+name: fig:control_architecture_dvf
#+caption: Low Authority Control: Decentralized Direct Velocity Feedback
#+RESULTS:
[[file:figs/control_architecture_dvf.png]]

** 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
  simulinkproject('../');
#+end_src

#+begin_src matlab
  load('mat/conf_simulink.mat');
  open('nass_model.slx')
#+end_src

** Initialization
#+begin_src matlab
  initializeGround();
  initializeGranite();
  initializeTy();
  initializeRy();
  initializeRz();
  initializeMicroHexapod();
  initializeAxisc();
  initializeMirror();

  initializeSimscapeConfiguration();
  initializeDisturbances('enable', false);
  initializeLoggingConfiguration('log', 'none');

  initializeController('type', 'hac-dvf');
#+end_src

We set the stiffness of the payload fixation:
#+begin_src matlab
  Kp = 1e8; % [N/m]
#+end_src

** Identification
#+begin_src matlab
  K = tf(zeros(6));
  Kdvf = tf(zeros(6));
#+end_src

We identify the system for the following payload masses:
#+begin_src matlab
  Ms = [1, 10, 50];
#+end_src

#+begin_src matlab :exports none
  Gm_dvf = {zeros(length(Ms), 1)};
#+end_src

The nano-hexapod has the following leg's stiffness and damping.
#+begin_src matlab
  initializeNanoHexapod('k', 1e5, 'c', 2e2);
#+end_src

#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'nass_model';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Controller'],    1, 'openinput');               io_i = io_i + 1; % Actuator Inputs
  io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm');  io_i = io_i + 1; % Force Sensors
#+end_src

#+begin_src matlab :exports none
  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_dvf = linearize(mdl, io);
    G_dvf.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
    G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
    Gm_dvf(i) = {G_dvf};
  end
#+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);

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ms)
    plot(freqs, abs(squeeze(freqresp(Gm_dvf{i}(1, 1), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:length(Ms)
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_dvf{i}(1, 1), freqs, 'Hz')))), ...
         'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);
  legend('location', 'northeast');

  linkaxes([ax1,ax2],'x');
#+end_src

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

  gains = logspace(2, 5, 300);

  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(real(pole(Gm_dvf{i})),  imag(pole(Gm_dvf{i})), 'x', ...
           'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
      set(gca,'ColorOrderIndex',i);
      plot(real(tzero(Gm_dvf{i})),  imag(tzero(Gm_dvf{i})), 'o', ...
           'HandleVisibility', 'off');
      for k = 1:length(gains)
          set(gca,'ColorOrderIndex',i);
          cl_poles = pole(feedback(Gm_dvf{i}, (gains(k)*s)*eye(6)));
          plot(real(cl_poles), imag(cl_poles), '.', ...
               'HandleVisibility', 'off');
      end
  end
  hold off;
  axis square;
  xlim([-140, 10]); ylim([0, 150]);

  xlabel('Real Part'); ylabel('Imaginary Part');
  legend('location', 'northwest');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/opt_stiff_dvf_root_locus.pdf', 'width', 'wide', 'height', 'tall');
#+end_src

#+name: fig:opt_stiff_dvf_root_locus
#+caption: Root Locus for the DVF control for three payload masses
#+RESULTS:
[[file:figs/opt_stiff_dvf_root_locus.png]]

Damping as function of the gain
#+begin_src matlab :exports none
  c1 = [     0    0.4470    0.7410]; % Blue
  c2 = [0.8500    0.3250    0.0980]; % Orange
  c3 = [0.9290    0.6940    0.1250]; % Yellow
  c4 = [0.4940    0.1840    0.5560]; % Purple
  c5 = [0.4660    0.6740    0.1880]; % Green
  c6 = [0.3010    0.7450    0.9330]; % Light Blue
  c7 = [0.6350    0.0780    0.1840]; % Red
  colors = [c1; c2; c3; c4; c5; c6; c7];

  figure;

  gains = logspace(1, 4, 100);

  hold on;
  for i = 1:length(Ms)
      for k = 1:length(gains)
          cl_poles = pole(feedback(Gm_dvf{i}, (gains(k)*s)*eye(6)));
          set(gca,'ColorOrderIndex',i);
          plot(gains(k), sin(-pi/2 + angle(cl_poles)), '.', 'color', colors(i, :));
      end
  end
  hold off;
  xlabel('DVF Gain'); ylabel('Modal Damping');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  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

** 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 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]].

#+begin_important
  A zero with a positive real part is introduced in the transfer function from $\mathcal{F}_y$ to $\mathcal{X}_y$ after Decentralized Direct Velocity Feedback is applied.
#+end_important

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) \]
The obtained dynamics is shown in Figure [[fig:opt_stiff_primary_plant_damped_L]].

#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'nass_model';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Controller'],     1, 'input');            io_i = io_i + 1; % Actuator Inputs
  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
#+end_src

#+begin_src matlab :exports none
  load('mat/stages.mat', 'nano_hexapod');
#+end_src

*** Identification of the undamped plant                            :ignore:
#+begin_src matlab :exports none
  Kdvf_backup = Kdvf;
  Kdvf = tf(zeros(6));
#+end_src

#+begin_src matlab :exports none
  G_x = {zeros(length(Ms), 1)};
  G_l = {zeros(length(Ms), 1)};
#+end_src

#+begin_src matlab :exports none
  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  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
      G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

      Gx = -G*inv(nano_hexapod.kinematics.J');
      Gx.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
      G_x(i) = {Gx};

      Gl = -nano_hexapod.kinematics.J*G;
      Gl.OutputName  = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
      G_l(i) = {Gl};
  end
#+end_src

#+begin_src matlab :exports none
  Kdvf = Kdvf_backup;
#+end_src

*** Identification of the damped plant                              :ignore:
#+begin_src matlab :exports none
  Gm_x = {zeros(length(Ms), 1)};
  Gm_l = {zeros(length(Ms), 1)};
#+end_src

#+begin_src matlab :exports none
  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  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
      G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

      Gx = -G*inv(nano_hexapod.kinematics.J');
      Gx.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
      Gm_x(i) = {Gx};

      Gl = -nano_hexapod.kinematics.J*G;
      Gl.OutputName  = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
      Gm_l(i) = {Gl};
  end
#+end_src

*** Effect of the Damping on the plant diagonal dynamics            :ignore:
#+begin_src matlab :exports none
  freqs = logspace(0, 3, 5000);

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      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');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('$\mathcal{X}_x/\mathcal{F}_x$, $\mathcal{X}_y/\mathcal{F}_y$')

  ax2 = subplot(2, 2, 2);
  hold on;
  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');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('$\mathcal{X}_z/\mathcal{F}_z$')

  ax3 = subplot(2, 2, 3);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      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');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);

  ax4 = subplot(2, 2, 4);
  hold on;
  for i = 1:length(Ms)
      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');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);
  legend('location', 'southwest');

  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);

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  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');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:length(Ms)
      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');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);
  legend('location', 'southwest');

  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]].

#+begin_important
The magnitude of the coupling between $\tau_i$ and $d\mathcal{L}_j$ (Figure [[fig:opt_stiff_primary_plant_damped_coupling_L]]) around the resonance of the nano-hexapod (where the coupling is the highest) is considerably reduced when DVF is applied.
#+end_important

#+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');
  set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/N]');

  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');
  xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');

  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');
  xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
#+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]]

*** Conclusion                                                      :ignore:
#+begin_important
  Decentralized Direct Velocity Feedback is shown to increase the effect of stages vibrations at high frequency and to reduce the effect of ground motion and direct forces at low frequency.
#+end_important

** Conclusion
#+begin_important

#+end_important

* Primary Control in the leg space
<<sec:primary_control_L>>
** Introduction                                                      :ignore:
In this section we implement the control architecture shown in Figure [[fig:control_architecture_hac_dvf_pos_L]] consisting of:
- an inner loop with a decentralized direct velocity feedback control
- an outer loop where the controller $\bm{K}_\mathcal{L}$ is designed in the frame of the legs

#+name: fig:control_architecture_hac_dvf_pos_L
#+caption: Cascade Control Architecture. The inner loop consist of a decentralized Direct Velocity Feedback. The outer loop consist of position control in the leg's space
[[file:figs/control_architecture_hac_dvf_pos_L.png]]

The controller for decentralized direct velocity feedback is the one designed in Section [[sec:lac_dvf]].

** Plant in the leg space
We now look at the transfer function matrix from $\bm{\tau}^\prime$ to $\bm{\epsilon}_{\mathcal{X}_n}$ for the design of $\bm{K}_\mathcal{L}$.

The diagonal elements of the transfer function matrix from $\bm{\tau}^\prime$ to $\bm{\epsilon}_{\mathcal{X}_n}$ for the three considered masses are shown in Figure [[fig:opt_stiff_primary_plant_L]].

The plant dynamics below $100\ [Hz]$ is only slightly dependent on the payload mass.

#+begin_src matlab :exports none
  freqs = logspace(0, 3, 1000);

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ms)
      for j = 1:6
          set(gca,'ColorOrderIndex',i);
          plot(freqs, abs(squeeze(freqresp(Gm_l{i}(j, j), freqs, 'Hz'))));
      end
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Diagonal elements of the Plant');

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:length(Ms)
      for j = 1:6
          set(gca,'ColorOrderIndex',i);
          plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_l{i}(j, j), freqs, 'Hz')))));
      end
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);

  linkaxes([ax1,ax2],'x');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_primary_plant_L.pdf', 'width', 'full', 'height', 'full')
#+end_src

#+name: fig:opt_stiff_primary_plant_L
#+caption: Diagonal elements of the transfer function matrix from $\bm{\tau}^\prime$ to $\bm{\epsilon}_{\mathcal{X}_n}$ for the three considered masses
#+RESULTS:
[[file:figs/opt_stiff_primary_plant_L.png]]


#+begin_src matlab :exports none
  c1 = [     0    0.4470    0.7410 0.2]; % Blue
  c2 = [0.8500    0.3250    0.0980 0.2]; % Orange
  c3 = [0.9290    0.6940    0.1250 0.2]; % Yellow
  c4 = [0.4940    0.1840    0.5560 0.2]; % Purple
  c5 = [0.4660    0.6740    0.1880 0.2]; % Green
  c6 = [0.3010    0.7450    0.9330 0.2]; % Light Blue
  c7 = [0.6350    0.0780    0.1840 0.2]; % Red
  colors = [c1; c2; c3; c4; c5; c6; c7];

  freqs = logspace(0, 3, 1000);

  figure;

  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz'))), '-');
      for j = 1:5
          for k = j+1:6
              plot(freqs, abs(squeeze(freqresp(Gm_l{i}(j, k), freqs, 'Hz'))), '--', 'color', colors(i, :));
          end
      end
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
  ylim([1e-9, inf]);
#+end_src

** Control in the leg space
We design a diagonal controller with all the same diagonal elements.

The requirements for the controller are:
- Crossover frequency of around 100Hz
- Stable for all the considered payload masses
- Sufficient phase and gain margin
- Integral action at low frequency

The design controller is as follows:
- Lead centered around the crossover
- An integrator below 10Hz
- A low pass filter at 250Hz

The loop gain is shown in Figure [[fig:opt_stiff_primary_loop_gain_L]].

#+begin_src matlab
  h = 2.0;
  Kl = 2e7 * eye(6) * ...
       1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
       1/h*(s/(2*pi*200/h) + 1)/(s/(2*pi*200*h) + 1) * ...
       (s/2/pi/10 + 1)/(s/2/pi/10) * ...
       1/(1 + s/2/pi/300);
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(0, 3, 1000);

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ms)
      for j = 1:6
          set(gca,'ColorOrderIndex',i);
          plot(freqs, abs(squeeze(freqresp(Gm_l{i}(j, j)*Kl(j,j), freqs, 'Hz'))));
      end
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:length(Ms)
      for j = 1:6
          set(gca,'ColorOrderIndex',i);
          plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_l{i}(j, j)*Kl(j,j), freqs, 'Hz'))));
      end
  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');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_primary_loop_gain_L.pdf', 'width', 'full', 'height', 'full')
#+end_src

#+name: fig:opt_stiff_primary_loop_gain_L
#+caption: Loop gain for the primary plant
#+RESULTS:
[[file:figs/opt_stiff_primary_loop_gain_L.png]]

Finally, we include the Jacobian in the control and we ignore the measurement of the vertical rotation as for the real system.
#+begin_src matlab
  load('mat/stages.mat', 'nano_hexapod');
  K = Kl*nano_hexapod.kinematics.J*diag([1, 1, 1, 1, 1, 0]);
#+end_src

#+begin_src matlab :exports none
  for i = 1:length(Ms)
      isstable(feedback(nano_hexapod.kinematics.J\Gm_l{i}*K, eye(6), -1))
  end
#+end_src

** Sensibility to Disturbances and Noise Budget
*** Identification                                                  :ignore:
We identify the transfer function from disturbances to the position error of the sample when the HAC-LAC control is applied.

#+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
  Gd_L = {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_L(i) = {G};
  end
#+end_src

*** Obtained Sensibility to Disturbances                            :ignore:
We compare the norm of these transfer function for the vertical direction when no control is applied and when HAC-LAC control is applied: Figure [[fig:opt_stiff_primary_control_L_senbility_dist]].

#+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_L{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_L{i}('Ez', 'Fdz'), freqs, 'Hz'))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/N]');

  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_L{i}('Ez', 'Fty_z'), freqs, 'Hz'))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');

  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_L{i}('Ez', 'Frz_z'), freqs, 'Hz'))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_primary_control_L_senbility_dist.pdf', 'width', 'full', 'height', 'full')
#+end_src

#+name: fig:opt_stiff_primary_control_L_senbility_dist
#+caption: Sensibility to disturbances when the HAC-LAC control is applied
#+RESULTS:
[[file:figs/opt_stiff_primary_control_L_senbility_dist.png]]

*** Noise Budgeting                                                 :ignore:
Then, we load the Power Spectral Density of the perturbations and we look at the obtained PSD of the displacement error in the vertical direction due to the disturbances:
- Figure [[fig:opt_stiff_primary_control_L_psd_dist]]: Amplitude Spectral Density of the vertical position error due to both the vertical ground motion and the vertical vibrations of the spindle
- Figure [[fig:opt_stiff_primary_control_L_psd_tot]]: Comparison of the Amplitude Spectral Density of the vertical position error in Open Loop and with the HAC-DVF Control
- Figure [[fig:opt_stiff_primary_control_L_cas_tot]]: Comparison of the Cumulative Amplitude Spectrum of the vertical position error in Open Loop and with the HAC-DVF Control

#+begin_src matlab :exports none
  load('./mat/dist_psd.mat', 'dist_f');
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(dist_f.f, sqrt(dist_f.psd_gm).*abs(squeeze(freqresp(Gd_L{1}('Ez', 'Dwz'  ), dist_f.f, 'Hz'))), 'DisplayName', '$D_w$')
  plot(dist_f.f, sqrt(dist_f.psd_rz).*abs(squeeze(freqresp(Gd_L{1}('Ez', 'Frz_z'), dist_f.f, 'Hz'))), 'DisplayName', '$F_{R_z}$')
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('Amplitude Spectral Density [$m/\sqrt{Hz}$]');
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  legend('location', 'southwest');
  xlim([1, 500]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_primary_control_L_psd_dist.pdf', 'width', 'full', 'height', 'tall')
#+end_src

#+name: fig:opt_stiff_primary_control_L_psd_dist
#+caption: Amplitude Spectral Density of the vertical position error of the sample when the HAC-DVF control is applied due to both the ground motion and spindle vibrations
#+RESULTS:
[[file:figs/opt_stiff_primary_control_L_psd_dist.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(dist_f.f, sqrt(dist_f.psd_gm.*abs(squeeze(freqresp(Gd{1}('Ez', 'Dwz'  ), dist_f.f, 'Hz'))).^2 + ...
                      dist_f.psd_rz.*abs(squeeze(freqresp(Gd{1}('Ez', 'Frz_z'), dist_f.f, 'Hz'))).^2), 'DisplayName', 'Open-Loop')
  plot(dist_f.f, sqrt(dist_f.psd_gm.*abs(squeeze(freqresp(Gd_L{1}('Ez', 'Dwz'  ), dist_f.f, 'Hz'))).^2 + ...
                      dist_f.psd_rz.*abs(squeeze(freqresp(Gd_L{1}('Ez', 'Frz_z'), dist_f.f, 'Hz'))).^2), 'DisplayName', 'HAC-DVF')
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('Amplitude Spectral Density [$m/\sqrt{Hz}$]');
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  legend('location', 'northeast');
  xlim([1, 500]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_primary_control_L_psd_tot.pdf', 'width', 'full', 'height', 'tall')
#+end_src

#+name: fig:opt_stiff_primary_control_L_psd_tot
#+caption: Amplitude Spectral Density of the vertical position error of the sample in Open-Loop and when the HAC-DVF control is applied
#+RESULTS:
[[file:figs/opt_stiff_primary_control_L_psd_tot.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(dist_f.f, sqrt(flip(-cumtrapz(flip(dist_f.f), flip(dist_f.psd_gm.*abs(squeeze(freqresp(Gd{i}('Ez', 'Dwz'  ), dist_f.f, 'Hz'))).^2 + ...
                                                        dist_f.psd_rz.*abs(squeeze(freqresp(Gd{i}('Ez', 'Frz_z'), dist_f.f, 'Hz'))).^2)))), ...
           'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
      set(gca,'ColorOrderIndex',i);
      plot(dist_f.f, sqrt(flip(-cumtrapz(flip(dist_f.f), flip(dist_f.psd_gm.*abs(squeeze(freqresp(Gd_L{i}('Ez', 'Dwz'  ), dist_f.f, 'Hz'))).^2 + ...
                                                        dist_f.psd_rz.*abs(squeeze(freqresp(Gd_L{i}('Ez', 'Frz_z'), dist_f.f, 'Hz'))).^2)))), '--', ...
           'HandleVisibility', 'off')
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('Amplitude Spectral Density [$m/\sqrt{Hz}$]');
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  legend('location', 'southwest');
  xlim([0.1, 500]); ylim([1e-12, 1e-6]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'height', 'tall')
#+end_src

#+name: fig:opt_stiff_primary_control_L_cas_tot
#+caption: Cumulative Amplitude Spectrum of the vertical position error of the sample in Open-Loop and when the HAC-DVF control is applied
#+RESULTS:
[[file:figs/opt_stiff_primary_control_L_cas_tot.png]]

** Simulations of Tomography Experiment
Let's now simulate a tomography experiment.
To do so, we include all disturbances except vibrations of the translation stage.
#+begin_src matlab
  initializeDisturbances();
  initializeSimscapeConfiguration('gravity', false);
  initializeLoggingConfiguration('log', 'all');
#+end_src

#+begin_src matlab :exports none
  load('mat/conf_simulink.mat');
  set_param(conf_simulink, 'StopTime', '2');
#+end_src

And we run the simulation for all three payload Masses.
#+begin_src matlab :exports none
  hac_dvf_L = {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', 'Rz_period', Ms(i));

      sim('nass_model');
      hac_dvf_L(i) = {simout};
  end
#+end_src

#+begin_src matlab :exports none
  save('./mat/tomo_exp_hac_dvf.mat', 'hac_dvf_L', 'Ms');
#+end_src

** Results
#+begin_src matlab :exports none
  load('./mat/experiment_tomography.mat', 'scans_rz_align_dist');
  load('./mat/tomo_exp_hac_dvf.mat', 'hac_dvf_L', 'Ms');
#+end_src

Let's now see how this controller performs.

First, we compute the Power Spectral Density of the sample's position error and we compare it with the open loop case in Figure [[fig:opt_stiff_hac_dvf_L_psd_disp_error]].

Similarly, the Cumulative Amplitude Spectrum is shown in Figure [[fig:opt_stiff_hac_dvf_L_cas_disp_error]].

Finally, the time domain position error signals are shown in Figure [[fig:opt_stiff_hac_dvf_L_pos_error]].
#+begin_src matlab :exports none
  n_av = 4;
  han_win = hanning(ceil(length(hac_dvf_L{1}.Em.En.Data(:,1))/n_av));

  t = hac_dvf_L{1}.Em.En.Time;
  Ts = t(2)-t(1);

  [pxx_ol, f] = pwelch(scans_rz_align_dist.Em.En.Data, han_win, [], [], 1/Ts);

  pxx_dvf_L = zeros(length(f), 6, length(Ms));
  for i = 1:length(Ms)
      [pxx, ~] = pwelch(hac_dvf_L{i}.Em.En.Data(ceil(0.2/Ts):end,:), han_win, [], [], 1/Ts);
      pxx_dvf_L(:, :, i) = pxx;
  end
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 3, 1);
  hold on;
  plot(f, sqrt(pxx_ol(:, 1)), 'k-')
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(f, sqrt(pxx_dvf_L(:, 1, i)))
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('$\Gamma_{D_x}$ [$m/\sqrt{Hz}$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax2 = subplot(2, 3, 2);
  hold on;
  plot(f, sqrt(pxx_ol(:, 2)), 'k-')
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(f, sqrt(pxx_dvf_L(:, 2, i)))
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('$\Gamma_{D_y}$ [$m/\sqrt{Hz}$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax3 = subplot(2, 3, 3);
  hold on;
  plot(f, sqrt(pxx_ol(:, 3)), 'k-')
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(f, sqrt(pxx_dvf_L(:, 3, i)))
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('$\Gamma_{D_z}$ [$m/\sqrt{Hz}$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax4 = subplot(2, 3, 4);
  hold on;
  plot(f, sqrt(pxx_ol(:, 4)), 'k-')
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(f, sqrt(pxx_dvf_L(:, 4, i)))
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('$\Gamma_{R_x}$ [$rad/\sqrt{Hz}$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax5 = subplot(2, 3, 5);
  hold on;
  plot(f, sqrt(pxx_ol(:, 5)), 'k-')
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(f, sqrt(pxx_dvf_L(:, 5, i)))
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('$\Gamma_{R_y}$ [$rad/\sqrt{Hz}$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax6 = subplot(2, 3, 6);
  hold on;
  plot(f, sqrt(pxx_ol(:, 6)), 'k-', 'DisplayName', '$\mu$-Station')
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(f, sqrt(pxx_dvf_L(:, 6, i)), ...
           'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)))
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('$\Gamma_{R_z}$ [$rad/\sqrt{Hz}$]');
  legend('location', 'southwest');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
  xlim([f(2), f(end)])
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_hac_dvf_L_psd_disp_error.pdf', 'width', 'full', 'height', 'full')
#+end_src

#+name: fig:opt_stiff_hac_dvf_L_psd_disp_error
#+caption: Amplitude Spectral Density of the position error in Open Loop and with the HAC-LAC controller
#+RESULTS:
[[file:figs/opt_stiff_hac_dvf_L_psd_disp_error.png]]

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 3, 1);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 1))))), 'k-')
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 1, i))))));
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('CAS $D_x$ [$m$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylim([1e-11, 1e-5]);

  ax2 = subplot(2, 3, 2);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 2))))), 'k-')
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 2, i))))));
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('CAS $D_y$ [$m$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylim([1e-11, 1e-5]);

  ax3 = subplot(2, 3, 3);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 3))))), 'k-')
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 3, i))))));
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('CAS $D_z$ [$m$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylim([1e-11, 1e-5]);

  ax4 = subplot(2, 3, 4);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 4))))), 'k-')
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 4, i))))));
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('CAS $R_x$ [$rad$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylim([1e-11, 1e-5]);

  ax5 = subplot(2, 3, 5);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 5))))), 'k-')
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 5, i))))));
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('CAS $R_y$ [$rad$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylim([1e-11, 1e-5]);

  ax6 = subplot(2, 3, 6);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 6))))), 'k-', 'DisplayName', '$\mu$-Station')
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 6, i))))), ...
           'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)));
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('CAS $R_z$ [$rad$]');
  legend('location', 'southwest');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylim([1e-11, 1e-5]);

  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
  xlim([f(2), f(end)])
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_hac_dvf_L_cas_disp_error.pdf', 'width', 'full', 'height', 'full')
#+end_src

#+name: fig:opt_stiff_hac_dvf_L_cas_disp_error
#+caption: Cumulative Amplitude Spectrum of the position error in Open Loop and with the HAC-LAC controller
#+RESULTS:
[[file:figs/opt_stiff_hac_dvf_L_cas_disp_error.png]]

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 3, 1);
  hold on;
  plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 1), 'k-')
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 1));
  end
  hold off;
  xlabel('Time [s]');
  ylabel('Dx [m]');

  ax2 = subplot(2, 3, 2);
  hold on;
  plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 2), 'k-')
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 2));
  end
  hold off;
  xlabel('Time [s]');
  ylabel('Dy [m]');

  ax3 = subplot(2, 3, 3);
  hold on;
  plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 3), 'k-')
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 3));
  end
  hold off;
  xlabel('Time [s]');
  ylabel('Dz [m]');

  ax4 = subplot(2, 3, 4);
  hold on;
  plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 4), 'k-')
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 4));
  end
  hold off;
  xlabel('Time [s]');
  ylabel('Rx [rad]');

  ax5 = subplot(2, 3, 5);
  hold on;
  plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 5), 'k-')
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 5));
  end
  hold off;
  xlabel('Time [s]');
  ylabel('Ry [rad]');

  ax6 = subplot(2, 3, 6);
  hold on;
  plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 6), 'k-', ...
        'DisplayName', '$\mu$-Station');
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 6), ...
           'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)));
  end
  hold off;
  xlabel('Time [s]');
  ylabel('Rz [rad]');
  legend();

  linkaxes([ax1,ax2,ax3,ax4],'x');
  xlim([0.5, inf]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_hac_dvf_L_pos_error.pdf', 'width', 'full', 'height', 'full')
#+end_src

#+name: fig:opt_stiff_hac_dvf_L_pos_error
#+caption: Position Error of the sample during a tomography experiment when no control is applied and with the HAC-DVF control architecture
#+RESULTS:
[[file:figs/opt_stiff_hac_dvf_L_pos_error.png]]

** Actuator Stroke and Forces
#+begin_src matlab :exports none
  load('./mat/tomo_exp_hac_dvf.mat', 'hac_dvf_L', 'Ms');
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(hac_dvf_L{i}.u.Time, hac_dvf_L{i}.u.Data(:,1), ...
           'DisplayName', sprintf('$m = %.0f kg$', Ms(i)));
      for j = 2:6
          set(gca,'ColorOrderIndex',i);
          plot(hac_dvf_L{i}.u.Time, hac_dvf_L{i}.u.Data(:,j), ...
               'HandleVisibility', 'off');
      end
  end
  xlim([0.5, inf]);
  xlabel('Time [s]'); ylabel('Actuator Force [N]');
  legend('location', 'northeast');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_hac_dvf_L_act_force.pdf', 'width', 'full', 'height', 'tall')
#+end_src

#+name: fig:opt_stiff_hac_dvf_L_act_force
#+caption: Force applied by the actuator during the simulation
#+RESULTS:
[[file:figs/opt_stiff_hac_dvf_L_act_force.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(hac_dvf_L{i}.yn.Dnlm.Time, hac_dvf_L{i}.yn.Dnlm.Data(:,1), ...
           'DisplayName', sprintf('$m = %.0f kg$', Ms(i)));
      for j = 2:6
          set(gca,'ColorOrderIndex',i);
          plot(hac_dvf_L{i}.yn.Dnlm.Time, hac_dvf_L{i}.yn.Dnlm.Data(:,j), ...
               'HandleVisibility', 'off');
      end
  end
  xlim([0.5, inf]);
  xlabel('Time [s]'); ylabel('Actuator Stroke [m]');
  legend('location', 'northeast');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_hac_dvf_L_act_stroke.pdf', 'width', 'full', 'height', 'tall')
#+end_src

#+name: fig:opt_stiff_hac_dvf_L_act_stroke
#+caption: Leg's stroke during the simulation
#+RESULTS:
[[file:figs/opt_stiff_hac_dvf_L_act_stroke.png]]

** Conclusion
#+begin_important

#+end_important

* Further More complex simulations
** Introduction                                                      :ignore:
** Simulation with Micro-Hexapod Offset
*** Simulation
The micro-hexapod is inducing a 10mm offset of the sample center of mass with the rotation axis.
A tomography experiment is then simulated.

#+begin_src matlab
  initializeDisturbances();
  initializeSimscapeConfiguration('gravity', false);
  initializeLoggingConfiguration('log', 'all');

  initializeSample('mass', 1, 'freq', 200);
  initializeMicroHexapod('AP', [10e-3 0 0]);
  initializeReferences('Rz_type', 'rotating', 'Rz_period', 1, ...
                       'Dh_pos', [10e-3; 0; 0; 0; 0; 0]);
#+end_src

#+begin_src matlab
  load('mat/conf_simulink.mat');
  set_param(conf_simulink, 'StopTime', '3');
  sim('nass_model');
#+end_src

#+begin_src matlab :exports none
  hac_dvf_L_Dh = simout;
  save('./mat/tomo_exp_hac_dvf.mat', 'hac_dvf_L_Dh', '-append');
#+end_src

*** Results
#+begin_src matlab :exports none
  load('./mat/tomo_exp_hac_dvf.mat', 'hac_dvf_L_Dh');
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 3, 1);
  hold on;
  plot(hac_dvf_L_Dh.Em.En.Time, hac_dvf_L_Dh.Em.En.Data(:, 1), 'k-');
  hold off;
  xlabel('Time [s]');
  ylabel('Dx [m]');

  ax2 = subplot(2, 3, 2);
  hold on;
  plot(hac_dvf_L_Dh.Em.En.Time, hac_dvf_L_Dh.Em.En.Data(:, 2), 'k-');
  hold off;
  xlabel('Time [s]');
  ylabel('Dy [m]');

  ax3 = subplot(2, 3, 3);
  hold on;
  plot(hac_dvf_L_Dh.Em.En.Time, hac_dvf_L_Dh.Em.En.Data(:, 3), 'k-');
  hold off;
  xlabel('Time [s]');
  ylabel('Dz [m]');

  ax4 = subplot(2, 3, 4);
  hold on;
  plot(hac_dvf_L_Dh.Em.En.Time, hac_dvf_L_Dh.Em.En.Data(:, 4), 'k-');
  hold off;
  xlabel('Time [s]');
  ylabel('Rx [rad]');

  ax5 = subplot(2, 3, 5);
  hold on;
  plot(hac_dvf_L_Dh.Em.En.Time, hac_dvf_L_Dh.Em.En.Data(:, 5), 'k-');
  hold off;
  xlabel('Time [s]');
  ylabel('Ry [rad]');

  ax6 = subplot(2, 3, 6);
  hold on;
  plot(hac_dvf_L_Dh.Em.En.Time, hac_dvf_L_Dh.Em.En.Data(:, 6), 'k-');
  hold off;
  xlabel('Time [s]');
  ylabel('Rz [rad]');

  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
  xlim([1, inf]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_hac_dvf_Dh_offset_disp_error.pdf', 'width', 'full', 'height', 'full')
#+end_src

#+name: fig:opt_stiff_hac_dvf_Dh_offset_disp_error
#+caption:
#+RESULTS:
[[file:figs/opt_stiff_hac_dvf_Dh_offset_disp_error.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  for j = 1:6
      plot(hac_dvf_L_Dh.u.Time, hac_dvf_L_Dh.u.Data(:,j), 'k-');
  end
  xlim([1, inf]);
  xlabel('Time [s]'); ylabel('Actuator Force [N]');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_hac_dvf_Dh_offset_F.pdf', 'width', 'normal', 'height', 'normal')
#+end_src

#+name: fig:opt_stiff_hac_dvf_Dh_offset_F
#+caption:
#+RESULTS:
[[file:figs/opt_stiff_hac_dvf_Dh_offset_F.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  for j = 1:6
      plot(hac_dvf_L_Dh.yn.Dnlm.Time, hac_dvf_L_Dh.yn.Dnlm.Data(:,j), 'k-');
  end
  xlim([1, inf]);
  xlabel('Time [s]'); ylabel('Actuator Stroke [m]');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_hac_dvf_Dh_offset_dL.pdf', 'width', 'normal', 'height', 'normal')
#+end_src

#+name: fig:opt_stiff_hac_dvf_Dh_offset_dL
#+caption:
#+RESULTS:
[[file:figs/opt_stiff_hac_dvf_Dh_offset_dL.png]]

** Simultaneous Translation scans and Spindle's rotation
*** Simulation
A simulation is now performed with translation scans and spindle rotation at the same time.

The sample has a mass of 1kg, the spindle rotation speed is 60rpm and the translation scans have a period of 4s and a triangular shape.

#+begin_src matlab
  initializeDisturbances();
  initializeSimscapeConfiguration('gravity', false);
  initializeLoggingConfiguration('log', 'all');

  initializeSample('mass', 1, 'freq', 200);
  initializeReferences('Rz_type', 'rotating', 'Rz_period', 1, ...
                       'Dy_type', 'triangular', 'Dy_amplitude', 5e-3, 'Dy_period', 4);
#+end_src

#+begin_src matlab :exports none
  load('mat/conf_simulink.mat');
  set_param(conf_simulink, 'StopTime', '9');
  sim('nass_model');
#+end_src

#+begin_src matlab :exports none
  hac_dvf_L_Dy = simout;
  save('./mat/tomo_exp_hac_dvf.mat', 'hac_dvf_L_Dy', '-append');
#+end_src

*** Results
#+begin_src matlab :exports none
  load('./mat/tomo_exp_hac_dvf.mat', 'hac_dvf_L_Dy');
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 3, 1);
  hold on;
  plot(hac_dvf_L_Dy.Em.En.Time, hac_dvf_L_Dy.Em.En.Data(:, 1), 'k-');
  hold off;
  xlabel('Time [s]');
  ylabel('Dx [m]');

  ax2 = subplot(2, 3, 2);
  hold on;
  plot(hac_dvf_L_Dy.Em.En.Time, hac_dvf_L_Dy.Em.En.Data(:, 2), 'k-');
  hold off;
  xlabel('Time [s]');
  ylabel('Dy [m]');

  ax3 = subplot(2, 3, 3);
  hold on;
  plot(hac_dvf_L_Dy.Em.En.Time, hac_dvf_L_Dy.Em.En.Data(:, 3), 'k-');
  hold off;
  xlabel('Time [s]');
  ylabel('Dz [m]');

  ax4 = subplot(2, 3, 4);
  hold on;
  plot(hac_dvf_L_Dy.Em.En.Time, hac_dvf_L_Dy.Em.En.Data(:, 4), 'k-');
  hold off;
  xlabel('Time [s]');
  ylabel('Rx [rad]');

  ax5 = subplot(2, 3, 5);
  hold on;
  plot(hac_dvf_L_Dy.Em.En.Time, hac_dvf_L_Dy.Em.En.Data(:, 5), 'k-');
  hold off;
  xlabel('Time [s]');
  ylabel('Ry [rad]');

  ax6 = subplot(2, 3, 6);
  hold on;
  plot(hac_dvf_L_Dy.Em.En.Time, hac_dvf_L_Dy.Em.En.Data(:, 6), 'k-');
  hold off;
  xlabel('Time [s]');
  ylabel('Rz [rad]');

  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
  xlim([1, inf]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_hac_dvf_Dy_scans_disp_error.pdf', 'width', 'full', 'height', 'full')
#+end_src

#+name: fig:opt_stiff_hac_dvf_Dy_scans_disp_error
#+caption:
#+RESULTS:
[[file:figs/opt_stiff_hac_dvf_Dy_scans_disp_error.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  for j = 1:6
      plot(hac_dvf_L_Dy.u.Time, hac_dvf_L_Dy.u.Data(:,j), 'k-');
  end
  xlim([1, inf]);
  xlabel('Time [s]'); ylabel('Actuator Force [N]');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_hac_dvf_Dy_scans_F.pdf', 'width', 'normal', 'height', 'normal')
#+end_src

#+name: fig:opt_stiff_hac_dvf_Dy_scans_F
#+caption:
#+RESULTS:
[[file:figs/opt_stiff_hac_dvf_Dy_scans_F.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  for j = 1:6
      plot(hac_dvf_L_Dy.yn.Dnlm.Time, hac_dvf_L_Dy.yn.Dnlm.Data(:,j), 'k-');
  end
  xlim([1, inf]);
  xlabel('Time [s]'); ylabel('Actuator Stroke [m]');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_hac_dvf_Dy_scans_dL.pdf', 'width', 'normal', 'height', 'normal')
#+end_src

#+name: fig:opt_stiff_hac_dvf_Dy_scans_dL
#+caption:
#+RESULTS:
[[file:figs/opt_stiff_hac_dvf_Dy_scans_dL.png]]

* Primary Control in the task space
<<sec:primary_control_X>>
** Introduction                                                      :ignore:
In this section, the control architecture shown in Figure [[fig:control_architecture_hac_dvf_pos_X]] is applied and consists of:
- an inner Low Authority Control loop consisting of a decentralized direct velocity control controller
- an outer loop with the primary controller $\bm{K}_\mathcal{X}$ designed in the task space

#+name: fig:control_architecture_hac_dvf_pos_X
#+caption: HAC-LAC architecture
[[file:figs/control_architecture_hac_dvf_pos_X.png]]

** Plant in the task space
Let's look $\bm{G}_\mathcal{X}(s)$.

#+begin_src matlab :exports none
  freqs = logspace(0, 3, 5000);

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for i = 1:length(Ms)
      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');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('$\mathcal{X}_x/\mathcal{F}_x$, $\mathcal{X}_y/\mathcal{F}_y$')

  ax2 = subplot(2, 2, 2);
  hold on;
  for i = 1:length(Ms)
      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');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('$\mathcal{X}_z/\mathcal{F}_z$')

  ax3 = subplot(2, 2, 3);
  hold on;
  for i = 1:length(Ms)
      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');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);

  ax4 = subplot(2, 2, 4);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(3, 3), freqs, 'Hz')))), ...
           'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);
  legend('location', 'southwest');

  linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(0, 3, 1000);

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(4, 4), freqs, 'Hz'))));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(5, 5), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [rad/(N m)]'); set(gca, 'XTickLabel',[]);
  title('$\mathcal{X}_{R_x}/\mathcal{M}_x$, $\mathcal{X}_{R_y}/\mathcal{M}_y$')

  ax2 = subplot(2, 2, 2);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(6, 6), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [rad/(N m)]'); set(gca, 'XTickLabel',[]);
  title('$\mathcal{X}_{R_z}/\mathcal{M}_z$')

  ax3 = subplot(2, 2, 3);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(4, 4), freqs, 'Hz')))));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(5, 5), freqs, 'Hz')))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);

  ax4 = subplot(2, 2, 4);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(6, 6), freqs, 'Hz')))), ...
           'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);
  legend('location', 'southwest');

  linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src

** Control in the task space
#+begin_src matlab
  Kx = tf(zeros(6));

  h = 2.5;
  Kx(1,1) = 3e7 * ...
            1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
            (s/2/pi/1 + 1)/(s/2/pi/1);

  Kx(2,2) = Kx(1,1);

  h = 2.5;
  Kx(3,3) = 3e7 * ...
            1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
            (s/2/pi/1 + 1)/(s/2/pi/1);
#+end_src

#+begin_src matlab
  h = 1.5;
  Kx(4,4) = 5e5 * ...
            1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
            (s/2/pi/1 + 1)/(s/2/pi/1);

  Kx(5,5) = Kx(4,4);

  h = 1.5;
  Kx(6,6) = 5e4 * ...
            1/h*(s/(2*pi*30/h) + 1)/(s/(2*pi*30*h) + 1) * ...
            (s/2/pi/1 + 1)/(s/2/pi/1);
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(0, 3, 1000);

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(1, 1)*Kx(1,1), freqs, 'Hz'))));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(2, 2)*Kx(2,2), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
  title('Loop Gain $x$ and $y$')

  ax2 = subplot(2, 2, 2);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(3, 3)*Kx(3,3), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
  title('Loop Gain $z$')

  ax3 = subplot(2, 2, 3);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(1, 1)*Kx(1,1), freqs, 'Hz')))));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(2, 2)*Kx(2,2), freqs, 'Hz')))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);

  ax4 = subplot(2, 2, 4);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(3, 3)*Kx(3,3), freqs, 'Hz')))), ...
           'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);
  legend('location', 'southwest');

  linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(0, 3, 1000);

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(4, 4)*Kx(4,4), freqs, 'Hz'))));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(5, 5)*Kx(5,5), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [rad/(N m)]'); set(gca, 'XTickLabel',[]);
  title('$\mathcal{X}_{R_x}/\mathcal{M}_x$, $\mathcal{X}_{R_y}/\mathcal{M}_y$')

  ax2 = subplot(2, 2, 2);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(6, 6)*Kx(6,6), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [rad/(N m)]'); set(gca, 'XTickLabel',[]);
  title('$\mathcal{X}_{R_z}/\mathcal{M}_z$')

  ax3 = subplot(2, 2, 3);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(4, 4)*Kx(4,4), freqs, 'Hz')))));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(5, 5)*Kx(5,5), freqs, 'Hz')))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);

  ax4 = subplot(2, 2, 4);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(6, 6)*Kx(6,6), freqs, 'Hz')))), ...
           'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);
  legend('location', 'southwest');

  linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src

*** Stability
#+begin_src matlab
  for i = 1:length(Ms)
      isstable(feedback(Gm_x{i}*Kx, eye(6), -1))
  end
#+end_src

** Simulation
** Conclusion
#+begin_important

#+end_important