nass-simscape/org/uncertainty_optimal_stiffness.org

#+TITLE: Determination of the optimal nano-hexapod's stiffness
#+SETUPFILE: ./setup/org-setup-file.org

* Introduction                                                        :ignore:
As shown before, many parameters other than the nano-hexapod itself do influence the plant dynamics:
- The micro-station compliance (studied [[file:uncertainty_support.org][here]])
- The payload mass and dynamical properties (studied [[file:uncertainty_payload.org][here]] and [[file:uncertainty_experiment.org][here]])
- The experimental conditions, mainly the spindle rotation speed (studied [[file:uncertainty_experiment.org][here]])

As seen before, the stiffness of the nano-hexapod greatly influence the effect of such parameters.

We wish here to see if we can determine an optimal stiffness of the nano-hexapod such that:
- Section [[sec:spindle_rotation_speed]]: the change of its dynamics due to the spindle rotation speed is acceptable
- Section [[sec:micro_station_compliance]]: the support compliance dynamics is not much present in the nano-hexapod dynamics
- Section [[sec:payload_impedance]]: the change of payload impedance has acceptable effect on the plant dynamics

The overall goal is to design a nano-hexapod that will allow the highest possible control bandwidth.

* Spindle Rotation Speed
<<sec:spindle_rotation_speed>>

** Introduction                                                      :ignore:
In this section, we look at the effect of the spindle rotation speed on the plant dynamics.

The rotation speed will have an effect due to the Coriolis effect.

** 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
We initialize all the stages with the default parameters.
#+begin_src matlab
  initializeGround();
  initializeGranite();
  initializeTy();
  initializeRy();
  initializeRz();
  initializeMicroHexapod();
  initializeAxisc();
  initializeMirror();
#+end_src

We use a sample mass of 10kg.
#+begin_src matlab
  initializeSample('mass', 10);
#+end_src

We don't include disturbances in this model as it adds complexity to the simulations and does not alter the obtained dynamics.
We however include gravity.
#+begin_src matlab
  initializeSimscapeConfiguration('gravity', true);
  initializeDisturbances('enable', false);
  initializeLoggingConfiguration('log', 'none');
  initializeController();
#+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;
  io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En');   io_i = io_i + 1;
#+end_src

** Identification when rotating at maximum speed
We identify the dynamics for the following spindle rotation speeds =Rz_rpm=:
#+begin_src matlab
  Rz_rpm = linspace(0, 60, 6);
#+end_src

And for the following nano-hexapod actuator stiffness =Ks=:
#+begin_src matlab
  Ks = logspace(3,9,7); % [N/m]
#+end_src

#+begin_src matlab :exports none
  Gk_wz_iff = {zeros(length(Ks), length(Rz_rpm))};
  Gk_wz_dvf = {zeros(length(Ks), length(Rz_rpm))};
  Gk_wz_err = {zeros(length(Ks), length(Rz_rpm))};
#+end_src

#+begin_src matlab :exports none
  for i = 1:length(Ks)
    for j = 1:length(Rz_rpm)
      initializeReferences('Rz_type', 'rotating-not-filtered', ...
                          'Rz_period', 60/Rz_rpm(j));
      initializeNanoHexapod('k', Ks(i));

      %% Run the linearization
      G = linearize(mdl, io);
      G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
      G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
                      'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
                      'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

      Gk_wz_iff(i,j) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
      Gk_wz_dvf(i,j) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};

      Jinvt = tf(inv(nano_hexapod.kinematics.J)');
      Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
      Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
      Gk_wz_err(i,j) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
    end
  end
#+end_src

#+begin_src matlab :exports none
  save('mat/optimal_stiffness_Gk_wz.mat', 'Ks', 'Rz_rpm', ...
       'Gk_wz_iff', 'Gk_wz_dvf', 'Gk_wz_err');
#+end_src

** Change of dynamics
#+begin_src matlab :exports none
  load('mat/optimal_stiffness_Gk_wz.mat');
#+end_src

We plot the change of dynamics due to the change of the spindle rotation speed (from 0rpm to 60rpm):
- Figure [[fig:opt_stiffness_wz_iff]]: from actuator force $\tau$ to force sensor $\tau_m$ (IFF plant)
- Figure [[fig:opt_stiffness_wz_dvf]]: from actuator force $\tau$ to actuator relative displacement $d\mathcal{L}$ (Decentralized positioning plant)
- Figure [[fig:opt_stiffness_wz_fx_dx]]: from force in the task space $\mathcal{F}_x$ to sample displacement $\mathcal{X}_x$ (Centralized positioning plant)
- Figure [[fig:opt_stiffness_wz_coupling]]: from force in the task space $\mathcal{F}_x$ to sample displacement $\mathcal{X}_y$ (coupling of the centralized positioning plant)

#+begin_src matlab :exports none
  figure;

  subplot(2,2,1)
  gains = logspace(0, 4, 500);
  i = 1;
  title(sprintf('$k = %.0g$ [N/m]', Ks(i)))
  hold on;
  j = length(Rz_rpm);
  set(gca,'ColorOrderIndex',1);
  plot(real(pole(Gk_wz_iff{i,j})),  imag(pole(Gk_wz_iff{i,j})),  'x');
  set(gca,'ColorOrderIndex',1);
  plot(real(tzero(Gk_wz_iff{i,j})),  imag(tzero(Gk_wz_iff{i,j})),  'o');
  for k = 1:length(gains)
      set(gca,'ColorOrderIndex',1);
      cl_poles = pole(feedback(Gk_wz_iff{i,j}, -(gains(k)/s)*eye(6)));
      plot(real(cl_poles), imag(cl_poles), '.');
  end
  j = 1
  set(gca,'ColorOrderIndex',2);
  plot(real(pole(Gk_wz_iff{i,j})),  imag(pole(Gk_wz_iff{i,j})),  'x');
  set(gca,'ColorOrderIndex',2);
  plot(real(tzero(Gk_wz_iff{i,j})),  imag(tzero(Gk_wz_iff{i,j})),  'o');
  for k = 1:length(gains)
      set(gca,'ColorOrderIndex',2);
      cl_poles = pole(feedback(Gk_wz_iff{i,j}, -(gains(k)/s)*eye(6)));
      plot(real(cl_poles), imag(cl_poles), '.');
  end
  hold off;
  axis square
  ylim([0, 20]);
  xlim([-18, 2]);
  xlabel('Real Part')
  ylabel('Imaginary Part')


  subplot(2,2,2)
  gains = logspace(0, 4, 500);
  i = 3;
  title(sprintf('$k = %.0g$ [N/m]', Ks(i)))
  hold on;
  j = length(Rz_rpm);
  set(gca,'ColorOrderIndex',1);
  plot(real(pole(Gk_wz_iff{i,j})),  imag(pole(Gk_wz_iff{i,j})),  'x');
  set(gca,'ColorOrderIndex',1);
  plot(real(tzero(Gk_wz_iff{i,j})),  imag(tzero(Gk_wz_iff{i,j})),  'o');
  for k = 1:length(gains)
      set(gca,'ColorOrderIndex',1);
      cl_poles = pole(feedback(Gk_wz_iff{i,j}, -(gains(k)/s)*eye(6)));
      plot(real(cl_poles), imag(cl_poles), '.');
  end
  j = 1
  set(gca,'ColorOrderIndex',2);
  plot(real(pole(Gk_wz_iff{i,j})),  imag(pole(Gk_wz_iff{i,j})),  'x');
  set(gca,'ColorOrderIndex',2);
  plot(real(tzero(Gk_wz_iff{i,j})),  imag(tzero(Gk_wz_iff{i,j})),  'o');
  for k = 1:length(gains)
      set(gca,'ColorOrderIndex',2);
      cl_poles = pole(feedback(Gk_wz_iff{i,j}, -(gains(k)/s)*eye(6)));
      plot(real(cl_poles), imag(cl_poles), '.');
  end
  axis square
  ylim([0, 120]);
  xlim([-120, 5]);
  xlabel('Real Part')
  ylabel('Imaginary Part')


  subplot(2,2,3)
  gains = logspace(0, 4, 500);
  i = 5;
  title(sprintf('$k = %.0g$ [N/m]', Ks(i)))
  hold on;
  j = length(Rz_rpm);
  set(gca,'ColorOrderIndex',1);
  plot(real(pole(Gk_wz_iff{i,j})),  imag(pole(Gk_wz_iff{i,j})),  'x');
  set(gca,'ColorOrderIndex',1);
  plot(real(tzero(Gk_wz_iff{i,j})),  imag(tzero(Gk_wz_iff{i,j})),  'o');
  for k = 1:length(gains)
      set(gca,'ColorOrderIndex',1);
      cl_poles = pole(feedback(Gk_wz_iff{i,j}, -(gains(k)/s)*eye(6)));
      plot(real(cl_poles), imag(cl_poles), '.');
  end
  j = 1
  set(gca,'ColorOrderIndex',2);
  plot(real(pole(Gk_wz_iff{i,j})),  imag(pole(Gk_wz_iff{i,j})),  'x');
  set(gca,'ColorOrderIndex',2);
  plot(real(tzero(Gk_wz_iff{i,j})),  imag(tzero(Gk_wz_iff{i,j})),  'o');
  for k = 1:length(gains)
      set(gca,'ColorOrderIndex',2);
      cl_poles = pole(feedback(Gk_wz_iff{i,j}, -(gains(k)/s)*eye(6)));
      plot(real(cl_poles), imag(cl_poles), '.');
  end
  hold off;
  axis square
  ylim([0, 2000]);
  xlim([-1800,200]);
  xlabel('Real Part')
  ylabel('Imaginary Part')

  subplot(2,2,4)
  gains = logspace(3, 7, 500);
  i = 7;
  title(sprintf('$k = %.0g$ [N/m]', Ks(i)))
  hold on;
  j = length(Rz_rpm);
  set(gca,'ColorOrderIndex',1);
  plot(real(pole(Gk_wz_iff{i,j})),  imag(pole(Gk_wz_iff{i,j})),  'x');
  set(gca,'ColorOrderIndex',1);
  plot(real(tzero(Gk_wz_iff{i,j})),  imag(tzero(Gk_wz_iff{i,j})),  'o');
  for k = 1:length(gains)
      set(gca,'ColorOrderIndex',1);
      cl_poles = pole(feedback(Gk_wz_iff{i,j}, -(gains(k)/s)*eye(6)));
      plot(real(cl_poles), imag(cl_poles), '.');
  end
  j = 1
  set(gca,'ColorOrderIndex',2);
  plot(real(pole(Gk_wz_iff{i,j})),  imag(pole(Gk_wz_iff{i,j})),  'x');
  set(gca,'ColorOrderIndex',2);
  plot(real(tzero(Gk_wz_iff{i,j})),  imag(tzero(Gk_wz_iff{i,j})),  'o');
  for k = 1:length(gains)
      set(gca,'ColorOrderIndex',2);
      cl_poles = pole(feedback(Gk_wz_iff{i,j}, -(gains(k)/s)*eye(6)));
      plot(real(cl_poles), imag(cl_poles), '.');
  end
  hold off;
  axis square
  ylim([0, 18000]);
  xlim([-18000, 100]);
  xlabel('Real Part')
  ylabel('Imaginary Part')
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opti_stiffness_iff_root_locus.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+name: fig:opti_stiffness_iff_root_locus
#+caption: Root Locus plot for IFF control when not rotating (in red) and when rotating at 60rpm (in blue) for 4 different nano-hexapod stiffnesses ([[./figs/opti_stiffness_iff_root_locus.png][png]], [[./figs/opti_stiffness_iff_root_locus.pdf][pdf]])
[[file:figs/opti_stiffness_iff_root_locus.png]]

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ks)
      for j = 1:length(Rz_rpm)
          set(gca,'ColorOrderIndex',i);
          plot(freqs, abs(squeeze(freqresp(Gk_wz_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
      end
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:length(Ks)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_iff{i,1}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
           'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
      for j = 2:length(Rz_rpm)
          set(gca,'ColorOrderIndex',i);
          plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
               'HandleVisibility', 'off');
      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]);
  legend('location', 'northeast');

  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiffness_wz_iff.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:opt_stiffness_wz_iff
#+caption: Change of dynamics from actuator $\tau$ to actuator force sensor $\tau_m$ for a spindle rotation speed from 0rpm to 60rpm ([[./figs/opt_stiffness_wz_iff.png][png]], [[./figs/opt_stiffness_wz_iff.pdf][pdf]])
[[file:figs/opt_stiffness_wz_iff.png]]

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ks)
      for j = 1:length(Rz_rpm)
          set(gca,'ColorOrderIndex',i);
          plot(freqs, abs(squeeze(freqresp(Gk_wz_dvf{i,j}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
      end
  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(Ks)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_dvf{i,1}('Dnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
           'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
      for j = 2:length(Rz_rpm)
          set(gca,'ColorOrderIndex',i);
          plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_dvf{i,j}('Dnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
               'HandleVisibility', 'off');
      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]);
  legend('location', 'northeast');

  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiffness_wz_dvf.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:opt_stiffness_wz_dvf
#+caption: Change of dynamics from actuator force $\tau$ to actuator displacement $d\mathcal{L}$ for a spindle rotation speed from 0rpm to 60rpm ([[./figs/opt_stiffness_wz_dvf.png][png]], [[./figs/opt_stiffness_wz_dvf.pdf][pdf]])
[[file:figs/opt_stiffness_wz_dvf.png]]

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ks)
      for j = 1:length(Rz_rpm)
          set(gca,'ColorOrderIndex',i);
          plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-');
      end
  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(Ks)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_err{i,1}('Ex', 'Fx'), freqs, 'Hz')))), '-', ...
           'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
      for j = 2:length(Rz_rpm)
          set(gca,'ColorOrderIndex',i);
          plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-', ...
               'HandleVisibility', 'off');
      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]);
  legend('location', 'northeast');

  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiffness_wz_fx_dx.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:opt_stiffness_wz_fx_dx
