#+TITLE: Study of the Flexible Joints
#+SETUPFILE: ./setup/org-setup-file.org

* Introduction                                                        :ignore:
In this document is studied the effect of the mechanical behavior of the flexible joints that are located the extremities of each nano-hexapod's legs.

Ideally, we want the x and y rotations to be free and all the translations to be blocked.
However, this is never the case and be have to consider:
- Finite x and y rotational stiffnesses (Section [[sec:rot_stiffness]])
- Translation stiffness in the direction of the legs (Section [[sec:trans_stiffness]])

This may impose some limitations, also, the goal is to specify the required joints stiffnesses.

* Rotational Stiffness
<<sec:rot_stiffness>>

** Introduction                                                      :ignore:
In this section, we wish to study the effect of the rotation flexibility of the nano-hexapod joints.

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

#+begin_src matlab :exports none
  initializeSimscapeConfiguration();
  initializeDisturbances('enable', false);
  initializeLoggingConfiguration('log', 'none');

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

Let's consider the heaviest mass which should we the most problematic with it comes to the flexible joints.
#+begin_src matlab
  initializeSample('mass', 50, 'freq', 200*ones(6,1));
  initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', 60);
#+end_src

#+begin_src matlab :exports none
  K = tf(zeros(6));
  Kdvf = tf(zeros(6));
#+end_src

** Realistic Rotational Stiffness Values
*** Introduction                                                    :ignore:
Let's compare the ideal case (zero stiffness in rotation and infinite stiffness in translation) with a more realistic case:
- $K_{\theta, \phi} = 15\,[Nm/rad]$ stiffness in flexion
- $K_{\psi} = 20\,[Nm/rad]$ stiffness in torsion

#+begin_src matlab
  Kf_M = 15*ones(6,1);
  Kt_M = 20*ones(6,1);
  Kf_F = 15*ones(6,1);
  Kt_F = 20*ones(6,1);
#+end_src

The stiffness and damping of the nano-hexapod's legs are:
#+begin_src matlab
  k = 1e5; % [N/m]
  c = 2e2; % [N/(m/s)]
#+end_src

*** Direct Velocity Feedback
We identify the dynamics from actuators force $\tau_i$ to relative motion sensors $d\mathcal{L}_i$ with and without considering the flexible joint stiffness.

The obtained dynamics are shown in Figure [[fig:flex_joint_rot_dvf]].
It is shown that the adding of stiffness for the flexible joints does increase a little bit the frequencies of the mass suspension modes. It stiffen the structure.

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

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

#+begin_src matlab :exports none
  initializeNanoHexapod('k', k, 'c', c, ...
                        'type_F', 'universal_p', ...
                        'type_M', 'spherical_p');

  G_dvf_p = linearize(mdl, io);
  G_dvf_p.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G_dvf_p.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
#+end_src

#+begin_src matlab :exports none
  initializeNanoHexapod('k', k, 'c', c, ...
                        'type_F', 'universal', ...
                        'type_M', 'spherical', ...
                        'Kf_M', Kf_M, ...
                        'Kt_M', Kt_M, ...
                        'Kf_F', Kf_F, ...
                        'Kt_F', Kt_F);

  G_dvf = linearize(mdl, io);
  G_dvf.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
#+end_src

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(G_dvf(1, 1), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(G_dvf_p(1, 1), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf(1, 1), freqs, 'Hz')))), ...
       'DisplayName', 'Flexible Joints');
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_p(1, 1), freqs, 'Hz')))), ...
       'DisplayName', 'Perfect Joints');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);
  legend('location', 'northeast');

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

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

#+name: fig:flex_joint_rot_dvf
#+caption: Dynamics from actuators force $\tau_i$ to relative motion sensors $d\mathcal{L}_i$ with (blue) and without (red) considering the flexible joint stiffness
#+RESULTS:
[[file:figs/flex_joint_rot_dvf.png]]

*** Primary Plant
#+begin_src matlab :exports none
  Kdvf = 5e3*s/(1+s/2/pi/1e3)*eye(6);
#+end_src

Let's now identify the dynamics from $\bm{\tau}^\prime$ to $\bm{\epsilon}_{\mathcal{X}_n}$ (for the primary controller designed in the frame of the legs).

The dynamics is compare with and without the joint flexibility in Figure [[fig:flex_joints_rot_primary_plant_L]].
The plant dynamics is not found to be changing significantly.

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

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

  load('mat/stages.mat', 'nano_hexapod');
#+end_src

#+begin_src matlab :exports none
  initializeNanoHexapod('k', k, 'c', c, ...
                        'type_F', 'universal_p', ...
                        'type_M', 'spherical_p');

  G_p = linearize(mdl, io);
  G_p.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G_p.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

  Gx_p = -G_p*inv(nano_hexapod.kinematics.J');
  Gx_p.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};

  Gl_p = -nano_hexapod.kinematics.J*G_p;
  Gl_p.OutputName  = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
#+end_src

#+begin_src matlab :exports none
  initializeNanoHexapod('k', k, 'c', c, ...
                        'type_F', 'universal', ...
                        'type_M', 'spherical', ...
                        'Kf_M', Kf_M, ...
                        'Kt_M', Kt_M, ...
                        'Kf_F', Kf_F, ...
                        'Kt_F', Kt_F);

  G = linearize(mdl, io);
  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

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

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

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for j = 1:6
      set(gca,'ColorOrderIndex',1);
      plot(freqs, abs(squeeze(freqresp(Gl(j, j), freqs, 'Hz'))));
  end
  for j = 1:6
      set(gca,'ColorOrderIndex',2);
      plot(freqs, abs(squeeze(freqresp(Gl_p(j, j), 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 j = 1:6
      set(gca,'ColorOrderIndex',1);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gl(j, j), freqs, 'Hz')))));
  end
  for j = 1:6
      set(gca,'ColorOrderIndex',2);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gl_p(j, j), 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');
#+end_src

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

#+name: fig:flex_joints_rot_primary_plant_L
#+caption: Dynamics from $\bm{\tau}^\prime_i$ to $\bm{\epsilon}_{\mathcal{X}_n,i}$ with perfect joints (dashed) and with flexible joints (solid)
#+RESULTS:
[[file:figs/flex_joints_rot_primary_plant_L.png]]

*** Conclusion
#+begin_important
Considering realistic flexible joint rotational stiffness for the nano-hexapod does not seems to impose any limitation on the DVF control nor on the primary control.

It only increases a little bit the suspension modes of the sample on top of the nano-hexapod.
#+end_important

** Parametric Study
*** Introduction                                                    :ignore:
We wish now to see what is the impact of the rotation stiffness of the flexible joints on the dynamics.
This will help to determine the requirements on the joint's stiffness.

Let's consider the following rotational stiffness of the flexible joints:
#+begin_src matlab
  Ks = [1, 10, 100]; % [Nm/rad]
#+end_src

We also consider here a nano-hexapod with the identified optimal actuator stiffness.

#+begin_src matlab :exports none
  K = tf(zeros(6));
  Kdvf = tf(zeros(6));
#+end_src

*** Direct Velocity Feedback
The dynamics from the actuators to the relative displacement sensor in each leg is identified and shown in Figure [[fig:flex_joints_rot_study_dvf]].

The corresponding Root Locus plot is shown in Figure [[fig:flex_joints_rot_study_dvf_root_locus]].

It is shown that the rotational stiffness of the flexible joints does indeed change a little the dynamics, but critical damping is stiff achievable with Direct Velocity Feedback.

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

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

  Gdvf_s  = {zeros(length(Ks), 1)};

  for i = 1:length(Ks)
      initializeNanoHexapod('k', k, 'c', c, ...
                            'type_F', 'universal', ...
                            'type_M', 'spherical', ...
                            'Kf_M', Ks(i), ...
                            'Kt_M', Ks(i), ...
                            'Kf_F', Ks(i), ...
                            'Kt_F', Ks(i), ...
                            'Cf_M', 0.2*sqrt(Ks(i)*1), ...
                            'Ct_M', 0.2*sqrt(Ks(i)*1), ...
                            'Cf_F', 0.2*sqrt(Ks(i)*1), ...
                            'Ct_F', 0.2*sqrt(Ks(i)*1));

      Gdvf = linearize(mdl, io);
      G_dvf.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
      G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};

      Gdvf_s(i) = {Gdvf};
  end
#+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)
      plot(freqs, abs(squeeze(freqresp(Gdvf_s{i}(1, 1), freqs, 'Hz'))));
  end
  plot(freqs, abs(squeeze(freqresp(G_dvf_p(1, 1), freqs, 'Hz'))), 'k--');
  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)
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gdvf_s{i}(1, 1), freqs, 'Hz')))), ...
           'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
  end
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_p(1, 1), freqs, 'Hz')))), 'k--', ...
       'DisplayName', 'Ideal Joint');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);
  legend('location', 'southwest');

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

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

#+name: fig:flex_joints_rot_study_dvf
#+caption: Dynamics from $\tau_i$ to $d\mathcal{L}_i$ for all the considered Rotation Stiffnesses
#+RESULTS:
[[file:figs/flex_joints_rot_study_dvf.png]]

#+begin_src matlab :exports none
  figure;

  gains = logspace(2, 5, 300);

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

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

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

#+name: fig:flex_joints_rot_study_dvf_root_locus
#+caption: Root Locus for all the considered Rotation Stiffnesses
#+RESULTS:
[[file:figs/flex_joints_rot_study_dvf_root_locus.png]]

*** Primary Control
#+begin_src matlab
  Kdvf = 5e3*s/(1+s/2/pi/1e3)*eye(6);
#+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, 'input');            io_i = io_i + 1; % Actuator Inputs
  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror

  load('mat/stages.mat', 'nano_hexapod');

  Gx_3dof_s = {zeros(length(Ks), 1)};
  Gl_3dof_s = {zeros(length(Ks), 1)};

  for i = 1:length(Ks)
      initializeNanoHexapod('k', k, 'c', c, ...
                            'type_F', 'universal', ...
                            'type_M', 'spherical', ...
                            'Kf_M', Ks(i), ...
                            'Kt_M', 20, ...
                            'Kf_F', Ks(i), ...
                            'Kt_F', 20, ...
                            'Cf_M', 0, ...
                            'Ct_M', 0, ...
                            'Cf_F', 0, ...
                            'Ct_F', 0);

      G = linearize(mdl, io);
      G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
      G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

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

      Gl_3dof = -nano_hexapod.kinematics.J*G;
      Gl_3dof.OutputName  = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
      Gl_3dof_s(i) = {Gl_3dof};
  end
#+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)
      plot(freqs, abs(squeeze(freqresp(Gl_3dof_s{i}(1, 1), freqs, 'Hz'))));
  end
  plot(freqs, abs(squeeze(freqresp(Gl_p(1, 1), freqs, 'Hz'))), 'k--');
  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)
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gl_3dof_s{i}(1, 1), freqs, 'Hz')))), ...
           'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
  end
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gl_p(1, 1), freqs, 'Hz')))), 'k--', ...
       'DisplayName', 'Ideal Joint');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);
  legend('location', 'southwest');

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

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

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

*** Conclusion
#+begin_important

#+end_important

* Translation Stiffness
<<sec:trans_stiffness>>

** Introduction                                                      :ignore:
Let's know consider a flexibility in translation of the flexible joint.

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

#+begin_src matlab :exports none
  initializeSimscapeConfiguration();
  initializeDisturbances('enable', false);
  initializeLoggingConfiguration('log', 'none');

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

Let's consider the heaviest mass which should we the most problematic with it comes to the flexible joints.
#+begin_src matlab
  initializeSample('mass', 50, 'freq', 200*ones(6,1));
  initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', 60);
#+end_src

#+begin_src matlab :exports none
  K = tf(zeros(6));
  Kdvf = tf(zeros(6));
#+end_src

** Direct Velocity Feedback
#+begin_src matlab
  Kz_F = 60e6*ones(6,1); % [N/m]
  Kz_M = 60e6*ones(6,1); % [N/m]
  Cz_F = 1e2*ones(6,1); % [N/m]
  Cz_M = 1e2*ones(6,1); % [N/m]
#+end_src

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

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

#+begin_src matlab :exports none
  initializeNanoHexapod('k', 1e5, 'c', 2e2, ...
                        'type_F', 'universal_3dof', ...
                        'type_M', 'spherical_3dof', ...
                        'Kz_F', Kz_F, ...
                        'Kz_M', Kz_M, ...
                        'Cz_F', Cz_F, ...
                        'Cz_M', Cz_M);

  G_dvf_3dof = linearize(mdl, io);
  G_dvf_3dof.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G_dvf_3dof.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
#+end_src

#+begin_src matlab :exports none
  initializeNanoHexapod('k', 1e5, 'c', 2e2, ...
                        'type_F', 'universal', ...
                        'type_M', 'spherical');

  G_dvf = linearize(mdl, io);
  G_dvf.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
