#+TITLE: Determination of the optimal nano-hexapod's stiffness
:DRAWER:
#+STARTUP: overview

#+LANGUAGE: en
#+EMAIL: dehaeze.thomas@gmail.com
#+AUTHOR: Dehaeze Thomas

#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/zenburn.css"/>
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/bootstrap.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/readtheorg.js"></script>

#+HTML_MATHJAX: align: center tagside: right font: TeX

#+PROPERTY: header-args:matlab  :session *MATLAB*
#+PROPERTY: header-args:matlab+ :comments org
#+PROPERTY: header-args:matlab+ :results none
#+PROPERTY: header-args:matlab+ :exports both
#+PROPERTY: header-args:matlab+ :eval no-export
#+PROPERTY: header-args:matlab+ :output-dir figs
#+PROPERTY: header-args:matlab+ :tangle no
#+PROPERTY: header-args:matlab+ :mkdirp yes

#+PROPERTY: header-args:shell  :eval no-export

#+PROPERTY: header-args:latex  :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/org/}{config.tex}")
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
#+PROPERTY: header-args:latex+ :eval no-export
#+PROPERTY: header-args:latex+ :exports both
#+PROPERTY: header-args:latex+ :mkdirp yes
#+PROPERTY: header-args:latex+ :output-dir figs
:END:

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


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

The worst case scenario is a rotation speed of 60rpm with a payload mass of 1Kg.
#+begin_src matlab
  initializeSample('mass', 10);
#+end_src

We don't include gravity nor disturbances in this model as it adds complexity to the simulations and does not alter the obtained dynamics.
#+begin_src matlab
  initializeSimscapeConfiguration('gravity', false);
  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
  initializeController('type', 'stabilizing');
  initializeLoggingConfiguration('log', 'none');
#+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 not rotating
We set the range of stiffness that we want to use.
#+begin_src matlab
  Ks = logspace(3,9,7)
#+end_src

#+begin_src matlab
  initializeReferences();
#+end_src

#+begin_src matlab
  Gk_iff = {zeros(length(Ks))};
  Gk_dvf = {zeros(length(Ks))};
  Gk_err = {zeros(length(Ks))};
#+end_src

#+begin_src matlab
  for i = 1:length(Ks)
    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_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
    Gk_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};

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

** Identification when rotating at maximum speed
#+begin_src matlab
  Rz_rpm = 60;

  initializeReferences('Rz_type', 'rotating', ...
                       'Rz_period', 60/Rz_rpm, ... % Rotation period [s]
                       'Rz_amplitude', -0.2*(2*pi*Rz_rpm/60)); % Angle offset [rad]

  load('mat/nass_references.mat', 'Rz'); % We load the reference for the Spindle
  [~, i_end] = min(abs(Rz.signals.values)); % Obtain the indice where the spindle angle is zero
  t_sim = Rz.time(i_end); % Simulation time before identification [s]
#+end_src

#+begin_src matlab
  k_sta = -1e8;
#+end_src

#+begin_src matlab
  Gk_wz_iff = {zeros(length(Ks))};
  Gk_wz_dvf = {zeros(length(Ks))};
  Gk_wz_err = {zeros(length(Ks))};
#+end_src

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

    %% Run the linearization
    G = linearize(mdl, io, t_sim);
    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) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
    Gk_wz_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};

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

#+begin_src matlab
  save('mat/optimal_stiffness_Gk_wz.mat', 'Ks', ...
       'Gk_iff',    'Gk_dvf',    'Gk_err', ...
       '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

Change of dynamics for decentralized IFF control.
#+begin_src matlab :exports none
  freqs = logspace(-1, 3, 1000);

  figure;

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

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

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

Change of dynamics from $F_x$ to $D_x$.
#+begin_src matlab :exports none
  freqs = logspace(-1, 3, 1000);

  figure;

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

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

Change of dynamics from $F_z$ to $D_z$.
#+begin_src matlab :exports none
  freqs = logspace(-1, 3, 1000);

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Gk_err)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gk_err{i}(   'Ez', 'Fz'), freqs, 'Hz'))), '-');
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Soft Nano-Hexapod');

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:length(Gk_err)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '-', ...
         'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
    set(gca,'ColorOrderIndex',i);
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_wz_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([-180, 180]);
  yticks([-180, -90, 0, 90, 180]);
  legend('location', 'northeast');

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

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

Change of coupling from $F_x$ to $D_y$ when not rotating and when rotating at 60rpm.
#+begin_src matlab :exports none
  freqs = logspace(-1, 3, 1000);

  figure;

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

Comparison of the coupling from $F_x$ to $D_y$ when rotating at 60rpm to the direct term $F_x$ to $D_x$.
#+begin_src matlab :exports none
  freqs = logspace(-1, 3, 1000);

  figure;

  hold on;
  for i = 1:length(Gk_err)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('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}('Ey', 'Fx'), freqs, 'Hz'))), '--', ...
         'HandleVisibility', 'off');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);
  legend('location', 'northeast');