#+caption: Change of dynamics from force $\mathcal{F}_x$ to displacement $\mathcal{X}_x$ for a spindle rotation speed from 0rpm to 60rpm ([[./figs/opt_stiffness_wz_fx_dx.png][png]], [[./figs/opt_stiffness_wz_fx_dx.pdf][pdf]])
[[file:figs/opt_stiffness_wz_fx_dx.png]]

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

  figure;

  hold on;
  for i = 1:length(Ks)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
           'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,length(Rz_rpm)}('Ey', 'Fx'), freqs, 'Hz'))), '--', ...
       'HandleVisibility', 'off');
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,1}('Ey', 'Fx'), freqs, 'Hz'))), '--', ...
       'HandleVisibility', 'off');
      % for j = 1:length(Rz_rpm)
      %     set(gca,'ColorOrderIndex',i);
      %     plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('Ey', 'Fx'), freqs, 'Hz'))), '--', ...
      %      'HandleVisibility', 'off');
      % end
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);
  ylim([1e-10, inf]);
  legend('location', 'northeast');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiffness_wz_coupling.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:opt_stiffness_wz_coupling
#+caption: Change of Coupling from force $\mathcal{F}_x$ to displacement $\mathcal{X}_y$ for a spindle rotation speed from 0rpm to 60rpm ([[./figs/opt_stiffness_wz_coupling.png][png]], [[./figs/opt_stiffness_wz_coupling.pdf][pdf]])
[[file:figs/opt_stiffness_wz_coupling.png]]

** Conclusion                                                        :ignore:
#+begin_important
  The leg stiffness should be at higher than $k_i = 10^4\ [N/m]$ such that the main resonance frequency does not shift too much when rotating.
  For the coupling, it is more difficult to conclude about the minimum required leg stiffness.
#+end_important

#+begin_notes
  Note that we can use very soft nano-hexapod if we limit the spindle rotating speed.
#+end_notes

* Micro-Station Compliance Effect
<<sec:micro_station_compliance>>

** Introduction                                                      :ignore:
- take the 6dof compliance of the micro-station
- simple model + uncertainty

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

** Identification of the micro-station compliance
We initialize all the stages with the default parameters.
#+begin_src matlab
  initializeGround();
  initializeGranite();
  initializeTy();
  initializeRy();
  initializeRz();
  initializeMicroHexapod('type', 'compliance');
#+end_src

We put nothing on top of the micro-hexapod.
#+begin_src matlab
  initializeAxisc('type', 'none');
  initializeMirror('type', 'none');
  initializeNanoHexapod('type', 'none');
  initializeSample('type', 'none');
#+end_src

#+begin_src matlab :exports none
  initializeReferences();
  initializeDisturbances();
  initializeController();
  initializeSimscapeConfiguration();
  initializeLoggingConfiguration();
#+end_src

And we identify the dynamics from forces/torques applied on the micro-hexapod top platform to the motion of the micro-hexapod top platform at the same point.
#+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, '/Micro-Station/Micro Hexapod/Compliance/Fm'], 1, 'openinput');       io_i = io_i + 1; % Direct Forces/Torques applied on the micro-hexapod top platform
  io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Compliance/Dm'], 1, 'output'); io_i = io_i + 1; % Absolute displacement of the top platform

  %% Run the linearization
  Gm = linearize(mdl, io);
  Gm.InputName  = {'Fmx', 'Fmy', 'Fmz', 'Mmx', 'Mmy', 'Mmz'};
  Gm.OutputName = {'Dx', 'Dy', 'Dz', 'Drx', 'Dry', 'Drz'};
#+end_src

The diagonal element of the identified Micro-Station compliance matrix are shown in Figure [[fig:opt_stiff_micro_station_compliance]].

#+begin_src matlab :exports none
  labels = {'$D_x/F_{x}$', '$D_y/F_{y}$', '$D_z/F_{z}$', '$R_{x}/M_{x}$', '$R_{y}/M_{y}$', '$R_{R}/M_{z}$'};

  freqs = logspace(1, 3, 1000);

  figure;

  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gm(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]');
  ylabel('Compliance');
  legend('location', 'northwest');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiff_micro_station_compliance.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:opt_stiff_micro_station_compliance
#+caption: Identified Compliance of the Micro-Station ([[./figs/opt_stiff_micro_station_compliance.png][png]], [[./figs/opt_stiff_micro_station_compliance.pdf][pdf]])
[[file:figs/opt_stiff_micro_station_compliance.png]]

** Identification of the dynamics with a rigid micro-station
We now identify the dynamics when the micro-station is rigid.
This is equivalent of identifying the dynamics of the nano-hexapod when fixed to a rigid ground.
#+begin_src matlab :exports none
  initializeGround('type', 'rigid');
  initializeGranite('type', 'rigid');
  initializeTy('type', 'rigid');
  initializeRy('type', 'rigid');
  initializeRz('type', 'rigid');
  initializeMicroHexapod('type', 'rigid');

  initializeAxisc('type', 'rigid');
  initializeMirror('type', 'rigid');
#+end_src

We also choose the sample to be rigid and to have a mass of 10kg.
#+begin_src matlab
  initializeSample('type', 'rigid', 'mass', 10);
#+end_src

#+begin_src matlab :exports none
  initializeReferences();
  initializeDisturbances();
  initializeController();
  initializeSimscapeConfiguration();
  initializeLoggingConfiguration();
  initializeSimscapeConfiguration('gravity', false);
#+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;
  io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En');   io_i = io_i + 1;
#+end_src

As before, we identify the dynamics for the following actuator stiffnesses:
#+begin_src matlab
  Ks = logspace(3,9,7); % [N/m]
#+end_src

#+begin_src matlab :exports none
  Gmr_iff = {zeros(length(Ks))};
  Gmr_dvf = {zeros(length(Ks))};
  Gmr_err = {zeros(length(Ks))};
#+end_src

#+begin_src matlab :exports none
  for i = 1:length(Ks)
    initializeNanoHexapod('k', Ks(i));

    G = linearize(mdl, io);
    G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
    G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
                    'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
                    'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

    Gmr_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
    Gmr_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};

    Jinvt = tf(inv(nano_hexapod.kinematics.J)');
    Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
    Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
    Gmr_err(i) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
  end
#+end_src

** Identification of the dynamics with a flexible micro-station
We now initialize all the micro-station stages to be flexible.
And we identify the dynamics of the nano-hexapod.
#+begin_src matlab :exports none
  initializeGround();
  initializeGranite();
  initializeTy();
  initializeRy();
  initializeRz();
  initializeMicroHexapod();

  initializeAxisc();
  initializeMirror();
#+end_src

#+begin_src matlab :exports none
  Gmf_iff = {zeros(length(Ks))};
  Gmf_dvf = {zeros(length(Ks))};
  Gmf_err = {zeros(length(Ks))};
#+end_src

#+begin_src matlab :exports none
  for i = 1:length(Ks)
    initializeNanoHexapod('k', Ks(i));

    G = linearize(mdl, io);
    G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
    G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
                    'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
                    'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

    Gmf_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
    Gmf_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};

    Jinvt = tf(inv(nano_hexapod.kinematics.J)');
    Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
    Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
    Gmf_err(i) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
  end
#+end_src

#+begin_src matlab :exports none
  save('mat/optimal_stiffness_micro_station_compliance.mat', 'Ks', ...
       'Gmr_iff',    'Gmr_dvf',    'Gmr_err', ...
       'Gmf_iff',    'Gmf_dvf',    'Gmf_err');
#+end_src

** Obtained Dynamics
#+begin_src matlab :exports none
  load('mat/optimal_stiffness_micro_station_compliance.mat');
#+end_src

We plot the change of dynamics due to the compliance of the Micro-Station.
The solid curves are corresponding to the nano-hexapod without the micro-station, and the dashed curves with the micro-station:
- Figure [[fig:opt_stiffness_micro_station_iff]]: from actuator force $\tau$ to force sensor $\tau_m$ (IFF plant)
- Figure [[fig:opt_stiffness_micro_station_dvf]]: from actuator force $\tau$ to actuator relative displacement $d\mathcal{L}$ (Decentralized positioning plant)
- Figure [[fig:opt_stiffness_micro_station_fx_dx]]: from force in the task space $\mathcal{F}_x$ to sample displacement $\mathcal{X}_x$ (Centralized positioning plant)
- Figure [[fig:opt_stiffness_micro_station_fz_dz]]: from force in the task space $\mathcal{F}_z$ to sample displacement $\mathcal{X}_z$ (Centralized positioning plant)

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ks)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gmr_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gmf_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:length(Ks)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
         'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
    set(gca,'ColorOrderIndex',i);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_iff{i}('Fnlm1', 'Fnl1'), 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');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiffness_micro_station_iff.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:opt_stiffness_micro_station_iff
#+caption: Change of dynamics from actuator $\tau$ to actuator force sensor $\tau_m$ due to the micro-station compliance ([[./figs/opt_stiffness_micro_station_iff.png][png]], [[./figs/opt_stiffness_micro_station_iff.pdf][pdf]])
[[file:figs/opt_stiffness_micro_station_iff.png]]

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ks)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gmr_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gmf_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:length(Ks)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
         'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
    set(gca,'ColorOrderIndex',i);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_dvf{i}('Dnlm1', 'Fnl1'), 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');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiffness_micro_station_dvf.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:opt_stiffness_micro_station_dvf
#+caption: Change of dynamics from actuator force $\tau$ to actuator displacement $d\mathcal{L}$ due to the micro-station compliance ([[./figs/opt_stiffness_micro_station_dvf.png][png]], [[./figs/opt_stiffness_micro_station_dvf.pdf][pdf]])
[[file:figs/opt_stiffness_micro_station_dvf.png]]

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ks)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-');
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), 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(Ks)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz')))), '-', ...
         'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
    set(gca,'ColorOrderIndex',i);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), 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');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiffness_micro_station_fx_dx.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:opt_stiffness_micro_station_fx_dx
#+caption: Change of dynamics from force $\mathcal{F}_x$ to displacement $\mathcal{X}_x$ due to the micro-station compliance ([[./figs/opt_stiffness_micro_station_fx_dx.png][png]], [[./figs/opt_stiffness_micro_station_fx_dx.pdf][pdf]])
[[file:figs/opt_stiffness_micro_station_fx_dx.png]]

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ks)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '-');
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ez', 'Fz'), 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(Ks)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_err{i}('Ez', 'Fz'), freqs, 'Hz')))), '-', ...
         'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
    set(gca,'ColorOrderIndex',i);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_err{i}('Ez', 'Fz'), 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');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiffness_micro_station_fz_dz.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:opt_stiffness_micro_station_fz_dz
#+caption: Change of dynamics from force $\mathcal{F}_z$ to displacement $\mathcal{X}_z$ due to the micro-station compliance ([[./figs/opt_stiffness_micro_station_fz_dz.png][png]], [[./figs/opt_stiffness_micro_station_fz_dz.pdf][pdf]])
[[file:figs/opt_stiffness_micro_station_fz_dz.png]]

** Conclusion                                                        :ignore:
#+begin_important
  The dynamics of the nano-hexapod is not affected by the micro-station dynamics (compliance) when the stiffness of the legs is less than $10^6\ [N/m]$.
  When the nano-hexapod is stiff ($k>10^7\ [N/m]$), the compliance of the micro-station appears in the primary plant.
#+end_important

* Payload "Impedance" Effect
<<sec:payload_impedance>>

** Introduction                                                      :ignore:

** 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
We initialize all the stages with the default parameters.
#+begin_src matlab :exports none
  initializeGround();
  initializeGranite();
  initializeTy();
  initializeRy();
  initializeRz();
  initializeMicroHexapod();
  initializeAxisc();
  initializeMirror();
#+end_src

We don't include disturbances in this model as it adds complexity to the simulations and does not alter the obtained dynamics. :exports none
#+begin_src matlab
  initializeDisturbances('enable', false);
#+end_src

We set the controller type to Open-Loop, and we do not need to log any signal.
#+begin_src matlab
  initializeSimscapeConfiguration('gravity', true);
  initializeController();
  initializeLoggingConfiguration('log', 'none');
  initializeReferences();
#+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;
  io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En');   io_i = io_i + 1;
#+end_src

** Identification of the dynamics while change the payload dynamics
We make the following change of payload dynamics:
- Change of mass: from 1kg to 50kg
- Change of resonance frequency: from 50Hz to 500Hz
- The damping ratio of the payload is fixed to $\xi = 0.2$

We identify the dynamics for the following payload masses =Ms= and nano-hexapod leg's stiffnesses =Ks=:
#+begin_src matlab
  Ms = [1, 20, 50]; % [Kg]
  Ks = logspace(3,9,7); % [N/m]
#+end_src

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

#+begin_src matlab :exports none
  for i = 1:length(Ks)
    for j = 1:length(Ms)
      initializeNanoHexapod('k', Ks(i));
      initializeSample('mass', Ms(j), 'freq', 100*ones(6,1));

      G = linearize(mdl, io);
      G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
      G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
                      'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
                      'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

      Gm_iff(i,j) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
      Gm_dvf(i,j) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};

      Jinvt = tf(inv(nano_hexapod.kinematics.J)');
      Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
      Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
      Gm_err(i,j) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
    end
  end
#+end_src

We then identify the dynamics for the following payload resonance frequencies =Fs=:
#+begin_src matlab
  Fs = [50, 200, 500]; % [Hz]
#+end_src

#+begin_src matlab :exports none
  Gf_iff = {zeros(length(Ks), length(Fs))};
  Gf_dvf = {zeros(length(Ks), length(Fs))};
  Gf_err = {zeros(length(Ks), length(Fs))};
#+end_src

#+begin_src matlab :exports none
  for i = 1:length(Ks)
    for j = 1:length(Fs)
      initializeNanoHexapod('k', Ks(i));
      initializeSample('mass', 10, 'freq', Fs(j)*ones(6,1));

      G = linearize(mdl, io);
      G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
      G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
                      'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
                      'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

      Gf_iff(i,j) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
      Gf_dvf(i,j) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};

      Jinvt = tf(inv(nano_hexapod.kinematics.J)');
      Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
      Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
      Gf_err(i,j) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
    end
  end
#+end_src

#+begin_src matlab :exports none
  save('mat/optimal_stiffness_Gm_Gf.mat', 'Ks', 'Ms', 'Fs', ...
       'Gm_iff',    'Gm_dvf',    'Gm_err', ...
       'Gf_iff',    'Gf_dvf',    'Gf_err');
#+end_src

** TODO Change of optimal gain for decentralized control           :noexport:
For each payload, compute the optimal gain for the IFF control.
The optimal value corresponds to critical damping to *all* the 6 modes of the nano-hexapod.

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

Change of Mass
#+begin_src matlab :exports none
  freqs = logspace(-1, 3, 1000);

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ks)
    for j = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
    end
  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(Ks)
    for j = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      if j == 1
        plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
            'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
      else
        plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
             'HandleVisibility', 'off');
      end
    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]);
  legend('location', 'northeast');

  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

Optimal gains:
#+begin_src matlab
  opt_gains = [20 60 200 600 2000 6000 20000];
#+end_src

Change of poles with mass
#+begin_src matlab :exports none
  i = 7;

  figure;
  hold on;
  for j = 1:length(Ms)
    set(gca,'ColorOrderIndex',j);
    cl_poles = pole(feedback(Gm_iff{i,j}, (-gains(k)/s)*eye(6)));
    plot(real(cl_poles), imag(cl_poles), '.');
  end
#+end_src

#+begin_src matlab :exports none
  i = 4;

  gains = logspace(1, 3, 500);

  figure;
  hold on;
  for j = 1:length(Ms)
    for k = 1:length(gains)
        set(gca,'ColorOrderIndex',j);
        cl_poles = pole(feedback(Gm_iff{i,j}, (-gains(k)/s)*eye(6)));
        poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
        plot(gains(k)*ones(size(poles_damp)), poles_damp, '.');
    end
  end
  xlabel('Control Gain');
  ylabel('Damping of the Poles');
  set(gca, 'XScale', 'log');
  ylim([0,pi/2]);
#+end_src

#+begin_src matlab :exports none
  i = 7;

  gains = logspace(3, 5, 500);

  figure;
  hold on;
  for j = 1:length(Ms)
    for k = 1:length(gains)
        set(gca,'ColorOrderIndex',j);
        cl_poles = pole(feedback(Gm_iff{i,j}, (-gains(k)/s)*eye(6)));
        poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
        plot(gains(k)*ones(size(poles_damp)), poles_damp, '.');
    end
  end
  xlabel('Control Gain');
  ylabel('Damping of the Poles');
  set(gca, 'XScale', 'log');
  ylim([0,pi/2]);
#+end_src

Change of payload resonance frequency
#+begin_src matlab :exports none
  i = 1;

  gains = logspace(0, 2, 100);

  figure;
  hold on;
  for j = 1:length(Fs)
    for k = 1:length(gains)
        set(gca,'ColorOrderIndex',j);
        cl_poles = pole(feedback(Gf_iff{i,j}, (-gains(k)/s)*eye(6)));
        poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
        plot(gains(k)*ones(size(poles_damp)), poles_damp, '.');
    end
  end
  xlabel('Control Gain');
  ylabel('Damping of the Poles');
  set(gca, 'XScale', 'log');
  ylim([0,pi/2]);
#+end_src

#+begin_src matlab :exports none
  i = 7;

  gains = logspace(3, 5, 100);

  figure;
  hold on;
  for j = 1:length(Fs)
    for k = 1:length(gains)
        set(gca,'ColorOrderIndex',j);
        cl_poles = pole(feedback(Gf_iff{i,j}, (-gains(k)/s)*eye(6)));
        poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
        plot(gains(k)*ones(size(poles_damp)), poles_damp, '.');
    end
  end
  xlabel('Control Gain');
  ylabel('Damping of the Poles');
  set(gca, 'XScale', 'log');
  ylim([0,pi/2]);
#+end_src


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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ks)
    for j = 1:length(Fs)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
    end
  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(Ks)
    for j = 1:length(Fs)
      set(gca,'ColorOrderIndex',i);
      if j == 1
        plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
            'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
      else
        plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
             'HandleVisibility', 'off');
      end
    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]);
  legend('location', 'southwest');

  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

** Change of dynamics for the primary controller
#+begin_src matlab :exports none
  load('mat/optimal_stiffness_Gm_Gf.mat');
#+end_src

*** Frequency variation
We here compare the dynamics for the same payload mass, but different stiffness resulting in different resonance frequency of the payload:
- Figure [[fig:opt_stiffness_payload_freq_fz_dz]]: dynamics from a force $\mathcal{F}_z$ applied in the task space in the vertical direction to the vertical displacement of the sample $\mathcal{X}_z$ for both a very soft and a very stiff nano-hexapod.
- Figure [[fig:opt_stiffness_payload_freq_all]]: same, but for all tested nano-hexapod stiffnesses

We can see two mass lines for the soft nano-hexapod (Figure [[fig:opt_stiffness_payload_freq_fz_dz]]):
- The first mass line corresponds to $\frac{1}{(m_n + m_p)s^2}$ where $m_p = 10\ [kg]$ is the mass of the payload and $m_n = 15\ [Kg]$ is the mass of the nano-hexapod top platform and attached mirror
- The second mass line corresponds to $\frac{1}{m_n s^2}$
- The zero corresponds to the resonance of the payload alone (fixed nano-hexapod's top platform)

#+begin_src matlab :exports none
  i = 1;

  freqs = logspace(-1, 3, 1000);

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for j = 1:length(Fs)
    plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title(sprintf('$k = %.0e$ [N/m]', Ks(i)))

  ax2 = subplot(2, 2, 3);
  hold on;
  for j = 1:length(Fs)
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz')))), '-', ...
         'DisplayName', sprintf('$\\omega_0 = %.0f$ [Hz]', Fs(j)));
  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');
  xlim([freqs(1), freqs(end)]);

  i = 7;

  ax3 = subplot(2, 2, 2);
  hold on;
  for j = 1:length(Fs)
    plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title(sprintf('$k = %.0e$ [N/m]', Ks(i)))

  ax4 = subplot(2, 2, 4);
  hold on;
  for j = 1:length(Fs)
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz')))), '-', ...
         'DisplayName', sprintf('$\\omega_0 = %.0f$ [Hz]', Fs(j)));
  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([ax3,ax4],'x');
  xlim([freqs(1), freqs(end)]);

  linkaxes([ax1,ax3],'y');
#+end_src

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

#+name: fig:opt_stiffness_payload_freq_fz_dz
#+caption: Dynamics from $\mathcal{F}_z$ to $\mathcal{X}_z$ for varying payload resonance frequency, both for a soft nano-hexapod and a stiff nano-hexapod
#+RESULTS:
[[file:figs/opt_stiffness_payload_freq_fz_dz.png]]

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ks)
    for j = 1:length(Fs)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
    end
  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(Ks)
    for j = 1:length(Fs)
      set(gca,'ColorOrderIndex',i);
      if j == 1
        plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz')))), '-', ...
         'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
      else
        plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz')))), '-', ...
             'HandleVisibility', 'off');
      end
    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]);
  legend('location', 'southwest');

  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiffness_payload_freq_all.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:opt_stiffness_payload_freq_all
#+caption: Dynamics from $\mathcal{F}_z$ to $\mathcal{X}_z$ for varying payload resonance frequency ([[./figs/opt_stiffness_payload_freq_all.png][png]], [[./figs/opt_stiffness_payload_freq_all.pdf][pdf]])
[[file:figs/opt_stiffness_payload_freq_all.png]]

*** Mass variation
We here compare the dynamics for different payload mass with the same resonance frequency (100Hz):
- Figure [[fig:opt_stiffness_payload_mass_fz_dz]]: dynamics from a force $\mathcal{F}_z$ applied in the task space in the vertical direction to the vertical displacement of the sample $\mathcal{X}_z$ for both a very soft and a very stiff nano-hexapod.
- Figure [[fig:opt_stiffness_payload_mass_all]]: same, but for all tested nano-hexapod stiffnesses

We can see here that for the soft nano-hexapod:
- the first resonance $\omega_n$ is changing with the mass of the payload as $\omega_n = \sqrt{\frac{k_n}{m_p + m_n}}$ with $k_p$ the stiffness of the nano-hexapod, $m_p$ the payload's mass and $m_n$ the mass of the nano-hexapod top platform
- the first mass line corresponding to $\frac{1}{(m_p + m_n)s^2}$ is changing with the payload mass
- the zero at 100Hz is not changing as it corresponds to the resonance of the payload itself
- the second mass line does not change

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

  figure;

  i = 1;

  ax1 = subplot(2, 2, 1);
  hold on;
  for j = 1:length(Ms)
    plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title(sprintf('$k = %.0e$ [N/m]', Ks(i)))

  ax2 = subplot(2, 2, 3);
  hold on;
  for j = 1:length(Ms)
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), 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]);

  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);

  i = 7;

  ax3 = subplot(2, 2, 2);
  hold on;
  for j = 1:length(Ms)
    plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title(sprintf('$k = %.0e$ [N/m]', Ks(i)))

  ax4 = subplot(2, 2, 4);
  hold on;
  for j = 1:length(Ms)
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz')))), '-', ...
          'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(j)));
  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([ax3,ax4],'x');
  xlim([freqs(1), freqs(end)]);

  linkaxes([ax1,ax3],'y');
#+end_src

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

#+name: fig:opt_stiffness_payload_mass_fz_dz
#+caption: Dynamics from $\mathcal{F}_z$ to $\mathcal{X}_z$ for varying payload mass, both for a soft nano-hexapod and a stiff nano-hexapod
#+RESULTS:
[[file:figs/opt_stiffness_payload_mass_fz_dz.png]]

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ks)
    for j = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
    end
  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(Ks)
    for j = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      if j == 1
        plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz')))), '-', ...
            'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
      else
        plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz')))), '-', ...
             'HandleVisibility', 'off');
      end
    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]);
  legend('location', 'southwest');

  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiffness_payload_mass_all.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:opt_stiffness_payload_mass_all
#+caption: Dynamics from $\mathcal{F}_z$ to $\mathcal{X}_z$ for varying payload mass ([[./figs/opt_stiffness_payload_mass_all.png][png]], [[./figs/opt_stiffness_payload_mass_all.pdf][pdf]])
[[file:figs/opt_stiffness_payload_mass_all.png]]

*** Total variation
We now plot the total change of dynamics due to change of the payload (Figures [[fig:opt_stiffness_payload_impedance_all_fz_dz]] and [[fig:opt_stiffness_payload_impedance_fz_dz]]):
- mass from 1kg to 50kg
- main resonance from 50Hz to 500Hz

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ks)
    for j = 1:length(Fs)
        set(gca,'ColorOrderIndex',i);
        plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-');
    end
    for j = 1:length(Ms)
        set(gca,'ColorOrderIndex',i);
        plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-');
    end
  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(Ks)
    for j = 1:length(Fs)
      set(gca,'ColorOrderIndex',i);
      if j == 1
        plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-', ...
            'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
      else
        plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-', ...
             'HandleVisibility', 'off');
      end
    end
    for j = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz')))), '-', ...
           'HandleVisibility', 'off');
    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]);
  legend('location', 'southwest');

  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiffness_payload_impedance_all_fz_dz.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:opt_stiffness_payload_impedance_all_fz_dz
#+caption: Dynamics from $\mathcal{F}_z$ to $\mathcal{X}_z$ for varying payload dynamics, both for a soft nano-hexapod and a stiff nano-hexapod ([[./figs/opt_stiffness_payload_impedance_all_fz_dz.png][png]], [[./figs/opt_stiffness_payload_impedance_all_fz_dz.pdf][pdf]])
[[file:figs/opt_stiffness_payload_impedance_all_fz_dz.png]]

#+begin_src matlab :exports none
  i = 1;

  freqs = logspace(-1, 3, 1000);

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for j = 1:length(Fs)
    plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
  end
  for j = 1:length(Ms)
    plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title(sprintf('$k = %.0e$ [N/m]', Ks(i)))

  ax2 = subplot(2, 2, 3);
  hold on;
  for j = 1:length(Fs)
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
  end
  for j = 1:length(Ms)
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
  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');
  xlim([freqs(1), freqs(end)]);

  i = 7;

  ax1 = subplot(2, 2, 2);
  hold on;
  for j = 1:length(Fs)
    plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
  end
  for j = 1:length(Ms)
    plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title(sprintf('$k = %.0e$ [N/m]', Ks(i)))

  ax2 = subplot(2, 2, 4);
  hold on;
  for j = 1:length(Fs)
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
  for j = 1:length(Ms)
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
  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');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiffness_payload_impedance_fz_dz.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:opt_stiffness_payload_impedance_fz_dz
#+caption: Dynamics from $\mathcal{F}_z$ to $\mathcal{X}_z$ for varying payload dynamics, both for a soft nano-hexapod and a stiff nano-hexapod ([[./figs/opt_stiffness_payload_impedance_fz_dz.png][png]], [[./figs/opt_stiffness_payload_impedance_fz_dz.pdf][pdf]])
[[file:figs/opt_stiffness_payload_impedance_fz_dz.png]]

** Conclusion                                                        :ignore:
#+begin_important

#+end_important

* Total Change of dynamics
#+begin_src matlab :exports none
  load('mat/optimal_stiffness_Gm_Gf.mat');
  load('mat/optimal_stiffness_micro_station_compliance.mat');
  load('mat/optimal_stiffness_Gk_wz.mat');
#+end_src

We now consider the total change of nano-hexapod dynamics due to:
- =Gk_wz_err= - Change of spindle rotation speed
- =Gf_err= and =Gm_err= - Change of payload resonance
- =Gmf_err= and =Gmr_err= - Micro-Station compliance

The obtained dynamics are shown:
- Figure [[fig:opt_stiffness_plant_dynamics_fx_dx_k_1e3]] for a stiffness $k = 10^3\ [N/m]$
- Figure [[fig:opt_stiffness_plant_dynamics_fx_dx_k_1e5]] for a stiffness $k = 10^5\ [N/m]$
- Figure [[fig:opt_stiffness_plant_dynamics_fx_dx_k_1e7]] for a stiffness $k = 10^7\ [N/m]$
- Figure [[fig:opt_stiffness_plant_dynamics_fx_dx_k_1e9]] for a stiffness $k = 10^9\ [N/m]$

And finally, in Figures [[fig:opt_stiffness_plant_dynamics_task_space]] and [[fig:opt_stiffness_plant_dynamics_task_space_colors]] are shown an animation of the change of dynamics with the nano-hexapod's stiffness.

#+begin_src matlab :exports none
  i = 1;

  freqs = logspace(-1, 3, 1000);

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  % =Gf_err= - Change of payload resonance
  plot(freqs, abs(squeeze(freqresp(Gf_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Payload Freq');
  for j = 2:length(Fs)
    set(gca,'ColorOrderIndex',1);
    plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
         'HandleVisibility', 'off');
  end
  % =Gm_err= - Change of payload mass
  set(gca,'ColorOrderIndex',2);
  plot(freqs, abs(squeeze(freqresp(Gm_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Payload Mass');
  for j = 2:length(Ms)
    set(gca,'ColorOrderIndex',2);
    plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
         'HandleVisibility', 'off');
  end
  % =Gm_err= - Change of payload mass
  set(gca,'ColorOrderIndex',3);
  plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Rotationg Speed');
  for j = 2:length(Rz_rpm)
    set(gca,'ColorOrderIndex',3);
    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
         'HandleVisibility', 'off');
  end
  set(gca,'ColorOrderIndex',4);
  % =Gmr_err= - Rigid Micro-Station
  plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Rigid $\mu$-station');
  % =Gmf_err= - Flexible Micro-Station
  plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Flexible $\mu$-station');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title(sprintf('$k = %.0e$ [N/m]', Ks(i)))
  legend('location', 'southwest');

  ax2 = subplot(2, 1, 2);
  hold on;
  for j = 1:length(Fs)
    set(gca,'ColorOrderIndex',1);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  end
  for j = 1:length(Ms)
    set(gca,'ColorOrderIndex',2);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  end
  for j = 1:length(Rz_rpm)
    set(gca,'ColorOrderIndex',3);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  end
  set(gca,'ColorOrderIndex',4);
  % =Gmr_err= - Rigid Micro-Station
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  % =Gmf_err= - Flexible Micro-Station
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  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');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiffness_plant_dynamics_fx_dx_k_1e3.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:opt_stiffness_plant_dynamics_fx_dx_k_1e3
#+caption: Total variation of the dynamics from $\mathcal{F}_x$ to $\mathcal{X}_x$. Nano-hexapod leg's stiffness is equal to $k = 10^3\ [N/m]$ ([[./figs/opt_stiffness_plant_dynamics_fx_dx_k_1e3.png][png]], [[./figs/opt_stiffness_plant_dynamics_fx_dx_k_1e3.pdf][pdf]])
[[file:figs/opt_stiffness_plant_dynamics_fx_dx_k_1e3.png]]

#+begin_src matlab :exports none
  i = 3;

  freqs = logspace(-1, 3, 1000);

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  % =Gf_err= - Change of payload resonance
  plot(freqs, abs(squeeze(freqresp(Gf_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Payload Freq');
  for j = 2:length(Fs)
    set(gca,'ColorOrderIndex',1);
    plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
         'HandleVisibility', 'off');
  end
  % =Gm_err= - Change of payload mass
  set(gca,'ColorOrderIndex',2);
  plot(freqs, abs(squeeze(freqresp(Gm_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Payload Mass');
  for j = 2:length(Ms)
    set(gca,'ColorOrderIndex',2);
    plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
         'HandleVisibility', 'off');
  end
  % =Gm_err= - Change of payload mass
  set(gca,'ColorOrderIndex',3);
  plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Rotationg Speed');
  for j = 2:length(Rz_rpm)
    set(gca,'ColorOrderIndex',3);
    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
         'HandleVisibility', 'off');
  end
  set(gca,'ColorOrderIndex',4);
  % =Gmr_err= - Rigid Micro-Station
  plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Rigid $\mu$-station');
  % =Gmf_err= - Flexible Micro-Station
  plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Flexible $\mu$-station');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title(sprintf('$k = %.0e$ [N/m]', Ks(i)))
  legend('location', 'southwest');

  ax2 = subplot(2, 1, 2);
  hold on;
  for j = 1:length(Fs)
    set(gca,'ColorOrderIndex',1);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  end
  for j = 1:length(Ms)
    set(gca,'ColorOrderIndex',2);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  end
  for j = 1:length(Rz_rpm)
    set(gca,'ColorOrderIndex',3);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  end
  set(gca,'ColorOrderIndex',4);
  % =Gmr_err= - Rigid Micro-Station
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  % =Gmf_err= - Flexible Micro-Station
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  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');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiffness_plant_dynamics_fx_dx_k_1e5.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:opt_stiffness_plant_dynamics_fx_dx_k_1e5
#+caption: Total variation of the dynamics from $\mathcal{F}_x$ to $\mathcal{X}_x$. Nano-hexapod leg's stiffness is equal to $k = 10^5\ [N/m]$ ([[./figs/opt_stiffness_plant_dynamics_fx_dx_k_1e5.png][png]], [[./figs/opt_stiffness_plant_dynamics_fx_dx_k_1e5.pdf][pdf]])
[[file:figs/opt_stiffness_plant_dynamics_fx_dx_k_1e5.png]]

#+begin_src matlab :exports none
  i = 5;

  freqs = logspace(-1, 3, 1000);

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  % =Gf_err= - Change of payload resonance
  plot(freqs, abs(squeeze(freqresp(Gf_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Payload Freq');
  for j = 2:length(Fs)
    set(gca,'ColorOrderIndex',1);
    plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
         'HandleVisibility', 'off');
  end
  % =Gm_err= - Change of payload mass
  set(gca,'ColorOrderIndex',2);
  plot(freqs, abs(squeeze(freqresp(Gm_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Payload Mass');
  for j = 2:length(Ms)
    set(gca,'ColorOrderIndex',2);
    plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
         'HandleVisibility', 'off');
  end
  % =Gm_err= - Change of payload mass
  set(gca,'ColorOrderIndex',3);
  plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Rotationg Speed');
  for j = 2:length(Rz_rpm)
    set(gca,'ColorOrderIndex',3);
    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
         'HandleVisibility', 'off');
  end
  set(gca,'ColorOrderIndex',4);
  % =Gmr_err= - Rigid Micro-Station
  plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Rigid $\mu$-station');
  % =Gmf_err= - Flexible Micro-Station
  plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Flexible $\mu$-station');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title(sprintf('$k = %.0e$ [N/m]', Ks(i)))
  legend('location', 'southwest');

  ax2 = subplot(2, 1, 2);
  hold on;
  for j = 1:length(Fs)
    set(gca,'ColorOrderIndex',1);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  end
  for j = 1:length(Ms)
    set(gca,'ColorOrderIndex',2);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  end
  for j = 1:length(Rz_rpm)
    set(gca,'ColorOrderIndex',3);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  end
  set(gca,'ColorOrderIndex',4);
  % =Gmr_err= - Rigid Micro-Station
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  % =Gmf_err= - Flexible Micro-Station
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  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');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiffness_plant_dynamics_fx_dx_k_1e7.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:opt_stiffness_plant_dynamics_fx_dx_k_1e7
#+caption: Total variation of the dynamics from $\mathcal{F}_x$ to $\mathcal{X}_x$. Nano-hexapod leg's stiffness is equal to $k = 10^7\ [N/m]$ ([[./figs/opt_stiffness_plant_dynamics_fx_dx_k_1e7.png][png]], [[./figs/opt_stiffness_plant_dynamics_fx_dx_k_1e7.pdf][pdf]])
[[file:figs/opt_stiffness_plant_dynamics_fx_dx_k_1e7.png]]

#+begin_src matlab :exports none
  i = 7;

  freqs = logspace(-1, 3, 1000);

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  % =Gf_err= - Change of payload resonance
  plot(freqs, abs(squeeze(freqresp(Gf_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Payload Freq');
  for j = 2:length(Fs)
      set(gca,'ColorOrderIndex',1);
      plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
           'HandleVisibility', 'off');
  end
  % =Gm_err= - Change of payload mass
  set(gca,'ColorOrderIndex',2);
  plot(freqs, abs(squeeze(freqresp(Gm_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Payload Mass');
  for j = 2:length(Ms)
      set(gca,'ColorOrderIndex',2);
      plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
           'HandleVisibility', 'off');
  end
  % =Gm_err= - Change of payload mass
  set(gca,'ColorOrderIndex',3);
  plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Rotationg Speed');
  for j = 2:length(Rz_rpm)
      set(gca,'ColorOrderIndex',3);
      plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
           'HandleVisibility', 'off');
  end
  set(gca,'ColorOrderIndex',4);
  % =Gmr_err= - Rigid Micro-Station
  plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Rigid $\mu$-station');
  % =Gmf_err= - Flexible Micro-Station
  plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', 'Flexible $\mu$-station');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title(sprintf('$k = %.0e$ [N/m]', Ks(i)))
  legend('location', 'southwest');

  ax2 = subplot(2, 1, 2);
  hold on;
  for j = 1:length(Fs)
    set(gca,'ColorOrderIndex',1);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  end
  for j = 1:length(Ms)
    set(gca,'ColorOrderIndex',2);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  end
  for j = 1:length(Rz_rpm)
    set(gca,'ColorOrderIndex',3);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  end
  set(gca,'ColorOrderIndex',4);
  % =Gmr_err= - Rigid Micro-Station
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  % =Gmf_err= - Flexible Micro-Station
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz')))), '-');
  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');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiffness_plant_dynamics_fx_dx_k_1e9.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:opt_stiffness_plant_dynamics_fx_dx_k_1e9
#+caption: Total variation of the dynamics from $\mathcal{F}_x$ to $\mathcal{X}_x$. Nano-hexapod leg's stiffness is equal to $k = 10^9\ [N/m]$ ([[./figs/opt_stiffness_plant_dynamics_fx_dx_k_1e9.png][png]], [[./figs/opt_stiffness_plant_dynamics_fx_dx_k_1e9.pdf][pdf]])
[[file:figs/opt_stiffness_plant_dynamics_fx_dx_k_1e9.png]]

#+NAME: fig:opt_stiffness_plant_dynamics_task_space
#+HEADER: :tangle no :exports results :results value file raw replace :noweb yes
#+begin_src matlab
  h = figure;
  filename = 'figs/opt_stiffness_plant_dynamics_task_space.gif';

  for i = 1:length(Ks)
      clf(h)
      ax1 = subplot(2, 1, 1);
      hold on;
      for j = 1:length(Fs)
        plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
      end
      for j = 1:length(Ms)
        plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
      end
      for j = 1:length(Rz_rpm)
        plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
      end
      plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
      plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
      hold off;
      set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
      ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
      ylim([1e-10, 1e-1]);
      title(sprintf('$k = %.0e$ [N/m]', Ks(i)))

      ax2 = subplot(2, 1, 2);
      hold on;
      for j = 1:length(Rz_rpm)
        plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
      end
      for j = 1:length(Fs)
        plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
      end
      for j = 1:length(Ms)
        plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
      end
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
      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');
      xlim([freqs(1), freqs(end)]);

      set(h, 'visible', 'off');
      set(h, 'pos', [0, 0, 1200, 800]);
      drawnow;

      % Capture the plot as an image
      frame = getframe(h);
      im = frame2im(frame);
      [imind,cm] = rgb2ind(im,256);

      % Write to the GIF File
      if i == 1
          imwrite(imind,cm,filename,'gif','DelayTime',1.0,'Loopcount',inf);
      else
          imwrite(imind,cm,filename,'gif','DelayTime',1.0,'WriteMode','append');
      end
  end

  set(h, 'visible', 'on');
  ans = filename;
#+end_src

#+NAME: fig:opt_stiffness_plant_dynamics_task_space
#+CAPTION: Variability of the dynamics from $\mathcal{F}_x$ to $\mathcal{X}_x$ with varying nano-hexapod stiffness
#+RESULTS: fig:opt_stiffness_plant_dynamics_task_space
[[file:figs/opt_stiffness_plant_dynamics_task_space.gif]]

#+NAME: fig:opt_stiffness_plant_dynamics_task_space_colors
#+HEADER: :tangle no :exports results :results value file raw replace :noweb yes
#+begin_src matlab
  h = figure;
  filename = 'figs/opt_stiffness_plant_dynamics_task_space_colors.gif';

  for i = 1:length(Ks)
      clf(h)
      ax1 = subplot(2, 1, 1);
      hold on;
      for j = 1:length(Fs)
          set(gca,'ColorOrderIndex',1);
          plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-');
      end
      for j = 1:length(Ms)
          set(gca,'ColorOrderIndex',2);
          plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-');
      end
      for j = 2:length(Rz_rpm)
          set(gca,'ColorOrderIndex',3);
          plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-');
      end
      set(gca,'ColorOrderIndex',4);
      plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-');
      plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-');
      hold off;
      set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
      ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
      ylim([1e-10, 1e-1]);
      title(sprintf('$k = %.0e$ [N/m]', Ks(i)))

      ax2 = subplot(2, 1, 2);
      hold on;
      for j = 1:length(Fs)
          set(gca,'ColorOrderIndex',1);
          plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-');
      end
      for j = 1:length(Ms)
          set(gca,'ColorOrderIndex',2);
          plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-');
      end
      for j = 1:length(Rz_rpm)
          set(gca,'ColorOrderIndex',3);
          plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-');
      end
      set(gca,'ColorOrderIndex',4);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz')))), '-');
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz')))), '-');
      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');
      xlim([freqs(1), freqs(end)]);

      set(h, 'visible', 'off');
      set(h, 'pos', [0, 0, 1200, 800]);
      drawnow;

      % Capture the plot as an image
      frame = getframe(h);
      im = frame2im(frame);
      [imind,cm] = rgb2ind(im,256);

      % Write to the GIF File
      if i == 1
          imwrite(imind,cm,filename,'gif','DelayTime',1.0,'Loopcount',inf);
      else
          imwrite(imind,cm,filename,'gif','DelayTime',1.0,'WriteMode','append');
      end
  end

  set(h, 'visible', 'on');
  ans = filename;
#+end_src

#+NAME: fig:opt_stiffness_plant_dynamics_task_space_colors
#+CAPTION: Variability of the dynamics from $\mathcal{F}_x$ to $\mathcal{X}_x$ with varying nano-hexapod stiffness
#+RESULTS: fig:opt_stiffness_plant_dynamics_task_space_colors
[[file:figs/opt_stiffness_plant_dynamics_task_space_colors.gif]]