#+end_src

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(G_dvf(1, 1), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(G_dvf_3dof(1, 1), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf(1, 1), freqs, 'Hz')))), ...
       'DisplayName', '2dof Joints');
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_3dof(1, 1), freqs, 'Hz')))), ...
       'DisplayName', '3dof Joints');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);
  legend('location', 'northeast');

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

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

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

** Primary Plant
#+begin_src matlab
  Kdvf = 5e3*s/(1+s/2/pi/1e3)*eye(6);
#+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, 'input');            io_i = io_i + 1; % Actuator Inputs
  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror

  load('mat/stages.mat', 'nano_hexapod');
#+end_src

#+begin_src matlab :exports none
  initializeNanoHexapod('k', 1e5, 'c', 2e2, ...
                        'type_F', 'universal_3dof', ...
                        'type_M', 'spherical_3dof', ...
                        'Kz_F', Kz_F, ...
                        'Kz_M', Kz_M, ...
                        'Cz_F', Cz_F, ...
                        'Cz_M', Cz_M);

  G_3dof = linearize(mdl, io);
  G_3dof.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G_3dof.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

  Gx_3dof = -G_3dof*inv(nano_hexapod.kinematics.J');
  Gx_3dof.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};

  Gl_3dof = -nano_hexapod.kinematics.J*G_3dof;
  Gl_3dof.OutputName  = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
#+end_src

#+begin_src matlab :exports none
  initializeNanoHexapod('k', 1e5, 'c', 2e2, ...
                        'type_F', 'universal', ...
                        'type_M', 'spherical');

  G = linearize(mdl, io);
  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

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

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

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

  figure;

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

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

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

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

** Parametric study
*** Introduction                                                    :ignore:
#+begin_src matlab
  Kzs = [1e4, 1e5, 1e6, 1e7, 1e8, 1e9]; % [N/m]
#+end_src

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

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

#+begin_src matlab :exports none
  Gdvf_3dof_s  = {zeros(length(Kzs), 1)};

  for i = 1:length(Kzs)
      initializeNanoHexapod('k', 1e5, 'c', 2e2, ...
                            'type_F', 'universal_3dof', ...
                            'type_M', 'spherical_3dof', ...
                            'Kz_F', Kzs(i), ...
                            'Kz_M', Kzs(i), ...
                            'Cz_F', 0.2*sqrt(Kzs(i)*10), ...
                            'Cz_M', 0.2*sqrt(Kzs(i)*10));

      G = linearize(mdl, io);
      G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
      G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
   
      Gdvf_3dof_s(i) = {G};
  end
#+end_src

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Kzs)
    plot(freqs, abs(squeeze(freqresp(Gdvf_3dof_s{i}(1, 1), freqs, 'Hz'))));
  end
  plot(freqs, abs(squeeze(freqresp(Gdvf(1, 1), freqs, 'Hz'))), 'k--');
  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(Kzs)
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gdvf_3dof_s{i}(1, 1), freqs, 'Hz')))), ...
         'DisplayName', sprintf('$k = %.0g$ [N/m]', Kzs(i)));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gdvf(1, 1), freqs, 'Hz')))), 'k--', ...
         'DisplayName', 'Perfect Joint');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);
  legend('location', 'northeast');

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

*** Primary Control
#+begin_src matlab
  Kdvf = 5e3*s/(1+s/2/pi/1e3)*eye(6);
#+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, 'input');            io_i = io_i + 1; % Actuator Inputs
  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror

  load('mat/stages.mat', 'nano_hexapod');
#+end_src

#+begin_src matlab :exports none
  Gx_3dof_s = {zeros(length(Kzs), 1)};
  Gl_3dof_s = {zeros(length(Kzs), 1)};

  for i = 1:length(Kzs)
      initializeNanoHexapod('k', 1e5, 'c', 2e2, ...
                            'type_F', 'universal_3dof', ...
                            'type_M', 'spherical_3dof', ...
                            'Kz_F', Kzs(i), ...
                            'Kz_M', Kzs(i), ...
                            'Cz_F', 0.2*sqrt(Kzs(i)*10), ...
                            'Cz_M', 0.2*sqrt(Kzs(i)*10));

      G = linearize(mdl, io);
      G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
      G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

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

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

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

  figure;

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

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

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

** Conclusion
#+begin_important

#+end_important