#+end_src

** 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
  open('nass_model.slx')
#+end_src

** Conclusion                                                        :ignore:
* 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
  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 don't include gravity nor disturbances in this model as it adds complexity to the simulations and does not alter the obtained dynamics.
#+begin_src matlab
  initializeSimscapeConfiguration('gravity', false);
  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
  initializeController('type', 'stabilizing');
  initializeLoggingConfiguration('log', 'none');
#+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

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

#+begin_src matlab
  initializeReferences();
  Ks = logspace(3,9,7) % [N/m]
#+end_src

#+begin_src matlab
  Ms = [1, 20, 50]; % [Kg]
#+end_src

#+begin_src matlab
  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
  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.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

#+begin_src matlab
  Fs = [50, 200, 500]; % [Hz]
#+end_src

#+begin_src matlab
  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
  for i = 1:length(Ks)
    for j = 1:length(Fs)
      initializeNanoHexapod('k', Ks(i));
      initializeSample('mass', 20, '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.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
  save('mat/optimal_stiffness_Gm_Gf.mat', 'Ks', 'Ms', 'Fs', ...
       'Gm_iff',    'Gm_dvf',    'Gm_err', ...
       'Gf_iff',    'Gf_dvf',    'Gf_err');
#+end_src

** Plots
** Change of optimal gain for decentralized control
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
  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*angle(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
            'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
      else
        plot(freqs, 180/pi*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([-180, 180]);
  yticks([-180, -90, 0, 90, 180]);
  legend('location', 'northeast');

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

Change of payload resonance frequency
#+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*angle(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
            'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
      else
        plot(freqs, 180/pi*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([-180, 180]);
  yticks([-180, -90, 0, 90, 180]);
  legend('location', 'northeast');

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

** Change of dynamics for the primary controller
For each stiffness, plot the total spread of dynamics.

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

*** Frequency variation
Same payload mass, but different stiffness resulting in different resonance frequency.

All curves
#+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
  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*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
            'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
      else
        plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
             'HandleVisibility', 'off');
      end
    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]);
  legend('location', 'northeast');

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

X direction
#+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'))), '-');
  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*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), 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([-180, 180]);
  yticks([-180, -90, 0, 90, 180]);
  legend('location', 'northeast');

  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'))), '-');
  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*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), 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([-180, 180]);
  yticks([-180, -90, 0, 90, 180]);
  legend('location', 'northeast');

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

Z direction:
We can see two mass lines for the soft nano-hexapod:
- The first mass line corresponds to $\frac{1}{(m_n + m_p)s^2}$ where $m_p = 20\ [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}$

#+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*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([-180, 180]);
  yticks([-180, -90, 0, 90, 180]);
  legend('location', 'northeast');

  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}('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, 4);
  hold on;
  for j = 1:length(Fs)
    plot(freqs, 180/pi*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([-180, 180]);
  yticks([-180, -90, 0, 90, 180]);
  legend('location', 'northeast');

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

*** Mass variation
All mixed, X direction
#+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}('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(Ms)
      set(gca,'ColorOrderIndex',i);
      if j == 1
        plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
            'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
      else
        plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
             'HandleVisibility', 'off');
      end
    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]);
  legend('location', 'northeast');

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

All mixed, Z direction
#+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*angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-', ...
            'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
      else
        plot(freqs, 180/pi*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([-180, 180]);
  yticks([-180, -90, 0, 90, 180]);
  legend('location', 'northeast');

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

Z direction
#+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*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([-180, 180]);
  yticks([-180, -90, 0, 90, 180]);

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

  i = 7;

  ax1 = 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)))

  ax2 = subplot(2, 2, 4);
  hold on;
  for j = 1:length(Ms)
      plot(freqs, 180/pi*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([-180, 180]);
  yticks([-180, -90, 0, 90, 180]);
  legend('location', 'northeast');

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

*** Total variation
Total change of dynamics due to change of the payload:
- mass from 1kg to 50kg
- main resonance from 50Hz to 500Hz

For a soft nano-hexapod
#+begin_src matlab :exports none
  i = 1;

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

  figure;

  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
  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 j = 1:length(Fs)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
  for j = 1:length(Ms)
    plot(freqs, 180/pi*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([-180, 180]);
  yticks([-180, -90, 0, 90, 180]);

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

For a stiff nano-hexapod
#+begin_src matlab :exports none
  i = 7;

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

  figure;

  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
  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 j = 1:length(Fs)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
  for j = 1:length(Ms)
    plot(freqs, 180/pi*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([-180, 180]);
  yticks([-180, -90, 0, 90, 180]);

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


** Conclusion                                                        :ignore: