#+TITLE: Simscape Model - Nano Active Stabilization System
:DRAWER:
#+LANGUAGE: en
#+EMAIL: dehaeze.thomas@gmail.com
#+AUTHOR: Dehaeze Thomas

#+HTML_LINK_HOME: ../index.html
#+HTML_LINK_UP:   ../index.html

#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/>
#+HTML_HEAD: <script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>

#+BIND: org-latex-image-default-option "scale=1"
#+BIND: org-latex-image-default-width ""

#+LaTeX_CLASS: scrreprt
#+LaTeX_CLASS_OPTIONS: [a4paper, 10pt, DIV=12, parskip=full, bibliography=totoc]
#+LATEX_HEADER: \input{preamble.tex}
#+LATEX_HEADER_EXTRA: \input{preamble_extra.tex}
#+LATEX_HEADER_EXTRA: \bibliography{simscape-nass.bib}

#+BIND: org-latex-bib-compiler "biber"

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

#+PROPERTY: header-args:latex  :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/tikz/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 file raw replace
#+PROPERTY: header-args:latex+ :buffer no
#+PROPERTY: header-args:latex+ :tangle no
#+PROPERTY: header-args:latex+ :eval no-export
#+PROPERTY: header-args:latex+ :exports results
#+PROPERTY: header-args:latex+ :mkdirp yes
#+PROPERTY: header-args:latex+ :output-dir figs
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
:END:

#+latex: \clearpage

* Build                                                             :noexport:
#+NAME: startblock
#+BEGIN_SRC emacs-lisp :results none :tangle no
(add-to-list 'org-latex-classes
             '("scrreprt"
               "\\documentclass{scrreprt}"
               ("\\chapter{%s}" . "\\chapter*{%s}")
               ("\\section{%s}" . "\\section*{%s}")
               ("\\subsection{%s}" . "\\subsection*{%s}")
               ("\\paragraph{%s}" . "\\paragraph*{%s}")
               ))


;; Remove automatic org heading labels
(defun my-latex-filter-removeOrgAutoLabels (text backend info)
  "Org-mode automatically generates labels for headings despite explicit use of `#+LABEL`. This filter forcibly removes all automatically generated org-labels in headings."
  (when (org-export-derived-backend-p backend 'latex)
    (replace-regexp-in-string "\\\\label{sec:org[a-f0-9]+}\n" "" text)))
(add-to-list 'org-export-filter-headline-functions
             'my-latex-filter-removeOrgAutoLabels)

;; Remove all org comments in the output LaTeX file
(defun delete-org-comments (backend)
  (loop for comment in (reverse (org-element-map (org-element-parse-buffer)
                                    'comment 'identity))
        do
        (setf (buffer-substring (org-element-property :begin comment)
                                (org-element-property :end comment))
              "")))
(add-hook 'org-export-before-processing-hook 'delete-org-comments)

;; Use no package by default
(setq org-latex-packages-alist nil)
(setq org-latex-default-packages-alist nil)

;; Do not include the subtitle inside the title
(setq org-latex-subtitle-separate t)
(setq org-latex-subtitle-format "\\subtitle{%s}")

(setq org-export-before-parsing-hook '(org-ref-glossary-before-parsing
                                       org-ref-acronyms-before-parsing))
#+END_SRC

* Notes                                                             :noexport:
** Notes
Prefix is =nass=

The goals of this report are:
- [X] ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/positioning_error.org][positioning_error]]): Explain how the NASS control is made (computation of the wanted position, measurement of the sample position, computation of the errors)
- [X] ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/uncertainty_experiment.org][uncertainty_experiment]]): Effect of experimental conditions on the plant (payload mass, Ry position, Rz position, Rz velocity, etc...)
- [ ] Determination of the *optimal stiffness* for the hexapod actuators:
  - [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/uncertainty_optimal_stiffness.org][uncertainty_optimal_stiffness]]
  - [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/optimal_stiffness_disturbances.org][optimal_stiffness_disturbances]]
  - [ ] [[file:~/Cloud/work-projects/ID31-NASS/documents/state-of-thesis-2020/index.org][state-of-thesis-2020]]
  - [ ] [[file:/home/thomas/Cloud/meetings/group-meetings-me/2020-04-06-NASS-Design/2020-04-06-NASS-Design.org][group-meeting-optimal-stiffness]]
  Should this be in this report? *This should be in chapter 2*
- [X] Explain why HAC-LAC strategy is nice (*It was already explained in uniaxial model*)
  - [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/control.org][different control architectures]]
  - [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/control-vibration-isolation.org][hexapod - vibration isolation]]
- [X] How to apply/optimize IFF on an hexapod? ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/control_active_damping.org][control_active_damping]], [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/control-active-damping.org][active damping for stewart platforms]])
- [X] ([[file:~/Cloud/research/matlab/decoupling-strategies/svd-control.org][decoupling-strategies]]): Decoupling strategies for HAC? (maybe also in previous report)
  *Will be in chapter 2*
- [X] Validation of the concept using simulations:
  - [X] Find where this simulation in OL/CL is made (maybe for the conference?)
    It was re-made for micro-station validation. Will just have to do the same simulation but with nano-hexapod in closed-loop
  - Tomography experiment (maybe also Ty scans)
  - Open VS Closed loop results
  - *Conclusion*: concept validation
    nano hexapod architecture with APA
    decentralized IFF + centralized HAC

- In this section simple control (in the frame of the struts)
- Justify future used control architecture (control in the frame of the struts? Need to check what was done in ID31 tests)
- Table that compares different approaches (specify performances in different DoF, same plans on the diagonal, etc...)
- Literature review about Stewart platform control?
*In chapter 2: Special section about MIMO control, complementary filters, etc...*

** Outline
*** Control Kinematics

- Explain how the position error can be expressed in the frame of the nano-hexapod
- ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/positioning_error.org][positioning_error]]): Explain how the NASS control is made (computation of the wanted position, measurement of the sample position, computation of the errors)
- Control architecture, block diagram

*** LAC

- How to apply/optimize IFF on an hexapod? ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/control_active_damping.org][control_active_damping]], [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/control-active-damping.org][active damping for stewart platforms]])
- Robustness to payload mass
- Root Locus
- Damping optimization

*** HAC

- ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/uncertainty_experiment.org][uncertainty_experiment]]): Effect of experimental conditions on the plant (payload mass, Ry position, Rz position, Rz velocity, etc...)
- Determination of the *optimal stiffness* for the hexapod actuators:
  - [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/uncertainty_optimal_stiffness.org][uncertainty_optimal_stiffness]]
  - [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/optimal_stiffness_disturbances.org][optimal_stiffness_disturbances]]
  - [ ] [[file:~/Cloud/work-projects/ID31-NASS/documents/state-of-thesis-2020/index.org][state-of-thesis-2020]]
  - [ ] [[file:/home/thomas/Cloud/meetings/group-meetings-me/2020-04-06-NASS-Design/2020-04-06-NASS-Design.org][group-meeting-optimal-stiffness]]
- Effect of micro-station compliance
- Effect of IFF
- Effect of payload mass
- Decoupled plant
- Controller design

*** Simulations

- Take into account disturbances, metrology sensor noise. Maybe say here that we don't take in account other noise sources as they will be optimized latter (detail design phase)
- Tomography + lateral scans (same as what was done in open loop [[file:~/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/A4-simscape-micro-station/simscape-micro-station.org::*Simulation of Scientific Experiments][here]])
- Validation of concept

** Backup
*** Sensitivity to disturbances
** DONE Old Outline
CLOSED: [2024-11-07 Thu 16:19]
*** Introduction                                                    :ignore:

Discussion of:
- Transformation matrices / control architecture (computation of the position error in the frame of the nano-hexapod)
- Control of parallel architectures
- Control in the frame of struts or cartesian?
- Effect of rotation on IFF? => APA
- HAC-LAC
- New noise budgeting?

*** Control Kinematics

- Explain how the position error can be expressed in the frame of the nano-hexapod
- block diagram
- Explain how to go from external metrology to the frame of the nano-hexapod

*** High Authority Control - Low Authority Control (HAC-LAC)

- general idea
- case for parallel manipulator: decentralized LAC + centralized HAC

*** Decoupling Strategies for parallel manipulators
[[file:~/Cloud/research/matlab/decoupling-strategies/svd-control.org::+TITLE: Diagonal control using the SVD and the Jacobian Matrix][study]]

- Jacobian matrices, CoK, CoM, ...
- Discussion of cubic architecture
- SVD, Modal, ...

*** Decentralized Integral Force Feedback (LAC)

- Root Locus
- Damping optimization

*** Decoupled Dynamics

- Centralized HAC
- Control in the frame of the struts
- Effect of IFF

*** Centralized Position Controller (HAC)

- Decoupled plant
- Controller design

*** Time domain simulations

Goal: validation of the concept
- Take into account disturbances, sensor noise, etc...
- Tomography + lateral scans (same as what was done in open loop [[file:~/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/A4-simscape-micro-station/simscape-micro-station.org::*Simulation of Scientific Experiments][here]])

** DONE [#A] Merge the micro-station model with the nano-hexapod model
CLOSED: [2025-02-12 Wed 12:10] SCHEDULED: <2025-02-12 Wed>

- [X] *Start from the Simscape model of the ID31 tests*
  =/home/thomas/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/C5-test-bench-id31/matlab/nass_model_id31.slx=
- [X] Remove LION metrology to have perfect measurement
- [X] Remove nano-hexapod model and add simplified model
- [ ] Add "cylindrical" payloads (configurable in mass)

** DONE [#B] Add payload configurable subsystem
CLOSED: [2025-02-12 Wed 14:17] SCHEDULED: <2025-02-12 Wed>

** DONE [#A] Verify formulas to have the errors in the frame of the nano-hexapod and in the frame of the granite
CLOSED: [2025-02-17 Mon 10:35] SCHEDULED: <2025-02-17 Mon>

Errors in the frame of the nano-hexapod:
\begin{equation}\label{eq:nass_transformation_error}
  \bm{T}_{\text{error}} = \bm{T}_{\mu\text{-station}}^{-1} \cdot \bm{T}_{\text{sample}}
\end{equation}

Errors in the frame of the granite:
WTe(1:3, 4, i) = WTr(1:3, 4, i) - WTm(1:3, 4, i);
WTe(1:3, 1:3, i) = WTr(1:3, 1:3, end)*WTm(1:3, 1:3, end)';

** DONE [#A] Fix IFF and HAC controllers
CLOSED: [2025-02-17 Mon 16:00] SCHEDULED: <2025-02-17 Mon>

** DONE [#A] Compute all figures
CLOSED: [2025-02-17 Mon 18:26] SCHEDULED: <2025-02-17 Mon>

** DONE [#B] Discuss the necessity of estimated Rz?
CLOSED: [2025-02-17 Mon 18:26]

One big advantage of doing the control in the cartesian plane, is that we don't need the estimation of nano-hexapod Rz, therefore we don't need the encoders anymore!

Maybe this should be done *here*.
Here it can be reminded when doing the control in the cartesian frame.

** DONE [#B] Determine which .mat files are used and which are not
CLOSED: [2025-02-17 Mon 23:04]

#+begin_src matlab :eval no :tangle no
load("nass_model_conf_log.mat");
load("nass_model_conf_simscape.mat");
dist = load("nass_model_disturbances.mat");
load("nass_model_references.mat");
load("nass_model_controller.mat");
load("nass_model_stages.mat");
J_L_to_X = inv(nano_hexapod.geometry.J);
#+end_src

- [ ] matlab/mat/conf_log.mat
- [ ] matlab/mat/conf_simscape.mat
- [ ] matlab/mat/conf_simulink.mat
- [ ] matlab/mat/nano_hexapod.mat
- [ ] matlab/mat/nass_disturbances.mat
- [X] matlab/mat/nass_model_conf_log.mat
- [X] matlab/mat/nass_model_conf_simscape.mat
- [X] matlab/mat/nass_model_controller.mat
- [X] matlab/mat/nass_model_disturbances.mat
- [X] matlab/mat/nass_model_references.mat
- [X] matlab/mat/nass_model_stages.mat
- [ ] matlab/mat/nass_references.mat
- [ ] matlab/mat/nass_stages.mat

** TODO [#B] Check all figures

- [ ] Caption
- [ ] Units
- [ ] Legend

** TODO [#B] Check all matlab files

** TODO [#B] Check if things are compatible to results of uniaxial model

** DONE [#C] Check if it would be interesting to show soft/stiff nano-hexapod plants
CLOSED: [2025-02-17 Mon 18:26]

- [ ] Would we see u-station dynamics with very stiff nano-hexapod?
- [ ] Would rotation be difficult to handle with soft nano-hexapod?

** DONE [#C] Why not plant with very stiff actuators?
CLOSED: [2025-02-17 Mon 18:26]

- [ ] Check if it is confirms that having very stiff actuators is bad
  Not much better decoupling: 10Hz of bandwidth achievable, but may have worst sensitivity to disturbances

#+begin_src matlab
%% Identify the IFF plant dynamics using the Simscape model

% Initialize each Simscape model elements
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeSimplifiedNanoHexapod();

% Initial Simscape Configuration
initializeSimscapeConfiguration('gravity', false);
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();

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

initializeSimplifiedNanoHexapod('actuator_k', 1e8, 'actuator_kp', 0, 'actuator_c', 1e2);
initializeSample('type', 'cylindrical', 'm', 1);
G_m1_iff_pz = linearize(mdl, io);
G_m1_iff_pz.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_m1_iff_pz.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
#+end_src

#+begin_src matlab :exports none :results none
%% IFF Plant - Without parallel stiffness
f = logspace(0,4,1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(f, abs(squeeze(freqresp(G_m1_iff_pz(i,j), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
plot(f, abs(squeeze(freqresp(G_m1_iff_pz(1,1), f, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $k_p = 0$')
for i = 2:6
    plot(f, abs(squeeze(freqresp(G_m1_iff_pz(i,i), f, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end
plot(f, abs(squeeze(freqresp(G_m1_iff_pz(1,2), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$f_{ni}/f_j$ - $k_p = 0$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-4, 1e1]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(f, 180/pi*unwrap(angle(squeeze(freqresp(G_m1_iff_pz(i,i), f, 'Hz')))), 'color', colors(1,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([0:45:180]);

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

#+begin_src matlab
%% Identify the IFF plant dynamics using the Simscape model

% Initialize each Simscape model elements
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeSimplifiedNanoHexapod('actuator_k', 1e8, 'actuator_kp', 0, 'actuator_c', 1e2);
initializeSample('type', 'cylindrical', 'm', 1);

% Initial Simscape Configuration
initializeSimscapeConfiguration('gravity', false);
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'input');                  io_i = io_i + 1; % Actuator Inputs [N]
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL');  io_i = io_i + 1; % Strut errors [m]

%% Identify HAC Plant without using IFF
initializeSample('type', 'cylindrical', 'm', 1);
G_m1_pz = linearize(mdl, io);
G_m1_pz.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m1_pz.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

initializeSample('type', 'cylindrical', 'm', 25);
G_m25_pz = linearize(mdl, io);
G_m25_pz.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m25_pz.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

initializeSample('type', 'cylindrical', 'm', 50);
G_m50_pz = linearize(mdl, io);
G_m50_pz.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m50_pz.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
#+end_src

#+begin_src matlab :exports none :results none
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m1_pz(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$f_{ni}/f_i$ - 1kg')
plot(freqs, abs(squeeze(freqresp(G_m25_pz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$f_{ni}/f_i$ - 25kg')
plot(freqs, abs(squeeze(freqresp(G_m50_pz(1,1), freqs, 'Hz'))), 'color', colors(3,:), ...
     'DisplayName', '$f_{ni}/f_i$ - 50kg')
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_m1_pz(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m25_pz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m50_pz(i,j), freqs, 'Hz'))), 'color', [colors(3,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
% ylim([1e-5, 1e1]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1_pz(i,i), freqs, 'Hz')))), 'color', colors(1,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_pz(i,i), freqs, 'Hz')))), 'color', colors(2,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m50_pz(i,i), freqs, 'Hz')))), 'color', colors(3,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-180:45:180]);

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

Compare with Hexapod alone:
#+begin_src matlab
%% Identify the IFF plant dynamics using the Simscape model

% Initialize each Simscape model elements
initializeGround('type', 'rigid');
initializeGranite('type', 'rigid');
initializeTy('type', 'rigid');
initializeRy('type', 'rigid');
initializeRz('type', 'rigid');
initializeMicroHexapod('type', 'rigid');
initializeSimplifiedNanoHexapod('actuator_k', 1e8, 'actuator_kp', 0, 'actuator_c', 1e2);
initializeSample('type', 'cylindrical', 'm', 25);

% Initial Simscape Configuration
initializeSimscapeConfiguration('gravity', false);
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'input');                  io_i = io_i + 1; % Actuator Inputs [N]
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL');  io_i = io_i + 1; % Strut errors [m]

%% Identify HAC Plant without using IFF
G_m25_pz_rigid = linearize(mdl, io);
G_m25_pz_rigid.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m25_pz_rigid.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
#+end_src

#+begin_src matlab :exports none :results none
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$f_{ni}/f_i$ - 25kg')
plot(freqs, abs(squeeze(freqresp(G_m25_pz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$f_{ni}/f_i$ - 25kg')
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m25_pz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
% ylim([1e-5, 1e1]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_pz_rigid(i,i), freqs, 'Hz')))), 'color', colors(1,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_pz(i,i), freqs, 'Hz')))), 'color', colors(2,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-180:45:180]);

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

** DONE [#A] Add possibility to configure the nano-hexapod to be fully rigid
CLOSED: [2025-02-12 Wed 14:46]

- Use to compare TF without the NASS

** CANC [#C] What performance metric can we use?                   :@christophe:
CLOSED: [2024-11-12 Tue 09:22]
- State "CANC"       from "QUES"       [2024-11-12 Tue 09:22]
This can be nice to have a (scalar) performance metric that can be used for optimization.

In cite:hauge04_sensor_contr_space_based_six, a (scalar) performance metric representing the 6dof transmissibility is used.

** DONE [#C] Identify the sensibility to disturbances without the nano-hexapod and save the results
CLOSED: [2024-11-07 Thu 09:20]
This can then be used to compare with obtained performance with the nano-hexapod.

This should be done in the ustation report (A4).

* Introduction

The preceding chapters have established crucial foundational elements for the development of the Nano Active Stabilization System (NASS).
The uniaxial model study demonstrated that very stiff nano-hexapod configurations should be avoided due to their high coupling with the micro-station's dynamics.
A rotating three-degree-of-freedom model revealed that soft nano-hexapod designs prove unsuitable for rotating applications due to gyroscopic effect.
To further improve the model accuracy, a multi-body model of the micro-station was developed, which was carefully tuned using experimental modal analysis.
Furthermore, a multi-body model of the nano-hexapod was created, that can then be seamlessly integrated with the micro-station model, as illustrated in Figure ref:fig:nass_simscape_model.

#+name: fig:nass_simscape_model
#+caption: 3D view of the NASS multi-body model
#+attr_latex: :options [h!tbp]
#+attr_latex: :width 0.8\linewidth
[[file:figs/nass_simscape_model.jpg]]

Building upon these foundations, this chapter presents the validation of the NASS concept.
The investigation begins with the previously established nano-hexapod model, with actuator stiffness $k_a = 1\,N/\mu m$.
A thorough examination of the control kinematics is presented in Section ref:sec:nass_kinematics, detailing how both external metrology and nano-hexapod internal sensors are utilized in the control architecture.
The control strategy is then implemented in two steps: first, the decentralized IFF is used for active damping (Section ref:sec:nass_active_damping), then a High Authority Control is develop to stabilize the sample's position in a large bandwidth (Section ref:sec:nass_hac).

The robustness of the proposed control scheme is rigorously evaluated across various operational conditions.
Particular attention is paid to system performance under changing payload masses and varying spindle rotational velocities, as these represent critical operational parameters in practical applications.

This chapter marks the conclusion of the conceptual design phase, with simulation of tomography experiments providing strong evidence for the viability of the proposed NASS architecture.
The findings presented here establish a solid foundation for subsequent detailed design and experimental validation phases.

* Control Kinematics
:PROPERTIES:
:HEADER-ARGS:matlab+: :tangle matlab/nass_1_kinematics.m
:END:
<<sec:nass_kinematics>>
** Introduction                                                      :ignore:

Figure ref:fig:nass_concept_schematic presents a schematic overview of the NASS.
This section focuses specifically on the components of the "Instrumentation and Real-Time Control" block.

#+name: fig:nass_concept_schematic
#+caption: Schematic of the Nano Active Stabilization System
#+attr_latex: :options [h!tbp]
[[file:figs/nass_concept_schematic.png]]

As established in the previous section on Stewart platforms, the proposed control strategy combines Decentralized Integral Force Feedback with a High Authority Controller performed in the frame of the struts.

For the Nano Active Stabilization System, computing the positioning errors in the frame of the struts involves three key steps.
First, the system computes the desired sample pose relative to a frame representing the point where the X-ray light is focused using micro-station kinematics, as detailed in Section ref:ssec:nass_ustation_kinematics.
Second, it measures the actual sample pose relative to the same fix frame, described in Section ref:ssec:nass_sample_pose_error.
Finally, it determines the sample pose error and maps these errors to the nano-hexapod struts, as explained in Section ref:ssec:nass_error_struts.

The complete control architecture is detailed in Section ref:ssec:nass_control_architecture.

** 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 :noweb yes
<<m-init-path>>
#+end_src

#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-simscape>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src

** Micro Station Kinematics
<<ssec:nass_ustation_kinematics>>

The micro-station kinematics enables the computation of the desired sample pose from the reference signals of each micro-station stage.
These reference signals consist of the desired lateral position $r_{D_y}$, tilt angle $r_{R_y}$, and spindle angle $r_{R_z}$.
The micro-hexapod pose is defined by six parameters: three translations ($r_{D_{\mu x}}$, $r_{D_{\mu y}}$, $r_{D_{\mu z}}$) and three rotations ($r_{\theta_{\mu x}}$, $r_{\theta_{\mu y}}$, $r_{\theta_{\mu z}}$).

Using these reference signals, the desired sample position relative to the fixed frame is expressed through the homogeneous transformation matrix $\bm{T}_{\mu\text{-station}}$, as defined in equation eqref:eq:nass_sample_ref.

\begin{equation}\label{eq:nass_sample_ref}
  \bm{T}_{\mu\text{-station}} = \bm{T}_{D_y} \cdot \bm{T}_{R_y} \cdot \bm{T}_{R_z} \cdot \bm{T}_{\mu\text{-hexapod}}
\end{equation}

\begin{equation}\label{eq:nass_ustation_matrices}
  \begin{align}
  \bm{T}_{D_y} &= \begin{bmatrix}
      1 & 0 & 0 & 0 \\
      0 & 1 & 0 & r_{D_y} \\
      0 & 0 & 1 & 0 \\
      0 & 0 & 0 & 1
  \end{bmatrix} \quad
  \bm{T}_{\mu\text{-hexapod}} =
  \left[ \begin{array}{ccc|c}
      &   &   & r_{D_{\mu x}} \\
      & \bm{R}_x(r_{\theta_{\mu x}}) \bm{R}_y(r_{\theta_{\mu y}}) \bm{R}_{z}(r_{\theta_{\mu z}}) &   & r_{D_{\mu y}} \\
      &   &   & r_{D_{\mu z}} \cr
    \hline
    0 & 0 & 0 & 1
  \end{array} \right] \\
  \bm{T}_{R_z} &= \begin{bmatrix}
      \cos(r_{R_z}) & -\sin(r_{R_z}) & 0 & 0 \\
      \sin(r_{R_z}) & \cos(r_{R_z}) & 0 & 0 \\
      0 & 0 & 1 & 0 \\
      0 & 0 & 0 & 1
  \end{bmatrix} \quad
  \bm{T}_{R_y} = \begin{bmatrix}
      \cos(r_{R_y}) & 0 & \sin(r_{R_y}) & 0 \\
      0 & 1 & 0 & 0 \\
      -\sin(r_{R_y}) & 0 & \cos(r_{R_y}) & 0 \\
      0 & 0 & 0 & 1
  \end{bmatrix}
  \end{align}
\end{equation}


** Computation of the sample's pose error
<<ssec:nass_sample_pose_error>>

The external metrology system measures the sample position relative to the fixed granite.
Due to the system's symmetry, this metrology provides measurements for five degrees of freedom: three translations ($D_x$, $D_y$, $D_z$) and two rotations ($R_x$, $R_y$).

The sixth degree of freedom ($R_z$) is still required to compute the errors in the frame of the nano-hexapod struts (i.e. to compute the nano-hexapod inverse kinematics).
This $R_z$ rotation is estimated by combining measurements from the spindle encoder and the nano-hexapod's internal metrology, which consists of relative motion sensors in each strut (note that the micro-hexapod is not used for $R_z$ rotation, and is therefore ignore for $R_z$ estimation).

The measured sample pose is represented by the homogeneous transformation matrix $\bm{T}_{\text{sample}}$, as shown in equation eqref:eq:nass_sample_pose.

\begin{equation}\label{eq:nass_sample_pose}
\bm{T}_{\text{sample}} =
\left[ \begin{array}{ccc|c}
    &   &   & D_{x} \\
    & \bm{R}_x(R_{x}) \bm{R}_y(R_{y}) \bm{R}_{z}(R_{z}) &   & D_{y} \\
    &   &   & D_{z} \cr
  \hline
  0 & 0 & 0 & 1
\end{array} \right]
\end{equation}

** Position error in the frame of the struts
<<ssec:nass_error_struts>>


The homogeneous transformation formalism enables straightforward computation of the sample position error.
This computation involves the previously computed homogeneous $4 \times 4$ matrices: $\bm{T}_{\mu\text{-station}}$ representing the desired pose, and $\bm{T}_{\text{sample}}$ representing the measured pose.
Their combination yields $\bm{T}_{\text{error}}$, which expresses the position error of the sample in the frame of the rotating nano-hexapod, as shown in equation eqref:eq:nass_transformation_error.

\begin{equation}\label{eq:nass_transformation_error}
  \bm{T}_{\text{error}} = \bm{T}_{\mu\text{-station}}^{-1} \cdot \bm{T}_{\text{sample}}
\end{equation}

The known structure of the homogeneous transformation matrix facilitates efficient real-time computation of the inverse.
From $\bm{T}_{\text{error}}$, the position and orientation errors $\bm{\epsilon}_{\mathcal{X}} = [\epsilon_{D_x},\ \epsilon_{D_y},\ \epsilon_{D_z},\ \epsilon_{R_x},\ \epsilon_{R_y},\ \epsilon_{R_z}]$ of the sample are extracted using equation eqref:eq:nass_compute_errors:

\begin{equation}\label{eq:nass_compute_errors}
  \begin{align}
  \epsilon_{D_x} & = \bm{T}_{\text{error}}(1,4) \\
  \epsilon_{D_y} & = \bm{T}_{\text{error}}(2,4) \\
  \epsilon_{D_z} & = \bm{T}_{\text{error}}(3,4) \\
  \epsilon_{R_y} & = \text{atan2}(\bm{T}_{\text{error}}(1,3), \sqrt{\bm{T}_{\text{error}}(1,1)^2 + \bm{T}_{\text{error}}(1,2)^2}) \\
  \epsilon_{R_x} & = \text{atan2}(-\bm{T}_{\text{error}}(2,3)/\cos(\epsilon_{R_y}), \bm{T}_{\text{error}}(3,3)/\cos(\epsilon_{R_y})) \\
  \epsilon_{R_z} & = \text{atan2}(-\bm{T}_{\text{error}}(1,2)/\cos(\epsilon_{R_y}), \bm{T}_{\text{error}}(1,1)/\cos(\epsilon_{R_y})) \\
  \end{align}
\end{equation}

Finally, these errors are mapped to the strut space through the nano-hexapod Jacobian matrix eqref:eq:nass_inverse_kinematics.

\begin{equation}\label{eq:nass_inverse_kinematics}
  \bm{\epsilon}_{\mathcal{L}} = \bm{J} \cdot \bm{\epsilon}_{\mathcal{X}}
\end{equation}

** Control Architecture - Summary
<<ssec:nass_control_architecture>>

The complete control architecture is summarized in Figure ref:fig:nass_control_architecture.
The sample pose is measured using external metrology for five degrees of freedom, while the sixth degree of freedom (Rz) is estimated by combining measurements from the nano-hexapod encoders and spindle encoder.

The sample reference pose is determined by the reference signals of the translation stage, tilt stage, spindle, and micro-hexapod.
Position error computation follows a two-step process: first, homogeneous transformation matrices are used to determine the error in the nano-hexapod frame, then the Jacobian matrix $\bm{J}$ maps these errors to individual strut coordinates.

For control purposes, force sensors mounted on each strut are used in a decentralized way for active damping, as detailed in Section ref:sec:nass_active_damping.
Then, the high authority controller uses the computed errors in the frame of the struts to provides real-time stabilization of the sample position (Section ref:sec:nass_hac).

#+begin_src latex :file nass_control_architecture.pdf
\begin{tikzpicture}
  % Blocs
  \node[block={2.0cm}{1.0cm}, fill=colorblue!20!white] (metrology) {Metrology};
  \node[block={2.0cm}{2.0cm}, below=0.1 of metrology, align=center, fill=colorblue!20!white] (nhexa) {Nano\\Hexapod};
  \node[block={3.0cm}{1.5cm}, below=0.1 of nhexa, align=center, fill=colorblue!20!white] (ustation) {Micro\\Station};

  \coordinate[] (inputf)  at ($(nhexa.south west)!0.5!(nhexa.north west)$);
  \coordinate[] (outputfn) at ($(nhexa.south east)!0.3!(nhexa.north east)$);
  \coordinate[] (outputde) at ($(nhexa.south east)!0.7!(nhexa.north east)$);

  \coordinate[] (outputDy) at ($(ustation.south east)!0.1!(ustation.north east)$);
  \coordinate[] (outputRy) at ($(ustation.south east)!0.5!(ustation.north east)$);
  \coordinate[] (outputRz) at ($(ustation.south east)!0.9!(ustation.north east)$);

  \node[block={1.0cm}{1.0cm}, right=0.5 of outputde, fill=colorred!20!white] (Rz_kinematics) {$\bm{J}_{R_z}^{-1}$};
  \node[block={2.0cm}{2.0cm}, right=2.2 of ustation, align=center, fill=colorred!20!white] (ustation_kinematics) {Compute\\Reference\\Position};
  \node[block={2.0cm}{2.0cm}, right=0.8 of ustation_kinematics, align=center, fill=colorred!20!white] (compute_error) {Compute\\Error\\Position};
  \node[block={2.0cm}{2.0cm}, above=0.8 of compute_error, align=center, fill=colorred!20!white] (compute_pos) {Compute\\Sample\\Position};
  \node[block={1.0cm}{1.0cm}, right=0.8 of compute_error, fill=colorred!20!white] (hexa_jacobian) {$\bm{J}$};

  \coordinate[] (inputMetrology) at ($(compute_error.north east)!0.3!(compute_error.north west)$);
  \coordinate[] (inputRz) at ($(compute_error.north east)!0.7!(compute_error.north west)$);

  \node[addb={+}{}{}{}{}, right=0.4 of Rz_kinematics, fill=colorred!20!white] (addRz) {};
  \draw[->] (Rz_kinematics.east) -- (addRz.west);
  \draw[->] (outputRz-|addRz)node[branch]{} -- (addRz.south);

  \draw[->] (outputDy) node[above right]{$r_{D_y}$} -- (outputDy-|ustation_kinematics.west);
  \draw[->] (outputRy) node[above right]{$r_{R_y}$} -- (outputRy-|ustation_kinematics.west);
  \draw[->] (outputRz) node[above right]{$r_{R_z}$} -- (outputRz-|ustation_kinematics.west);

  \draw[->] (metrology.east)node[above right]{$[D_x,\,D_y,\,D_z,\,R_x,\,R_y]$} -- (compute_pos.west|-metrology);
  \draw[->] (addRz.east)node[above right]{$R_z$} -- (compute_pos.west|-addRz);
  \draw[->] (compute_pos.south)node -- (compute_error.north)node[above right]{$\bm{y}_{\mathcal{X}}$};

  \draw[->] (outputde) -- (Rz_kinematics.west) node[above left]{$\bm{\mathcal{L}}$};
  \draw[->] (ustation_kinematics.east) -- (compute_error.west) node[above left]{$\bm{r}_{\mathcal{X}}$};
  \draw[->] (compute_error.east) -- (hexa_jacobian.west) node[above left]{$\bm{\epsilon\mathcal{X}}$};
  \draw[->] (hexa_jacobian.east) -- ++(1.8, 0) node[above left]{$\bm{\epsilon\mathcal{L}}$};

  \draw[->] (outputfn) -- ($(outputfn-|hexa_jacobian.east) + (1.0, 0)$)coordinate(fn) node[above left]{$\bm{f}_n$};

  \begin{scope}[on background layer]
    \node[fit={(metrology.north-|ustation.west) (hexa_jacobian.east|-compute_error.south)}, fill=black!10!white, draw, dashed, inner sep=4pt] (plant) {};
    \node[anchor={north east}] at (plant.north east){$\text{Plant}$};
  \end{scope}

  \node[block, above=0.2 of plant, fill=coloryellow!20!white] (Kiff) {$\bm{K}_{\text{IFF}}$};
  \draw[->] ($(fn)-(0.6,0)$)node[branch]{} |- (Kiff.east);

  \node[addb={+}{}{}{}{}, left=0.8 of inputf] (addf) {};
  \draw[->] (Kiff.west) -| (addf.north);

  \begin{scope}[on background layer]
    \node[fit={(plant.south-|fn) (addf.west|-Kiff.north)}, fill=black!20!white, draw, dashed, inner sep=4pt] (damped_plant) {};
    \node[anchor={north east}] at (damped_plant.north east){$\text{Damped Plant}$};
  \end{scope}

  \begin{scope}[on background layer]
    \node[fit={(metrology.north-|ustation.west) (hexa_jacobian.east|-compute_error.south)}, fill=black!10!white, draw, dashed, inner sep=4pt] (plant) {};
    \node[anchor={north east}] at (plant.north east){$\text{Plant}$};
  \end{scope}

  \node[block, left=0.8 of addf, fill=colorgreen!20!white] (Khac) {$\bm{K}_{\text{HAC}}$};
  \draw[->] ($(hexa_jacobian.east)+(1.4,0)$)node[branch]{} |- ($(Khac.west)+(-0.4, -3.4)$) |- (Khac.west);
  \draw[->] (Khac.east) -- node[midway, above]{$\bm{f}^{\prime}$} (addf.west);
  \draw[->] (addf.east) -- (inputf) node[above left]{$\bm{f}$};
\end{tikzpicture}
#+end_src

#+name: fig:nass_control_architecture
#+caption: The physical systems are shown in blue, the control kinematics in red, the decentralized Integral Force Feedback in yellow and the centralized High Authority Controller in green.
#+attr_latex: :options [h!tbp]
#+attr_latex: :width \linewidth
#+RESULTS:
[[file:figs/nass_control_architecture.png]]

* Decentralized Active Damping
:PROPERTIES:
:HEADER-ARGS:matlab+: :tangle matlab/nass_2_active_damping.m
:END:
<<sec:nass_active_damping>>
** Introduction                                                      :ignore:

Building upon the uniaxial model study, this section implements decentralized Integral Force Feedback (IFF) as the first component of the HAC-LAC strategy.
Springs in parallel to the force sensors are used to guarantee the control robustness as was found using the 3DoF rotating model.
The objective here is to design a decentralized IFF controller that provides good damping of the nano-hexapod modes across payload masses ranging from $1$ to $50\,\text{kg}$ and rotational velocity up to $360\,\text{deg/s}$.
Used payloads have a cylindrical shape with 250 mm height and with masses of 1 kg, 25 kg, and 50 kg.

** 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 :noweb yes
<<m-init-path>>
#+end_src

#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-simscape>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src

** IFF Plant
<<ssec:nass_active_damping_plant>>

Transfer functions from actuator forces $f_i$ to force sensor measurements $f_{mi}$ are computed using the multi-body model.
Figure ref:fig:nass_iff_plant_effect_kp examines how parallel stiffness affects the plant dynamics, with identification performed at maximum spindle velocity $\Omega_z = 360\,\text{deg/s}$ and with a payload mass of 25 kg.

Without parallel stiffness (Figure ref:fig:nass_iff_plant_no_kp), the dynamics exhibits non-minimum phase zeros at low frequency, confirming predictions from the three-degree-of-freedom rotating model.
Adding parallel stiffness (Figure ref:fig:nass_iff_plant_kp) transforms these into minimum phase complex conjugate zeros, enabling unconditionally stable decentralized IFF implementation.

Though both cases show significant coupling around resonances, stability is guaranteed by the collocated arrangement of actuators and sensors [[cite:&preumont08_trans_zeros_struc_contr_with]].

#+begin_src matlab
%% Identify the IFF plant dynamics using the Simscape model

% Initialize each Simscape model elements
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeSimplifiedNanoHexapod();

% Initial Simscape Configuration
initializeSimscapeConfiguration('gravity', false);
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();

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

%% Identify for multi payload masses (no rotation)
initializeReferences(); % No Spindle Rotation
% 1kg Sample
initializeSample('type', 'cylindrical', 'm', 1);
G_iff_m1 = linearize(mdl, io);
G_iff_m1.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_iff_m1.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};

% 25kg Sample
initializeSample('type', 'cylindrical', 'm', 25);
G_iff_m25 = linearize(mdl, io);
G_iff_m25.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_iff_m25.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};

% 50kg Sample
initializeSample('type', 'cylindrical', 'm', 50);
G_iff_m50 = linearize(mdl, io);
G_iff_m50.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_iff_m50.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};

%% Effect of Rotation
initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 1); % 360 deg/s
initializeSample('type', 'cylindrical', 'm', 25);
G_iff_m25_Rz = linearize(mdl, io, 0.1);
G_iff_m25_Rz.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_iff_m25_Rz.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};

%% Effect of Rotation - No added parallel stiffness
initializeSimplifiedNanoHexapod('actuator_kp', 0);
initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 1); % 360 deg/s
initializeSample('type', 'cylindrical', 'm', 25);
G_iff_m25_Rz_no_kp = linearize(mdl, io, 0.1);
G_iff_m25_Rz_no_kp.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_iff_m25_Rz_no_kp.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
#+end_src

#+begin_src matlab :exports none :results none
%% IFF Plant - Without parallel stiffness
f = logspace(-1,3,1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(i,j), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(1,1), f, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $k_p = 0$')
for i = 2:6
    plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(i,i), f, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(1,2), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$f_{ni}/f_j$ - $k_p = 0$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-4, 1e1]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(f, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_m25_Rz_no_kp(i,i), f, 'Hz')))), 'color', colors(1,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([0:45:180]);

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_iff_plant_no_kp.pdf', 'width', 'half', 'height', 600);
#+end_src

#+begin_src matlab :exports none :results none
%% IFF Plant - With added parallel stiffness
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(i,j), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(1,1), f, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $k_p = 50N/mm$')
for i = 2:6
    plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(i,i), f, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(1,2), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$f_{ni}/f_j$ - $k_p = 50N/mm$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-4, 1e1]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(f, 180/pi*angle(squeeze(freqresp(G_iff_m25_Rz(i,i), f, 'Hz'))), 'color', colors(1,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([0:45:180]);

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_iff_plant_kp.pdf', 'width', 'half', 'height', 600);
#+end_src

#+name: fig:nass_iff_plant_effect_kp
#+caption: Effect of stiffness parallel to the force sensor on the IFF plant with $\Omega_z = 360\,\text{deg/s}$ and payload mass of 25kg. The dynamics without parallel stiffness has non-minimum phase zeros at low frequency (\subref{fig:nass_iff_plant_no_kp}). The added parallel stiffness transforms the non-minimum phase zeros to complex conjugate zeros (\subref{fig:nass_iff_plant_kp})
#+attr_latex: :options [h!tbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:nass_iff_plant_no_kp}without parallel stiffness}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/nass_iff_plant_no_kp.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:nass_iff_plant_kp}with parallel stiffness}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/nass_iff_plant_kp.png]]
#+end_subfigure
#+end_figure

The effect of rotation, shown in Figure ref:fig:nass_iff_plant_effect_rotation, is negligible as the actuator stiffness ($k_a = 1\,N/\mu m$) is large compared to the negative stiffness induced by gyroscopic effects (estimated from the 3DoF rotating model).

Figure ref:fig:nass_iff_plant_effect_payload illustrate the effect of payload mass on the plant dynamics.
While the poles and zeros are shifting with payload mass, the alternating pattern of poles and zeros is maintained, ensuring that the phase remains bounded between 0 and 180 degrees, and thus good robustness properties.

#+begin_src matlab :exports none :results none
%% Effect of spindle's rotation on the IFF Plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_iff_m25(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
             'HandleVisibility', 'off');
    end
end
plot(freqs, abs(squeeze(freqresp(G_iff_m25(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 0$ deg/s')
plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 360$ deg/s')
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
% plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(1,2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
%      'DisplayName', '$f_{ni}/f_j$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-4, 1e2]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', colors(1,:));
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m25_Rz(i,i), freqs, 'Hz'))), 'color', colors(2,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([0:45:180]);

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_iff_plant_effect_rotation.pdf', 'width', 'half', 'height', 600);
#+end_src

#+begin_src matlab :exports none :results none
%% Effect of the payload's mass on the IFF Plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_iff_m1(1,1), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
     'DisplayName', '$f_{ni}/f_i$ - 1kg')
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
         'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(G_iff_m25(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
     'DisplayName', '$f_{ni}/f_i$ - 25kg')
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
         'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(G_iff_m50(1,1), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
     'DisplayName', '$f_{ni}/f_i$ - 50kg')
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-4, 1e2]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
end
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
end
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([0:45:180]);

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_iff_plant_effect_payload.pdf', 'width', 'half', 'height', 600);
#+end_src

#+name: fig:nass_iff_plant_effect_rotation_payload
#+caption: Effect of the Spindle's rotational velocity on the IFF plant (\subref{fig:nass_iff_plant_effect_rotation}) and effect of the payload's mass on the IFF plant (\subref{fig:nass_iff_plant_effect_payload})
#+attr_latex: :options [h!tbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:nass_iff_plant_effect_rotation}Effect of Spindle rotation}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/nass_iff_plant_effect_rotation.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:nass_iff_plant_effect_payload}Effect of payload mass}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/nass_iff_plant_effect_payload.png]]
#+end_subfigure
#+end_figure

** Controller Design
<<ssec:nass_active_damping_control>>

Previous analysis using the 3DoF rotating model showed that decentralized Integral Force Feedback (IFF) with pure integrators is unstable due to gyroscopic effects caused by spindle rotation.
This finding is also confirmed with the multi-body model of the NASS: the system is unstable when using pure integrators and without parallel stiffness.

This instability can be mitigated by introducing sufficient stiffness in parallel with the force sensors.
However, as illustrated in Figure ref:fig:nass_iff_plant_kp, adding parallel stiffness increases the low frequency gain.
If using pure integrators, this would results in high loop gain at low frequencies, adversely affecting the damped plant dynamics, which is undesirable.
To resolve this issue, a second-order high-pass filter is introduced to limit the low frequency gain, as shown in Equation eqref:eq:nass_kiff.

\begin{equation}\label{eq:nass_kiff}
  \bm{K}_{\text{IFF}}(s) = g \cdot \begin{bmatrix}
      K_{\text{IFF}}(s) & & 0 \\
      & \ddots & \\
      0 & & K_{\text{IFF}}(s)
  \end{bmatrix}, \quad K_{\text{IFF}}(s) = \frac{1}{s} \cdot \frac{\frac{s^2}{\omega_z^2}}{\frac{s^2}{\omega_z^2} + 2 \xi_z \frac{s}{\omega_z} + 1}
\end{equation}

The cut-off frequency of the second-order high-pass filter is tuned to be below the frequency of the complex conjugate zero for the highest mass, which is at $5\,\text{Hz}$.
The overall gain is then increased to have large loop gain around resonances to be damped, as illustrated in Figure ref:fig:nass_iff_loop_gain.

#+begin_src matlab
%% Verify that parallel stiffness permits to have a stable plant
Kiff_pure_int = -200/s*eye(6);
isstable(feedback(G_iff_m25_Rz,       Kiff_pure_int, 1))
isstable(feedback(G_iff_m25_Rz_no_kp, Kiff_pure_int, 1))
#+end_src

#+begin_src matlab
%% IFF Controller Design
% Second order high pass filter
wz = 2*pi*2;
xiz = 0.7;
Ghpf = (s^2/wz^2)/(s^2/wz^2 + 2*xiz*s/wz + 1);

Kiff = -200 * ...              % Gain
       1/(0.01*2*pi + s) * ... % LPF: provides integral action
       Ghpf * ...              % 2nd order HPF (limit low frequency gain)
       eye(6);                 % Diagonal 6x6 controller (i.e. decentralized)

Kiff.InputName = {'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
Kiff.OutputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
#+end_src

#+begin_src matlab :exports none :tangle no
% The designed IFF controller is saved
save('./matlab/mat/nass_K_iff.mat', 'Kiff');
#+end_src

#+begin_src matlab :eval no
% The designed IFF controller is saved
save('./mat/nass_K_iff.mat', 'Kiff');
#+end_src

#+begin_src matlab :exports none :results none
%% Loop gain for the decentralized IFF
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m1(1,1), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
     'DisplayName', '1kg')
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
         'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m25(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
     'DisplayName', '25kg')
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
         'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m50(1,1), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
     'DisplayName', '50kg')
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ylim([1e-4, 1e2]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(-Kiff(1,1)*G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
end
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(-Kiff(1,1)*G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
end
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(-Kiff(1,1)*G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-110, 200]);
yticks([-180, -90, 0, 90, 180]);

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

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

#+name: fig:nass_iff_loop_gain
#+caption: Loop gain for the decentralized IFF: $K_{\text{IFF}}(s) \cdot \frac{f_{mi}}{f_i}(s)$
#+attr_latex: :options [h!tbp]
#+RESULTS:
[[file:figs/nass_iff_loop_gain.png]]

To verify stability, root loci for the three payload configurations are computed and shown in Figure ref:fig:nass_iff_root_locus.
The results demonstrate that the closed-loop poles remain within the left-half plane, indicating the robust stability properties of the applied decentralized IFF.

#+begin_src matlab :exports none :results none
%% Root Locus for the Decentralized IFF controller - 1kg Payload
figure;
gains = logspace(-2, 1, 200);

figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;

plot(real(pole(G_iff_m1)),  imag(pole(G_iff_m1)),  'x', 'color', colors(1,:), ...
    'DisplayName', '$g = 0$');
plot(real(tzero(G_iff_m1)), imag(tzero(G_iff_m1)), 'o', 'color', colors(1,:), ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(G_iff_m1, g*Kiff, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
        'HandleVisibility', 'off');
end

% Optimal gain
clpoles = pole(feedback(G_iff_m1, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
    'DisplayName', '$g_{opt}$');

xline(0);
yline(0);
hold off;
axis equal;
xlim([-900, 100]); ylim([-100, 900]);
xticks([-900:100:0]);
yticks([0:100:900]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_iff_root_locus_1kg.pdf', 'width', 'third', 'height', 'normal');
#+end_src

#+begin_src matlab :exports none :results none
%% Root Locus for the Decentralized IFF controller - 25kg Payload
gains = logspace(-2, 1, 200);

figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;

plot(real(pole(G_iff_m25)),  imag(pole(G_iff_m25)),  'x', 'color', colors(2,:), ...
    'DisplayName', '$g = 0$');
plot(real(tzero(G_iff_m25)), imag(tzero(G_iff_m25)), 'o', 'color', colors(2,:), ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(G_iff_m25, g*Kiff, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
        'HandleVisibility', 'off');
end

% Optimal gain
clpoles = pole(feedback(G_iff_m25, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
    'DisplayName', '$g_{opt}$');

xline(0);
yline(0);
hold off;
axis equal;
xlim([-900, 100]); ylim([-100, 900]);
xticks([-900:100:0]);
yticks([0:100:900]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_iff_root_locus_25kg.pdf', 'width', 'third', 'height', 'normal');
#+end_src

#+begin_src matlab :exports none :results none
%% Root Locus for the Decentralized IFF controller - 50kg Payload
gains = logspace(-2, 1, 200);

figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;

plot(real(pole(G_iff_m50)),  imag(pole(G_iff_m50)),  'x', 'color', colors(3,:), ...
    'DisplayName', '$g = 0$');
plot(real(tzero(G_iff_m50)), imag(tzero(G_iff_m50)), 'o', 'color', colors(3,:), ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(G_iff_m50, g*Kiff, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
        'HandleVisibility', 'off');
end

% Optimal gain
clpoles = pole(feedback(G_iff_m50, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
    'DisplayName', '$g_{opt}$');

xline(0);
yline(0);
hold off;
axis equal;
xlim([-900, 100]); ylim([-100, 900]);
xticks([-900:100:0]);
yticks([0:100:900]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_iff_root_locus_50kg.pdf', 'width', 'third', 'height', 'normal');
#+end_src

#+name: fig:nass_iff_root_locus
#+caption: Root Loci for Decentralized IFF for three payload masses. Closed-loop poles are shown by the black crosses.
#+attr_latex: :options [h!tbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:nass_iff_root_locus_1kg} $1\,\text{kg}$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/nass_iff_root_locus_1kg.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:nass_iff_root_locus_25kg} $25\,\text{kg}$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/nass_iff_root_locus_25kg.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:nass_iff_root_locus_50kg} $50\,\text{kg}$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/nass_iff_root_locus_50kg.png]]
#+end_subfigure
#+end_figure

* Centralized Active Vibration Control
:PROPERTIES:
:HEADER-ARGS:matlab+: :tangle matlab/nass_3_hac.m
:END:
<<sec:nass_hac>>
** Introduction                                                      :ignore:

The implementation of high-bandwidth position control for the nano-hexapod presents several technical challenges.
The plant dynamics exhibit complex behavior influenced by multiple factors including payload mass, rotational velocity, and the mechanical coupling between the nano-hexapod and the micro-station.
This section presents the development and validation of a centralized control strategy designed to achieve precise sample positioning during high-speed tomography experiments.

First, a comprehensive analysis of the plant dynamics is conducted in Section ref:ssec:nass_hac_plant, examining the effects of spindle rotation, payload mass variation, and the implementation of Integral Force Feedback (IFF).
Section ref:ssec:nass_hac_stiffness validates previous modeling predictions that both overly stiff and overly compliant nano-hexapod configurations lead to degraded performance, through detailed analysis using the multi-body model.
Building upon these findings, Section ref:ssec:nass_hac_controller presents the design of a robust high-authority controller capable of maintaining stability across varying payload masses while achieving the desired control bandwidth.

The performance of the developed control strategy is validated through simulations of tomography experiments in Section ref:ssec:nass_hac_tomography.
These simulations incorporate realistic disturbance sources and evaluate system performance against the stringent positioning requirements imposed by future beamline specifications.
Particular attention is paid to the system's behavior under maximum rotational velocity conditions and its ability to accommodate varying payload masses, demonstrating the practical viability of the proposed control approach.

** 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 :noweb yes
<<m-init-path>>
#+end_src

#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-simscape>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src

** HAC Plant
<<ssec:nass_hac_plant>>

The plant dynamics from force inputs $\bm{f}$ to the strut errors $\bm{\epsilon}_{\mathcal{L}}$ were first extracted from the multi-body model without implementation of the decentralized IFF.
The influence of spindle rotation on plant dynamics was investigated, with results presented in Figure ref:fig:nass_undamped_plant_effect_Wz.
While rotational motion introduces coupling effects at low frequencies, these remain minimal at operational velocities, owing to the high stiffness characteristics of the nano-hexapod assembly.

Payload mass emerged as a significant parameter affecting system behavior, as illustrated in Figure ref:fig:nass_undamped_plant_effect_mass.
As expected, increasing payload mass was found to decrease resonance frequencies while amplifying coupling at low frequency.
These mass-dependent dynamic changes present considerable challenges for control system design, particularly for configurations with high payload masses.

Additional operational parameters were systematically evaluated, including the $R_y$ tilt angle, $R_z$ spindle position, and micro-hexapod position.
These factors were found to exert negligible influence on the plant dynamics, attributable to the effective mechanical decoupling achieved between the plant and micro-station dynamics.
This decoupling characteristic ensures consistent performance across various operational configurations.
This also validates the developed control kinematics.

#+begin_src matlab
%% Identify the IFF plant dynamics using the Simscape model

% Initialize each Simscape model elements
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeSimplifiedNanoHexapod();
initializeSample('type', 'cylindrical', 'm', 1);

% Initial Simscape Configuration
initializeSimscapeConfiguration('gravity', false);
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'input');                  io_i = io_i + 1; % Actuator Inputs [N]
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL');  io_i = io_i + 1; % Strut errors [m]

%% Identify HAC Plant without using IFF
initializeSample('type', 'cylindrical', 'm', 1);
G_m1 = linearize(mdl, io);
G_m1.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m1.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

initializeSample('type', 'cylindrical', 'm', 25);
G_m25 = linearize(mdl, io);
G_m25.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m25.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

initializeSample('type', 'cylindrical', 'm', 50);
G_m50 = linearize(mdl, io);
G_m50.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m50.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

%% Effect of Rotation
initializeSample('type', 'cylindrical', 'm', 1);
initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 1); % 360 deg/s

G_m1_Rz = linearize(mdl, io, 0.1);
G_m1_Rz.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_m1_Rz.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
#+end_src

#+begin_src matlab :exports none :results none
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m1(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, $\Omega = 0$')
plot(freqs, abs(squeeze(freqresp(G_m1_Rz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, $\Omega = 360$ deg/s')
plot(freqs, abs(squeeze(freqresp(G_m1(1,2), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$')
plot(freqs, abs(squeeze(freqresp(G_m1_Rz(1,2), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$')
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_m1(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m1_Rz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_m1(i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m1_Rz(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-11, 2e-5]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1(i,i), freqs, 'Hz')))), 'color', colors(1,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1_Rz(i,i), freqs, 'Hz')))), 'color', colors(2,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-180:45:180]);

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_undamped_plant_effect_Wz.pdf', 'width', 'half', 'height', 600);
#+end_src

#+begin_src matlab :exports none :results none
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m1( 1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, 1 kg')
plot(freqs, abs(squeeze(freqresp(G_m25(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, 25 kg')
plot(freqs, abs(squeeze(freqresp(G_m50(1,1), freqs, 'Hz'))), 'color', colors(3,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, 50 kg')
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_m1(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m25(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m50(i,j), freqs, 'Hz'))), 'color', [colors(3,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m50(i,i), freqs, 'Hz'))), 'color', colors(3,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-11, 2e-5]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1(i,i), freqs, 'Hz')))), 'color', colors(1,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25(i,i), freqs, 'Hz')))), 'color', colors(2,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m50(i,i), freqs, 'Hz')))), 'color', colors(3,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-180:45:180]);

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_undamped_plant_effect_mass.pdf', 'width', 'half', 'height', 600);
#+end_src

#+name: fig:nass_undamped_plant_effect
#+caption: Effect of the Spindle's rotational velocity on the positioning plant (\subref{fig:nass_undamped_plant_effect_Wz}) and effect of the payload's mass on the positioning plant (\subref{fig:nass_undamped_plant_effect_mass})
#+attr_latex: :options [h!tbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:nass_undamped_plant_effect_Wz}Effect of rotational velocity $\Omega_z$}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/nass_undamped_plant_effect_Wz.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:nass_undamped_plant_effect_mass}Effect of payload's mass}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/nass_undamped_plant_effect_mass.png]]
#+end_subfigure
#+end_figure

The Decentralized Integral Force Feedback was implemented in the multi-body model, and transfer functions from force inputs $\bm{f}^\prime$ of the damped plant to the strut errors $\bm{\epsilon}_{\mathcal{L}}$ were extracted from this model.

The effectiveness of IFF implementation was first evaluated with a $1\,\text{kg}$ payload, as demonstrated in Figure ref:fig:nass_comp_undamped_damped_plant_m1.
The results indicate successful damping of the nano-hexapod resonance modes, though a minor increase in low-frequency coupling was observed.
This trade-off was considered acceptable given the overall improvement in system behavior.

The benefits of IFF implementation were further assessed across the full range of payload configurations, with results presented in Figure ref:fig:nass_hac_plants.
For all tested payloads ($1\,\text{kg}$, $25\,\text{kg}$ and $50\,\text{kg}$), decentralized IFF significantly damped the nano-hexapod modes and therefore simplified the system dynamics.
More importantly, is the fact that in the vicinity of the wanted high authority control bandwidth (i.e. between $10\,\text{Hz}$ and $50\,\text{Hz}$), the damped dynamics (shown in red) exhibited minimal gain and phase variations with frequency.
For the undamped system (shown in blue), achieving robust control with bandwidth above 10Hz while maintaining stability across different payload masses would be practically unfeasible.

#+begin_src matlab
%% Identify HAC Plant without using IFF
initializeReferences(); % No Spindle Rotation
initializeController('type', 'iff'); % Implemented IFF controller
load('nass_K_iff.mat', 'Kiff'); % Load designed IFF controller

% 1kg payload
initializeSample('type', 'cylindrical', 'm', 1);
G_hac_m1 = linearize(mdl, io);
G_hac_m1.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_hac_m1.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% 25kg payload
initializeSample('type', 'cylindrical', 'm', 25);
G_hac_m25 = linearize(mdl, io);
G_hac_m25.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_hac_m25.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% 50kg payload
initializeSample('type', 'cylindrical', 'm', 50);
G_hac_m50 = linearize(mdl, io);
G_hac_m50.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_hac_m50.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% Check stability
if not(isstable(G_hac_m1) && isstable(G_hac_m25) && isstable(G_hac_m50))
    warning('One of HAC plant is not stable')
end
#+end_src

#+begin_src matlab :exports none :results none
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m1( 1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - OL')
plot(freqs, abs(squeeze(freqresp(G_hac_m1(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - IFF')
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_m1(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_hac_m1(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_hac_m1(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-10, 5e-5]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1(i,i), freqs, 'Hz')))), 'color', colors(1,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_hac_m1(i,i), freqs, 'Hz')))), 'color', colors(2,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-180:45:180]);

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_comp_undamped_damped_plant_m1.pdf', 'width', 'half', 'height', 600);
#+end_src

#+begin_src matlab :exports none :results none
%% Comparison of all the undamped FRF and all the damped FRF
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m1(    1,1), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'DisplayName', '$\epsilon\mathcal{L}_i/f_i$ - OL');
plot(freqs, abs(squeeze(freqresp(G_hac_m1(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'DisplayName', '$\epsilon\mathcal{L}_i/f_i^\prime$ - IFF');
for i = 1:6
    plot(freqs, abs(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m50(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
end
for i = 1:6
    plot(freqs, abs(squeeze(freqresp(G_hac_m1( i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_hac_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_hac_m50(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([1e-10, 5e-5]);

ax2 = nexttile;
hold on;
for i =1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_m50(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
end
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_m1( i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_m50(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
ylim([-200, 20]);
yticks([-180:45:180]);

linkaxes([ax1,ax2],'x');
% xlim([1, 5e2]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_hac_plants.pdf', 'width', 'half', 'height', 600);
#+end_src

#+name: fig:nass_hac_plant
#+caption: Effect of the Spindle's rotational velocity on the positioning plant (\subref{fig:nass_undamped_plant_effect_Wz}) and effect of the payload's mass on the positioning plant (\subref{fig:nass_undamped_plant_effect_mass})
#+attr_latex: :options [h!tbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:nass_comp_undamped_damped_plant_m1}Effect of IFF - $m = 1\,\text{kg}$}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/nass_comp_undamped_damped_plant_m1.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:nass_hac_plants}Effect of IFF on the set of plants to control}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/nass_hac_plants.png]]
#+end_subfigure
#+end_figure

The coupling between the nano-hexapod and micro-station was evaluated through comparative analysis of plant dynamics under two mounting conditions.
In the first configuration, the nano-hexapod was mounted on an ideally rigid support, while in the second configuration, it was installed on the micro-station with finite compliance.

As illustrated in Figure ref:fig:nass_effect_ustation_compliance, the complex dynamics of the micro-station were found to have little impact on the plant dynamics.
The only observable difference manifests as alternating poles and zeros above 100Hz, a frequency range sufficiently beyond the control bandwidth to avoid interference with system performance.
This finding confirms effective dynamic decoupling between the nano-hexapod and the supporting micro-station structure.

#+begin_src matlab
%% Identify plant with "rigid" micro-station
initializeGround('type', 'rigid');
initializeGranite('type', 'rigid');
initializeTy('type', 'rigid');
initializeRy('type', 'rigid');
initializeRz('type', 'rigid');
initializeMicroHexapod('type', 'rigid');
initializeSimplifiedNanoHexapod();
initializeSample('type', 'cylindrical', 'm', 25);

initializeReferences();
initializeController('type', 'open-loop'); % Implemented IFF controller
load('nass_K_iff.mat', 'Kiff'); % Load designed IFF controller

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'input');                  io_i = io_i + 1; % Actuator Inputs [N]
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL');  io_i = io_i + 1; % Strut errors [m]

G_m25_rigid = linearize(mdl, io);
G_m25_rigid.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m25_rigid.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
#+end_src

#+begin_src matlab :exports none :results none
%% Effect of the micro-station limited compliance on the plant dynamics
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m25_rigid( 1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - Rigid support')
plot(freqs, abs(squeeze(freqresp(G_m25(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - $\mu$-station support')
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_m25_rigid(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m25(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_m25_rigid( i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-10, 5e-5]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_rigid(i,i), freqs, 'Hz')))), 'color', colors(1,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25(i,i), freqs, 'Hz')))), 'color', colors(2,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-180:45:180]);

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

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

#+name: fig:nass_effect_ustation_compliance
#+caption: Effect of the micro-station limited compliance on the plant dynamics
#+attr_latex: :options [h!tbp]
#+RESULTS:
[[file:figs/nass_effect_ustation_compliance.png]]

** Effect of Nano-Hexapod Stiffness on System Dynamics
<<ssec:nass_hac_stiffness>>

The influence of nano-hexapod stiffness was investigated to validate earlier findings from simplified uniaxial and three-degree-of-freedom (3DoF) models.
These models suggested that a moderate stiffness of approximately $1\,N/\mu m$ would provide better performance compared to either very stiff or very soft configurations.

For the stiff nano-hexapod analysis, a system with actuator stiffness of $100\,N/\mu m$ was simulated with a $25\,\text{kg}$ payload.
The transfer function from $\bm{f}$ to $\bm{\epsilon}_{\mathcal{L}}$ was evaluated under two conditions: mounting on an infinitely rigid base and mounting on the micro-station.
As shown in Figure ref:fig:nass_stiff_nano_hexapod_coupling_ustation, significant coupling was observed between the nano-hexapod and micro-station dynamics.
This coupling introduces complex behavior that proves difficult to model and predict accurately, corroborating the predictions of the simplified uniaxial model.

The soft nano-hexapod configuration was evaluated using a stiffness of $0.01\,N/\mu m$ with a $25\,\text{kg}$ payload.
Dynamic response was characterized at three rotational velocities: 0, 36, and 360 deg/s.
Figure ref:fig:nass_soft_nano_hexapod_effect_Wz demonstrates that rotation substantially impacts system dynamics, manifesting as instability at high rotational velocities, increased coupling from gyroscopic effects, and rotation-dependent resonance frequencies.
The current approach of controlling the motion in the strut frame proves inadequate for soft nano-hexapods; but even shifting control to the payload's center of mass frame would not overcome the substantial coupling and dynamic variations induced by gyroscopic effects.

#+begin_src matlab
%% Identify Dynamics with a Stiff nano-hexapod (100N/um)

% Initialize each Simscape model elements
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeSimplifiedNanoHexapod('actuator_k', 1e8, 'actuator_kp', 0, 'actuator_c', 1e3);
initializeSample('type', 'cylindrical', 'm', 25);

% Initial Simscape Configuration
initializeSimscapeConfiguration('gravity', false);
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'input');                  io_i = io_i + 1; % Actuator Inputs [N]
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL');  io_i = io_i + 1; % Strut errors [m]

% Identify Plant
G_m25_pz = linearize(mdl, io);
G_m25_pz.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m25_pz.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

%% Compare with Nano-Hexapod alone (rigid micro-station)
initializeGround('type', 'rigid');
initializeGranite('type', 'rigid');
initializeTy('type', 'rigid');
initializeRy('type', 'rigid');
initializeRz('type', 'rigid');
initializeMicroHexapod('type', 'rigid');

% Identify Plant
G_m25_pz_rigid = linearize(mdl, io);
G_m25_pz_rigid.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m25_pz_rigid.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
#+end_src

#+begin_src matlab :exports none :results none
%% Stiff nano-hexapod - Coupling with the micro-station
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - Rigid')
plot(freqs, abs(squeeze(freqresp(G_m25_pz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - $\mu$-station')
plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(1,2), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$ - Rigid')
plot(freqs, abs(squeeze(freqresp(G_m25_pz(1,2), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$ - $\mu$-station')
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m25_pz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m25_pz(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-12, 3e-7]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_pz_rigid(i,i), freqs, 'Hz')))), 'color', colors(1,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_pz(i,i), freqs, 'Hz')))), 'color', colors(2,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-180:45:180]);

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_stiff_nano_hexapod_coupling_ustation.pdf', 'width', 'half', 'height', 600);
#+end_src

#+begin_src matlab
%% Identify Dynamics with a Soft nano-hexapod (0.01N/um)
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeSimplifiedNanoHexapod('actuator_k', 1e4, 'actuator_kp', 0, 'actuator_c', 1);

% Initialize each Simscape model elements
initializeSample('type', 'cylindrical', 'm', 25); % 25kg payload
initializeController('type', 'open-loop');

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'input');                  io_i = io_i + 1; % Actuator Inputs [N]
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL');  io_i = io_i + 1; % Strut errors [m]

% Identify the dynamics without rotation
initializeReferences();
G_m1_vc = linearize(mdl, io);
G_m1_vc.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m1_vc.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% Identify the dynamics with 36 deg/s rotation
initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 10); % 36 deg/s

G_m1_vc_Rz_slow = linearize(mdl, io, 0.1);
G_m1_vc_Rz_slow.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_m1_vc_Rz_slow.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% Identify the dynamics with 360 deg/s rotation
initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 1); % 360 deg/s

G_m1_vc_Rz_fast = linearize(mdl, io, 0.1);
G_m1_vc_Rz_fast.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_m1_vc_Rz_fast.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
#+end_src

#+begin_src matlab :exports none :results none
%% Soft Nano-Hexapod - effect of rotational velocity on the dynamics
f = logspace(-1,2,200);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(squeeze(freqresp(G_m1_vc(1,1), f, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 0$')
plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_slow(1,1), f, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 36$ deg/s')
plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_fast(1,1), f, 'Hz'))), 'color', colors(3,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 360$ deg/s')
for i = 1:5
    for j = i+1:6
        plot(f, abs(squeeze(freqresp(G_m1_vc(i,j), f, 'Hz'))), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_slow(i,j), f, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_fast(i,j), f, 'Hz'))), 'color', [colors(3,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(f, abs(squeeze(freqresp(G_m1_vc(i,i), f, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end
for i = 2:6
    plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_slow(i,i), f, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
for i = 2:6
    plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_fast(i,i), f, 'Hz'))), 'color', colors(3,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-9, 1e-2]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(f, 180/pi*angle(squeeze(freqresp(G_m1_vc(i,i), f, 'Hz'))), 'color', colors(1,:));
    plot(f, 180/pi*angle(squeeze(freqresp(G_m1_vc_Rz_slow(i,i), f, 'Hz'))), 'color', colors(2,:));
    plot(f, 180/pi*angle(squeeze(freqresp(G_m1_vc_Rz_fast(i,i), f, 'Hz'))), 'color', colors(3,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180:90:180]);

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_soft_nano_hexapod_effect_Wz.pdf', 'width', 'half', 'height', 600);
#+end_src

#+name: fig:nass_soft_stiff_hexapod
#+caption: Plant dynamics of a stiff ($k_a = 100\,N/\mu m$) nano-hexapod (\subref{fig:nass_stiff_nano_hexapod_coupling_ustation}) and of a soft ($k_a = 0.01\,N/\mu m$) nano-hexapod (\subref{fig:nass_soft_nano_hexapod_effect_Wz})
#+attr_latex: :options [h!tbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:nass_stiff_nano_hexapod_coupling_ustation}Stiff nano-hexapod - Coupling with the micro-station}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/nass_stiff_nano_hexapod_coupling_ustation.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:nass_soft_nano_hexapod_effect_Wz}Soft nano-hexapod - Effect of Spindle rotational velocity}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/nass_soft_nano_hexapod_effect_Wz.png]]
#+end_subfigure
#+end_figure

** Controller design
<<ssec:nass_hac_controller>>

A high authority controller was designed to meet two key requirements: stable for all payload masses (i.e. for all the damped plants of Figure ref:fig:nass_hac_plants), and achievement of sufficient bandwidth (targeted at 10Hz) for high performance operation.
The controller structure is defined in Equation eqref:eq:nass_robust_hac, incorporating an integrator term for low frequency performance, a lead compensator for phase margin improvement, and a low-pass filter for robustness against high frequency modes.

\begin{equation}\label{eq:nass_robust_hac}
  K_{\text{HAC}}(s) = g_0 \cdot \underbrace{\frac{\omega_c}{s}}_{\text{int}} \cdot \underbrace{\frac{1}{\sqrt{\alpha}}\frac{1 + \frac{s}{\omega_c/\sqrt{\alpha}}}{1 + \frac{s}{\omega_c\sqrt{\alpha}}}}_{\text{lead}} \cdot \underbrace{\frac{1}{1 + \frac{s}{\omega_0}}}_{\text{LPF}}, \quad \left( \omega_c = 2\pi10\,\text{rad/s},\ \alpha = 2,\ \omega_0 = 2\pi80\,\text{rad/s} \right)
\end{equation}

#+begin_src matlab
%% HAC Design
% Wanted crossover
wc = 2*pi*10; % [rad/s]

% Integrator
H_int = wc/s;

% Lead to increase phase margin
a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));

% Low Pass filter to increase robustness
H_lpf = 1/(1 + s/2/pi/80);

% Gain to have unitary crossover at wc
H_gain = 1./abs(evalfr(G_hac_m50(1,1), 1j*wc));

% Decentralized HAC
Khac = -H_gain  * ... % Gain
        H_int   * ... % Integrator
        H_lead   * ... % Low Pass filter
        H_lpf   * ... % Low Pass filter
        eye(6);       % 6x6 Diagonal
#+end_src

#+begin_src matlab :exports none :tangle no
% The designed HAC controller is saved
save('./matlab/mat/nass_K_hac.mat', 'Khac');
#+end_src

#+begin_src matlab :eval no
% The designed HAC controller is saved
save('./mat/nass_K_hac.mat', 'Khac');
#+end_src

The controller's performance was evaluated through two complementary analyses.
First, the decentralized loop gain, shown in Figure ref:fig:nass_hac_loop_gain, confirms the achievement of the desired 10Hz bandwidth.
Second, the characteristic loci analysis presented in Figure ref:fig:nass_hac_loci demonstrates robustness for all payload masses, with adequate stability margins maintained throughout the operating envelope.

#+begin_src matlab :exports none :results none
%% "Diagonal" loop gain for the High Authority Controller
f = logspace(-1, 2, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m1( i,i), f, 'Hz'))), ...
     'color', [colors(1,:), 0.5], 'DisplayName', '1kg');
plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m25(i,i), f, 'Hz'))), ...
     'color', [colors(2,:), 0.5], 'DisplayName', '25kg');
plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m50(i,i), f, 'Hz'))), ...
     'color', [colors(3,:), 0.5], 'DisplayName', '50kg');
for i = 2:6
    plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m1( i,i), f, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m25(i,i), f, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m50(i,i), f, 'Hz'))), 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ylim([1e-2, 1e2]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(f, 180/pi*angle(squeeze(freqresp(-Khac(i,i)*G_hac_m1( i,i), f, 'Hz'))), 'color', [colors(1,:), 0.5]);
    plot(f, 180/pi*angle(squeeze(freqresp(-Khac(i,i)*G_hac_m25(i,i), f, 'Hz'))), 'color', [colors(2,:), 0.5]);
    plot(f, 180/pi*angle(squeeze(freqresp(-Khac(i,i)*G_hac_m50(i,i), f, 'Hz'))), 'color', [colors(3,:), 0.5]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 180])

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_hac_loop_gain.pdf', 'width', 'half', 'height', 600);
#+end_src

#+begin_src matlab :exports none :results none
%% Characteristic Loci for the High Authority Controller
Ldet_m1 = zeros(6, length(freqs));
Lmimo_m1 = squeeze(freqresp(-G_hac_m1*Khac, freqs, 'Hz'));
for i_f = 2:length(freqs)
    Ldet_m1(:, i_f) = eig(squeeze(Lmimo_m1(:,:,i_f)));
end
Ldet_m25 = zeros(6, length(freqs));
Lmimo_m25 = squeeze(freqresp(-G_hac_m25*Khac, freqs, 'Hz'));
for i_f = 2:length(freqs)
    Ldet_m25(:, i_f) = eig(squeeze(Lmimo_m25(:,:,i_f)));
end
Ldet_m50 = zeros(6, length(freqs));
Lmimo_m50 = squeeze(freqresp(-G_hac_m50*Khac, freqs, 'Hz'));
for i_f = 2:length(freqs)
    Ldet_m50(:, i_f) = eig(squeeze(Lmimo_m50(:,:,i_f)));
end

figure;
hold on;
plot(real(squeeze(Ldet_m1(1,:))), imag(squeeze(Ldet_m1(1,:))), ...
        '.', 'color', colors(1, :), ...
        'DisplayName', '1kg');
plot(real(squeeze(Ldet_m1(1,:))),-imag(squeeze(Ldet_m1(1,:))), ...
        '.', 'color', colors(1, :), ...
        'HandleVisibility', 'off');
plot(real(squeeze(Ldet_m25(1,:))), imag(squeeze(Ldet_m25(1,:))), ...
        '.', 'color', colors(2, :), ...
        'DisplayName', '25kg');
plot(real(squeeze(Ldet_m25(1,:))),-imag(squeeze(Ldet_m25(1,:))), ...
        '.', 'color', colors(2, :), ...
        'HandleVisibility', 'off');
plot(real(squeeze(Ldet_m50(1,:))), imag(squeeze(Ldet_m50(1,:))), ...
        '.', 'color', colors(3, :), ...
        'DisplayName', '50kg');
plot(real(squeeze(Ldet_m50(1,:))),-imag(squeeze(Ldet_m50(1,:))), ...
        '.', 'color', colors(3, :), ...
        'HandleVisibility', 'off');
for i = 2:6
    plot(real(squeeze(Ldet_m1(i,:))), imag(squeeze(Ldet_m1(i,:))), ...
            '.', 'color', colors(1, :), ...
            'HandleVisibility', 'off');
    plot(real(squeeze(Ldet_m1(i,:))), -imag(squeeze(Ldet_m1(i,:))), ...
            '.', 'color', colors(1, :), ...
            'HandleVisibility', 'off');

    plot(real(squeeze(Ldet_m25(i,:))), imag(squeeze(Ldet_m25(i,:))), ...
            '.', 'color', colors(2, :), ...
            'HandleVisibility', 'off');
    plot(real(squeeze(Ldet_m25(i,:))), -imag(squeeze(Ldet_m25(i,:))), ...
            '.', 'color', colors(2, :), ...
            'HandleVisibility', 'off');

    plot(real(squeeze(Ldet_m50(i,:))), imag(squeeze(Ldet_m50(i,:))), ...
            '.', 'color', colors(3, :), ...
            'HandleVisibility', 'off');
    plot(real(squeeze(Ldet_m50(i,:))), -imag(squeeze(Ldet_m50(i,:))), ...
            '.', 'color', colors(3, :), ...
            'HandleVisibility', 'off');
end
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
xlabel('Real Part'); ylabel('Imaginary Part');
axis square
xlim([-1.8, 0.2]); ylim([-1, 1]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_hac_loci.pdf', 'width', 'half', 'height', 600);
#+end_src

#+name: fig:nass_hac_controller
#+caption: High Authority Controller - "Diagonal Loop Gain" (\subref{fig:nass_hac_loop_gain}) and Characteristic Loci (\subref{fig:nass_hac_loci})
#+attr_latex: :options [h!tbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:nass_hac_loop_gain}Loop Gain}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/nass_hac_loop_gain.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:nass_hac_loci}Characteristic Loci}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/nass_hac_loci.png]]
#+end_subfigure
#+end_figure

** Tomography experiment
<<ssec:nass_hac_tomography>>

The Nano Active Stabilization System concept was validated through time-domain simulations of scientific experiments, with particular focus on tomography scanning due to its demanding performance requirements.
Simulations were conducted at the maximum operational rotational velocity of $\Omega_z = 360\,\text{deg/s}$ to evaluate system performance under the most challenging conditions.

Performance metrics were established based on anticipated future beamline specifications, which specify a beam size of 200nm (horizontal) × 100nm (vertical).
The primary requirement stipulates that the point of interest must remain within these beam dimensions throughout operation.
The simulation incorporated two principal disturbance sources: ground motion and spindle vibrations.
Additional noise sources, including measurement noise and electrical noise from DAC and voltage amplifiers, were not included in this analysis as these parameters will be optimized during the detailed design phase.

Figure ref:fig:nass_tomo_1kg_60rpm presents a comparative analysis of positioning errors under both open-loop and closed-loop conditions for a lightweight sample configuration (1kg).
The results demonstrate the system's capability to maintain position within the specified beam dimensions, validating the fundamental concept of the stabilization system.

#+begin_src matlab
% Sample is not centered with the rotation axis
% This is done by offsetfing the micro-hexapod by 0.9um
P_micro_hexapod = [0.9e-6; 0; 0]; % [m]

open(mdl);
set_param(mdl, 'StopTime', '2');

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod('AP', P_micro_hexapod);
initializeSample('type', 'cylindrical', 'm', 1);

initializeSimscapeConfiguration('gravity', false);
initializeLoggingConfiguration('log', 'all', 'Ts', 1e-3);

initializeDisturbances(...
    'Dw_x',  true, ... % Ground Motion - X direction
    'Dw_y',  true, ... % Ground Motion - Y direction
    'Dw_z',  true, ... % Ground Motion - Z direction
    'Fdy_x', false, ... % Translation Stage - X direction
    'Fdy_z', false, ... % Translation Stage - Z direction
    'Frz_x', true, ... % Spindle - X direction
    'Frz_y', true, ... % Spindle - Y direction
    'Frz_z', true);    % Spindle - Z direction

initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 1, ...
    'Dh_pos', [P_micro_hexapod; 0; 0; 0]);

% Open-Loop Simulation without Nano-Hexapod - 1kg payload
initializeSimplifiedNanoHexapod('type', 'none');
initializeController('type', 'open-loop');
sim(mdl);
exp_tomo_ol_m1 = simout;

% Closed-Loop Simulation with NASS
initializeSimplifiedNanoHexapod();
initializeController('type', 'hac-iff');
load('nass_K_iff.mat', 'Kiff');
load('nass_K_hac.mat', 'Khac');

% 1kg payload
initializeSample('type', 'cylindrical', 'm', 1);
sim(mdl);
exp_tomo_cl_m1 = simout;

% 25kg payload
initializeSample('type', 'cylindrical', 'm', 25);
sim(mdl);
exp_tomo_cl_m25 = simout;

% 50kg payload
initializeSample('type', 'cylindrical', 'm', 50);
sim(mdl);
exp_tomo_cl_m50 = simout;

% Slower tomography for high payload mass
% initializeReferences(...
%     'Rz_type', 'rotating', ...
%     'Rz_period', 10, ... % 36deg/s
%     'Dh_pos', [P_micro_hexapod; 0; 0; 0]);
% initializeSample('type', 'cylindrical', 'm', 50);
% set_param(mdl, 'StopTime', '5');
% sim(mdl);
% exp_tomo_cl_m50_slow = simout;
#+end_src

#+begin_src matlab :exports none :results none
%% Simulation of tomography experiment - 1kg payload - 360deg/s - XY errors
figure;
hold on;
plot(1e6*exp_tomo_ol_m1.y.x.Data, 1e6*exp_tomo_ol_m1.y.y.Data, 'DisplayName', 'OL')
plot(1e6*exp_tomo_cl_m1.y.x.Data(1e3:end), 1e6*exp_tomo_cl_m1.y.y.Data(1e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
hold off;
xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
axis equal
xlim([-2, 2]); ylim([-2, 2]);
xticks([-2:1:2]);
yticks([-2:1:2]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_tomo_1kg_60rpm_xy.pdf', 'width', 'half', 'height', 'normal');
#+end_src

#+begin_src matlab :exports none :results none
%% Simulation of tomography experiment - no payload, 30rpm - YZ errors
figure;
tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e6*exp_tomo_ol_m1.y.y.Data,          1e6*exp_tomo_ol_m1.y.z.Data, 'DisplayName', 'OL')
plot(1e6*exp_tomo_cl_m1.y.y.Data(1e3:end), 1e6*exp_tomo_cl_m1.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
hold off;
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
axis equal
xlim([-2, 2]); ylim([-0.4, 0.4]);
xticks([-2:1:2]);
yticks([-2:0.2:2]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile();
hold on;
plot(1e9*exp_tomo_cl_m1.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m1.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
hold off;
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
axis equal
xlim([-500, 500]); ylim([-100, 100]);
xticks([-500:100:500]);
yticks([-100:50:100]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_tomo_1kg_60rpm_yz.pdf', 'width', 'half', 'height', 'normal');
#+end_src

#+name: fig:nass_tomo_1kg_60rpm
#+caption: Position error of the sample in the XY (\subref{fig:nass_tomo_1kg_60rpm_xy}) and YZ (\subref{fig:nass_tomo_1kg_60rpm_yz}) planes during a simulation of a tomography experiment at $360\,\text{deg/s}$. 1kg payload is placed on top of the nano-hexapod.
#+attr_latex: :options [h!tbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:nass_tomo_1kg_60rpm_xy}XY plane}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :scale 0.9
[[file:figs/nass_tomo_1kg_60rpm_xy.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:nass_tomo_1kg_60rpm_yz}YZ plane}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :scale 0.9
[[file:figs/nass_tomo_1kg_60rpm_yz.png]]
#+end_subfigure
#+end_figure

The robustness of the NASS to payload mass variation was evaluated through additional tomography scan simulations with 25kg and 50kg payloads, complementing the initial 1kg test case.
As illustrated in Figure ref:fig:nass_tomography_hac_iff, system performance exhibits some degradation with increasing payload mass, aligning with predictions from the control analysis.
While the positioning accuracy for heavier payloads is outside the specified limits, it remains within acceptable bounds for typical operating conditions.

It should be noted that the maximum rotational velocity of 360deg/s is primarily intended for lightweight payload applications.
For higher mass configurations, rotational velocities are foreseen to be below 36deg/s.

#+begin_src matlab :exports none :results none
%% Simulation of tomography experiment - no payload, 30rpm - YZ errors
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e9*exp_tomo_cl_m1.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m1.y.z.Data(1e3:end), 'color', colors(1,:), 'DisplayName', '$m = 1$ kg')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
hold off;
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
axis equal
xlim([-200, 200]); ylim([-100, 100]);
xticks([-200:50:200]); yticks([-100:50:100]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_tomography_hac_iff_m1.pdf', 'width', 'third', 'height', 'normal');
#+end_src

#+begin_src matlab :exports none :results none
%% Simulation of tomography experiment - no payload, 30rpm - YZ errors
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e9*exp_tomo_cl_m25.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m25.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', '$m = 25$ kg')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
hold off;
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
axis equal
xlim([-200, 200]); ylim([-100, 100]);
xticks([-200:50:200]); yticks([-100:50:100]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_tomography_hac_iff_m25.pdf', 'width', 'third', 'height', 'normal');
#+end_src

#+begin_src matlab :exports none :results none
%% Simulation of tomography experiment - no payload, 30rpm - YZ errors
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e9*exp_tomo_cl_m50.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m50.y.z.Data(1e3:end), 'color', colors(3,:), 'DisplayName', '$m = 50$ kg')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
hold off;
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
axis equal
xlim([-200, 200]); ylim([-100, 100]);
xticks([-200:50:200]); yticks([-100:50:100]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/nass_tomography_hac_iff_m50.pdf', 'width', 'third', 'height', 'normal');
#+end_src

#+name: fig:nass_tomography_hac_iff
#+caption: Simulation of tomography experiments - 360deg/s. Beam size shown by dashed black
#+attr_latex: :options [h!tbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:nass_tomography_hac_iff_m1} $m = 1\,kg$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/nass_tomography_hac_iff_m1.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:nass_tomography_hac_iff_m25} $m = 25\,kg$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/nass_tomography_hac_iff_m25.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:nass_tomography_hac_iff_m50} $m = 50\,kg$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/nass_tomography_hac_iff_m50.png]]
#+end_subfigure
#+end_figure

* Conclusion
:PROPERTIES:
:UNNUMBERED: t
:END:
<<sec:nass_conclusion>>

The development and analysis presented in this chapter have successfully validated the Nano Active Stabilization System concept, marking the completion of the conceptual design phase.
A comprehensive control strategy has been established, effectively combining external metrology with nano-hexapod sensor measurements to achieve precise position control.
The implementation follows the proven High Authority Control - Low Authority Control architecture, which has been successfully adapted for this specific application.

The decentralized Integral Force Feedback component has been demonstrated to provide robust active damping across various operating conditions.
The addition of parallel springs to the force sensors, has been shown to ensure stability during spindle rotation.
The centralized High Authority Controller, operating in the frame of the struts for simplicity, has achieved the desired performance objectives.
This investigation has confirmed that the moderate actuator stiffness of $1\,N/\mu m$ represents an adequate choice for the nano-hexapod, as both very stiff and very compliant configurations have been shown to introduce significant performance limitations.

The control system has achieved the targeted bandwidth of 10 Hz while maintaining robustness to payload mass variations.
These theoretical predictions have been validated through simulations of tomography experiments, where positioning accuracy requirements were defined by the expected minimum beam dimensions of 200 nm × 100 nm.
The system has demonstrated excellent performance at maximum rotational velocity with lightweight samples.
While some degradation in positioning accuracy has been observed with heavier payloads, as anticipated by the control analysis, the overall performance remains sufficient to validate the fundamental concept of the NASS.

These results provide a solid foundation for advancing to the subsequent detailed design phase and experimental implementation.

* Bibliography                                                        :ignore:
#+latex: \printbibliography[heading=bibintoc,title={Bibliography}]

* Helping Functions                                                 :noexport:
** Initialize Path
#+NAME: m-init-path
#+BEGIN_SRC matlab
addpath('./matlab/');     % Path for scripts

%% Path for functions, data and scripts
addpath('./matlab/mat/'); % Path for Computed FRF
addpath('./matlab/src/'); % Path for functions
addpath('./matlab/STEPS/'); % Path for STEPS
addpath('./matlab/subsystems/'); % Path for Subsystems Simulink files

%% Data directory
data_dir = './matlab/mat/'
#+END_SRC

#+NAME: m-init-path-tangle
#+BEGIN_SRC matlab
%% Path for functions, data and scripts
addpath('./mat/'); % Path for Data
addpath('./src/'); % Path for functions
addpath('./STEPS/'); % Path for STEPS
addpath('./subsystems/'); % Path for Subsystems Simulink files

%% Data directory
data_dir = './mat/';
#+END_SRC

** Initialize Simscape Model
#+NAME: m-init-simscape
#+begin_src matlab
% Simulink Model name
mdl = 'nass_model';
#+end_src

** Initialize other elements
#+NAME: m-init-other
#+BEGIN_SRC matlab
%% Colors for the figures
colors = colororder;

%% Frequency Vector [Hz]
freqs = logspace(0, 3, 1000);
#+END_SRC

* Matlab Functions                                                  :noexport:
** =initializeSimscapeConfiguration=: Simscape Configuration
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeSimscapeConfiguration.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

*** Function description
#+begin_src matlab
  function [] = initializeSimscapeConfiguration(args)
#+end_src

*** Optional Parameters
#+begin_src matlab
  arguments
    args.gravity logical {mustBeNumericOrLogical} = true
  end
#+end_src

*** Structure initialization
#+begin_src matlab
  conf_simscape = struct();
#+end_src

*** Add Type
#+begin_src matlab
  if args.gravity
    conf_simscape.type = 1;
  else
    conf_simscape.type = 2;
  end
#+end_src

*** Save the Structure
#+begin_src matlab
if exist('./mat', 'dir')
    if exist('./mat/nass_model_conf_simscape.mat', 'file')
        save('mat/nass_model_conf_simscape.mat', 'conf_simscape', '-append');
    else
        save('mat/nass_model_conf_simscape.mat', 'conf_simscape');
    end
elseif exist('./matlab', 'dir')
    if exist('./matlab/mat/nass_model_conf_simscape.mat', 'file')
        save('matlab/mat/nass_model_conf_simscape.mat', 'conf_simscape', '-append');
    else
        save('matlab/mat/nass_model_conf_simscape.mat', 'conf_simscape');
    end
end
#+end_src

** =initializeLoggingConfiguration=: Logging Configuration
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeLoggingConfiguration.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

*** Function description
#+begin_src matlab
  function [] = initializeLoggingConfiguration(args)
#+end_src

*** Optional Parameters
#+begin_src matlab
  arguments
    args.log      char   {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none'
    args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
  end
#+end_src

*** Structure initialization
#+begin_src matlab
  conf_log = struct();
#+end_src

*** Add Type
#+begin_src matlab
  switch args.log
    case 'none'
      conf_log.type = 0;
    case 'all'
      conf_log.type = 1;
    case 'forces'
      conf_log.type = 2;
  end
#+end_src

*** Sampling Time
#+begin_src matlab
  conf_log.Ts = args.Ts;
#+end_src

*** Save the Structure
#+begin_src matlab
if exist('./mat', 'dir')
    if exist('./mat/nass_model_conf_log.mat', 'file')
        save('mat/nass_model_conf_log.mat', 'conf_log', '-append');
    else
        save('mat/nass_model_conf_log.mat', 'conf_log');
    end
elseif exist('./matlab', 'dir')
    if exist('./matlab/mat/nass_model_conf_log.mat', 'file')
        save('matlab/mat/nass_model_conf_log.mat', 'conf_log', '-append');
    else
        save('matlab/mat/nass_model_conf_log.mat', 'conf_log');
    end
end
#+end_src

** =initializeReferences=: Generate Reference Signals
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeReferences.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

*** Function Declaration and Documentation
#+begin_src matlab
  function [ref] = initializeReferences(args)
#+end_src

*** Optional Parameters
#+begin_src matlab
  arguments
      % Sampling Frequency [s]
      args.Ts           (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
      % Maximum simulation time [s]
      args.Tmax         (1,1) double {mustBeNumeric, mustBePositive} = 100
      % Either "constant" / "triangular" / "sinusoidal"
      args.Dy_type      char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
      % Amplitude of the displacement [m]
      args.Dy_amplitude (1,1) double {mustBeNumeric} = 0
      % Period of the displacement [s]
      args.Dy_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
      % Either "constant" / "triangular" / "sinusoidal"
      args.Ry_type      char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
      % Amplitude [rad]
      args.Ry_amplitude (1,1) double {mustBeNumeric} = 0
      % Period of the displacement [s]
      args.Ry_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
      % Either "constant" / "rotating"
      args.Rz_type      char {mustBeMember(args.Rz_type,{'constant', 'rotating', 'rotating-not-filtered'})} = 'constant'
      % Initial angle [rad]
      args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
      % Period of the rotating [s]
      args.Rz_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
      % For now, only constant is implemented
      args.Dh_type      char {mustBeMember(args.Dh_type,{'constant'})} = 'constant'
      % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
      args.Dh_pos       (6,1) double {mustBeNumeric} = zeros(6, 1), ...
      % For now, only constant is implemented
      args.Rm_type      char {mustBeMember(args.Rm_type,{'constant'})} = 'constant'
      % Initial position of the two masses
      args.Rm_pos       (2,1) double {mustBeNumeric} = [0; pi]
      % For now, only constant is implemented
      args.Dn_type      char {mustBeMember(args.Dn_type,{'constant'})} = 'constant'
      % Initial position [m,m,m,rad,rad,rad] of the top platform
      args.Dn_pos       (6,1) double {mustBeNumeric} = zeros(6,1)
  end
#+end_src


*** Initialize Parameters
#+begin_src matlab
  %% Set Sampling Time
  Ts = args.Ts;
  Tmax = args.Tmax;

  %% Low Pass Filter to filter out the references
  s = zpk('s');
  w0 = 2*pi*10;
  xi = 1;
  H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
#+end_src

*** Translation Stage
#+begin_src matlab
  %% Translation stage - Dy
  t = 0:Ts:Tmax; % Time Vector [s]
  Dy   = zeros(length(t), 1);
  Dyd  = zeros(length(t), 1);
  Dydd = zeros(length(t), 1);
  switch args.Dy_type
    case 'constant'
      Dy(:)   = args.Dy_amplitude;
      Dyd(:)  = 0;
      Dydd(:) = 0;
    case 'triangular'
      % This is done to unsure that we start with no displacement
      Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
      i0 = find(t>=args.Dy_period/4,1);
      Dy(1:end-i0+1) = Dy_raw(i0:end);
      Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value

      % The signal is filtered out
      Dy   = lsim(H_lpf,     Dy, t);
      Dyd  = lsim(H_lpf*s,   Dy, t);
      Dydd = lsim(H_lpf*s^2, Dy, t);
    case 'sinusoidal'
      Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t);
      Dyd   = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t);
      Dydd  = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t);
    otherwise
      warning('Dy_type is not set correctly');
  end

  Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);
#+end_src

*** Tilt Stage
#+begin_src matlab
  %% Tilt Stage - Ry
  t = 0:Ts:Tmax; % Time Vector [s]
  Ry   = zeros(length(t), 1);
  Ryd  = zeros(length(t), 1);
  Rydd = zeros(length(t), 1);

  switch args.Ry_type
    case 'constant'
      Ry(:) = args.Ry_amplitude;
      Ryd(:)   = 0;
      Rydd(:)  = 0;
    case 'triangular'
      Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
      i0 = find(t>=args.Ry_period/4,1);
      Ry(1:end-i0+1) = Ry_raw(i0:end);
      Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value

      % The signal is filtered out
      Ry   = lsim(H_lpf,     Ry, t);
      Ryd  = lsim(H_lpf*s,   Ry, t);
      Rydd = lsim(H_lpf*s^2, Ry, t);
    case 'sinusoidal'
      Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);

      Ryd  = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t);
      Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t);
    otherwise
      warning('Ry_type is not set correctly');
  end

  Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);
#+end_src

*** Spindle
#+begin_src matlab
  %% Spindle - Rz
  t = 0:Ts:Tmax; % Time Vector [s]
  Rz   = zeros(length(t), 1);
  Rzd  = zeros(length(t), 1);
  Rzdd = zeros(length(t), 1);

  switch args.Rz_type
    case 'constant'
      Rz(:)   = args.Rz_amplitude;
      Rzd(:)  = 0;
      Rzdd(:) = 0;
    case 'rotating-not-filtered'
      Rz(:) = 2*pi/args.Rz_period*t;

      % The signal is filtered out
      Rz(:)   = 2*pi/args.Rz_period*t;
      Rzd(:)  = 2*pi/args.Rz_period;
      Rzdd(:) = 0;

      % We add the angle offset
      Rz = Rz + args.Rz_amplitude;

    case 'rotating'
      Rz(:) = 2*pi/args.Rz_period*t;

      % The signal is filtered out
      Rz   = lsim(H_lpf,     Rz, t);
      Rzd  = lsim(H_lpf*s,   Rz, t);
      Rzdd = lsim(H_lpf*s^2, Rz, t);

      % We add the angle offset
      Rz = Rz + args.Rz_amplitude;
    otherwise
      warning('Rz_type is not set correctly');
  end

  Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);
#+end_src

*** Micro Hexapod
#+begin_src matlab
  %% Micro-Hexapod
  t = [0, Ts];
  Dh = zeros(length(t), 6);
  Dhl = zeros(length(t), 6);

  switch args.Dh_type
    case 'constant'
      Dh = [args.Dh_pos, args.Dh_pos];

      load('nass_model_stages.mat', 'micro_hexapod');

      AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];

      tx = args.Dh_pos(4);
      ty = args.Dh_pos(5);
      tz = args.Dh_pos(6);

      ARB = [cos(tz) -sin(tz) 0;
             sin(tz)  cos(tz) 0;
             0        0       1]*...
            [ cos(ty) 0 sin(ty);
              0       1 0;
             -sin(ty) 0 cos(ty)]*...
            [1 0        0;
             0 cos(tx) -sin(tx);
             0 sin(tx)  cos(tx)];

      [~, Dhl] = inverseKinematics(micro_hexapod, 'AP', AP, 'ARB', ARB);
      Dhl = [Dhl, Dhl];
    otherwise
      warning('Dh_type is not set correctly');
  end

  Dh = struct('time', t, 'signals', struct('values', Dh));
  Dhl = struct('time', t, 'signals', struct('values', Dhl));
#+end_src

*** Save the Structure
#+begin_src matlab
if exist('./mat', 'dir')
    if exist('./mat/nass_model_references.mat', 'file')
        save('mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append');
    else
        save('mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
    end
elseif exist('./matlab', 'dir')
    if exist('./matlab/mat/nass_model_references.mat', 'file')
        save('matlab/mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append');
    else
        save('matlab/mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
    end
end
#+end_src

** =initializeDisturbances=: Initialize Disturbances
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeDisturbances.m
:header-args:matlab+: :comments none :mkdirp yes
:header-args:matlab+: :eval no :results none
:END:

*** Function Declaration and Documentation
#+begin_src matlab
function [] = initializeDisturbances(args)
% initializeDisturbances - Initialize the disturbances
%
% Syntax: [] = initializeDisturbances(args)
%
% Inputs:
%    - args -

#+end_src

*** Optional Parameters
#+begin_src matlab
arguments
    % Global parameter to enable or disable the disturbances
    args.enable logical {mustBeNumericOrLogical} = true
    % Ground Motion - X direction
    args.Dw_x logical {mustBeNumericOrLogical} = true
    % Ground Motion - Y direction
    args.Dw_y logical {mustBeNumericOrLogical} = true
    % Ground Motion - Z direction
    args.Dw_z logical {mustBeNumericOrLogical} = true
    % Translation Stage - X direction
    args.Fdy_x logical {mustBeNumericOrLogical} = true
    % Translation Stage - Z direction
    args.Fdy_z logical {mustBeNumericOrLogical} = true
    % Spindle - X direction
    args.Frz_x logical {mustBeNumericOrLogical} = true
    % Spindle - Y direction
    args.Frz_y logical {mustBeNumericOrLogical} = true
    % Spindle - Z direction
    args.Frz_z logical {mustBeNumericOrLogical} = true
end
#+end_src

#+begin_src matlab
% Initialization of random numbers
rng("shuffle");
#+end_src

*** Ground Motion
#+begin_src matlab
%% Ground Motion
if args.enable
    % Load the PSD of disturbance
    load('ustation_disturbance_psd.mat', 'gm_dist')

    % Frequency Data
    Dw.f = gm_dist.f;
    Dw.psd_x = gm_dist.pxx_x;
    Dw.psd_y = gm_dist.pxx_y;
    Dw.psd_z = gm_dist.pxx_z;

    % Time data
    Fs = 2*Dw.f(end);      % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
    N  = 2*length(Dw.f);   % Number of Samples match the one of the wanted PSD
    T0 = N/Fs;             % Signal Duration [s]
    Dw.t = linspace(0, T0, N+1)'; % Time Vector [s]

    % ASD representation of the ground motion
    C = zeros(N/2,1);
    for i = 1:N/2
        C(i) = sqrt(Dw.psd_x(i)/T0);
    end

    if args.Dw_x
        theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
        Cx = [0 ; C.*complex(cos(theta),sin(theta))];
        Cx = [Cx; flipud(conj(Cx(2:end)))];;
        Dw.x = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
    else
        Dw.x = zeros(length(Dw.t), 1);
    end

    if args.Dw_y
        theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
        Cx = [0 ; C.*complex(cos(theta),sin(theta))];
        Cx = [Cx; flipud(conj(Cx(2:end)))];;
        Dw.y = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
    else
        Dw.y = zeros(length(Dw.t), 1);
    end

    if args.Dw_y
        theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
        Cx = [0 ; C.*complex(cos(theta),sin(theta))];
        Cx = [Cx; flipud(conj(Cx(2:end)))];;
        Dw.z = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
    else
        Dw.z = zeros(length(Dw.t), 1);
    end

else
    Dw.t = [0,1]; % Time Vector [s]
    Dw.x = [0,0]; % Ground Motion - X [m]
    Dw.y = [0,0]; % Ground Motion - Y [m]
    Dw.z = [0,0]; % Ground Motion - Z [m]
end
#+end_src

*** Translation stage
#+begin_src matlab
%% Translation stage
if args.enable
    % Load the PSD of disturbance
    load('ustation_disturbance_psd.mat', 'dy_dist')

    % Frequency Data
    Dy.f = dy_dist.f;
    Dy.psd_x = dy_dist.pxx_fx;
    Dy.psd_z = dy_dist.pxx_fz;

    % Time data
    Fs = 2*Dy.f(end);      % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
    N  = 2*length(Dy.f);   % Number of Samples match the one of the wanted PSD
    T0 = N/Fs;             % Signal Duration [s]
    Dy.t = linspace(0, T0, N+1)'; % Time Vector [s]

    % ASD representation of the disturbance voice
    C = zeros(N/2,1);
    for i = 1:N/2
        C(i) = sqrt(Dy.psd_x(i)/T0);
    end

    if args.Fdy_x
        theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
        Cx = [0 ; C.*complex(cos(theta),sin(theta))];
        Cx = [Cx; flipud(conj(Cx(2:end)))];;
        Dy.x = N/sqrt(2)*ifft(Cx); % Translation stage disturbances - X direction [N]
    else
        Dy.x = zeros(length(Dy.t), 1);
    end

    if args.Fdy_z
        theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
        Cx = [0 ; C.*complex(cos(theta),sin(theta))];
        Cx = [Cx; flipud(conj(Cx(2:end)))];;
        Dy.z = N/sqrt(2)*ifft(Cx); % Translation stage disturbances - Z direction [N]
    else
        Dy.z = zeros(length(Dy.t), 1);
    end

else
    Dy.t = [0,1]; % Time Vector [s]
    Dy.x = [0,0]; % Translation Stage disturbances - X [N]
    Dy.z = [0,0]; % Translation Stage disturbances - Z [N]
end
#+end_src

*** Spindle
#+begin_src matlab
%% Spindle
if args.enable
    % Load the PSD of disturbance
    load('ustation_disturbance_psd.mat', 'rz_dist')

    % Frequency Data
    Rz.f = rz_dist.f;
    Rz.psd_x = rz_dist.pxx_fx;
    Rz.psd_y = rz_dist.pxx_fy;
    Rz.psd_z = rz_dist.pxx_fz;

    % Time data
    Fs = 2*Rz.f(end);      % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
    N  = 2*length(Rz.f);   % Number of Samples match the one of the wanted PSD
    T0 = N/Fs;             % Signal Duration [s]
    Rz.t = linspace(0, T0, N+1)'; % Time Vector [s]

    % ASD representation of the disturbance voice
    C = zeros(N/2,1);
    for i = 1:N/2
        C(i) = sqrt(Rz.psd_x(i)/T0);
    end

    if args.Frz_x
        theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
        Cx = [0 ; C.*complex(cos(theta),sin(theta))];
        Cx = [Cx; flipud(conj(Cx(2:end)))];;
        Rz.x = N/sqrt(2)*ifft(Cx); % spindle disturbances - X direction [N]
    else
        Rz.x = zeros(length(Rz.t), 1);
    end

    if args.Frz_y
        theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
        Cx = [0 ; C.*complex(cos(theta),sin(theta))];
        Cx = [Cx; flipud(conj(Cx(2:end)))];;
        Rz.y = N/sqrt(2)*ifft(Cx); % spindle disturbances - Y direction [N]
    else
        Rz.y = zeros(length(Rz.t), 1);
    end

    if args.Frz_z
        theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
        Cx = [0 ; C.*complex(cos(theta),sin(theta))];
        Cx = [Cx; flipud(conj(Cx(2:end)))];;
        Rz.z = N/sqrt(2)*ifft(Cx); % spindle disturbances - Z direction [N]
    else
        Rz.z = zeros(length(Rz.t), 1);
    end

else
    Rz.t = [0,1]; % Time Vector [s]
    Rz.x = [0,0]; % Spindle disturbances - X [N]
    Rz.y = [0,0]; % Spindle disturbances - X [N]
    Rz.z = [0,0]; % Spindle disturbances - Z [N]
end
#+end_src

*** Direct Forces
#+begin_src matlab
u = zeros(100, 6);
Fd = u;
#+end_src

*** Set initial value to zero
#+begin_src matlab
Dw.x   = Dw.x   - Dw.x(1);
Dw.y   = Dw.y   - Dw.y(1);
Dw.z   = Dw.z   - Dw.z(1);

Dy.x   = Dy.x   - Dy.x(1);
Dy.z   = Dy.z   - Dy.z(1);

Rz.x   = Rz.x   - Rz.x(1);
Rz.y   = Rz.y   - Rz.y(1);
Rz.z   = Rz.z   - Rz.z(1);
#+end_src

*** Save the Structure
#+begin_src matlab
if exist('./mat', 'dir')
    save('mat/nass_model_disturbances.mat', 'Dw', 'Dy', 'Rz', 'Fd', 'args');
elseif exist('./matlab', 'dir')
    save('matlab/mat/nass_model_disturbances.mat', 'Dw', 'Dy', 'Rz', 'Fd', 'args');
end
#+end_src

** =initializeController=: Initialize Controller
#+begin_src matlab :tangle matlab/src/initializeController.m :comments none :mkdirp yes :eval no
function [] = initializeController(args)

    arguments
        args.type char {mustBeMember(args.type,{'open-loop', 'iff', 'dvf', 'hac-dvf', 'ref-track-L', 'ref-track-iff-L', 'cascade-hac-lac', 'hac-iff', 'stabilizing'})} = 'open-loop'
    end

    controller = struct();

    switch args.type
      case 'open-loop'
        controller.type = 1;
        controller.name = 'Open-Loop';
      case 'dvf'
        controller.type = 2;
        controller.name = 'Decentralized Direct Velocity Feedback';
      case 'iff'
        controller.type = 3;
        controller.name = 'Decentralized Integral Force Feedback';
      case 'hac-dvf'
        controller.type = 4;
        controller.name = 'HAC-DVF';
      case 'ref-track-L'
        controller.type = 5;
        controller.name = 'Reference Tracking in the frame of the legs';
      case 'ref-track-iff-L'
        controller.type = 6;
        controller.name = 'Reference Tracking in the frame of the legs + IFF';
      case 'cascade-hac-lac'
        controller.type = 7;
        controller.name = 'Cascade Control + HAC-LAC';
      case 'hac-iff'
        controller.type = 8;
        controller.name = 'HAC-IFF';
      case 'stabilizing'
        controller.type = 9;
        controller.name = 'Stabilizing Controller';
    end

    if exist('./mat', 'dir')
        save('mat/nass_model_controller.mat', 'controller');
    elseif exist('./matlab', 'dir')
        save('matlab/mat/nass_model_controller.mat', 'controller');
    end

end
#+end_src

** =describeMicroStationSetup=
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/describeMicroStationSetup.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

*** Function description
#+begin_src matlab
  function [] = describeMicroStationSetup()
  % describeMicroStationSetup -
  %
  % Syntax: [] = describeMicroStationSetup()
  %
  % Inputs:
  %    -  -
  %
  % Outputs:
  %    -  -
#+end_src

*** Simscape Configuration
#+begin_src matlab
  load('./mat/nass_model_conf_simscape.mat', 'conf_simscape');
#+end_src

#+begin_src matlab
  fprintf('Simscape Configuration:\n');

  if conf_simscape.type == 1
      fprintf('- Gravity is included\n');
  else
      fprintf('- Gravity is not included\n');
  end

  fprintf('\n');
#+end_src

*** Disturbances
#+begin_src matlab
  load('./mat/nass_model_disturbances.mat', 'args');
#+end_src

#+begin_src matlab
  fprintf('Disturbances:\n');
  if ~args.enable
      fprintf('- No disturbance is included\n');
  else
      if args.Dwx && args.Dwy && args.Dwz
          fprintf('- Ground motion\n');
      end
      if args.Fdy_x && args.Fdy_z
          fprintf('- Vibrations of the Translation Stage\n');
      end
      if args.Frz_z
          fprintf('- Vibrations of the Spindle\n');
      end
  end
  fprintf('\n');
#+end_src

*** References
#+begin_src matlab
  load('./mat/nass_model_references.mat', 'args');
#+end_src

#+begin_src matlab
  fprintf('Reference Tracking:\n');
  fprintf('- Translation Stage:\n');
  switch args.Dy_type
    case 'constant'
      fprintf('  - Constant Position\n');
      fprintf('  - Dy = %.0f [mm]\n', args.Dy_amplitude*1e3);
    case 'triangular'
      fprintf('  - Triangular Path\n');
      fprintf('  - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
      fprintf('  - Period = %.0f [s]\n', args.Dy_period);
    case 'sinusoidal'
      fprintf('  - Sinusoidal Path\n');
      fprintf('  - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
      fprintf('  - Period = %.0f [s]\n', args.Dy_period);
  end

  fprintf('- Tilt Stage:\n');
  switch args.Ry_type
    case 'constant'
      fprintf('  - Constant Position\n');
      fprintf('  - Ry = %.0f [mm]\n', args.Ry_amplitude*1e3);
    case 'triangular'
      fprintf('  - Triangular Path\n');
      fprintf('  - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
      fprintf('  - Period = %.0f [s]\n', args.Ry_period);
    case 'sinusoidal'
      fprintf('  - Sinusoidal Path\n');
      fprintf('  - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
      fprintf('  - Period = %.0f [s]\n', args.Ry_period);
  end

  fprintf('- Spindle:\n');
  switch args.Rz_type
    case 'constant'
      fprintf('  - Constant Position\n');
      fprintf('  - Rz = %.0f [deg]\n', 180/pi*args.Rz_amplitude);
    case { 'rotating', 'rotating-not-filtered' }
      fprintf('  - Rotating\n');
      fprintf('  - Speed = %.0f [rpm]\n', 60/args.Rz_period);
  end


  fprintf('- Micro Hexapod:\n');
  switch args.Dh_type
    case 'constant'
      fprintf('  - Constant Position\n');
      fprintf('  - Dh = %.0f, %.0f, %.0f [mm]\n',  args.Dh_pos(1), args.Dh_pos(2), args.Dh_pos(3));
      fprintf('  - Rh = %.0f, %.0f, %.0f [deg]\n', args.Dh_pos(4), args.Dh_pos(5), args.Dh_pos(6));
  end

  fprintf('\n');
#+end_src

*** Micro-Station
#+begin_src matlab
  load('./mat/nass_model_stages.mat', 'ground', 'granite', 'ty', 'ry', 'rz', 'micro_hexapod', 'axisc');
#+end_src

#+begin_src matlab
  fprintf('Micro Station:\n');

  if granite.type == 1 && ...
          ty.type == 1 && ...
          ry.type == 1 && ...
          rz.type == 1 && ...
          micro_hexapod.type == 1;
      fprintf('- All stages are rigid\n');
  elseif granite.type == 2 && ...
          ty.type == 2 && ...
          ry.type == 2 && ...
          rz.type == 2 && ...
          micro_hexapod.type == 2;
      fprintf('- All stages are flexible\n');
  else
      if granite.type == 1 || granite.type == 4
          fprintf('- Granite is rigid\n');
      else
          fprintf('- Granite is flexible\n');
      end
      if ty.type == 1 || ty.type == 4
          fprintf('- Translation Stage is rigid\n');
      else
          fprintf('- Translation Stage is flexible\n');
      end
      if ry.type == 1 || ry.type == 4
          fprintf('- Tilt Stage is rigid\n');
      else
          fprintf('- Tilt Stage is flexible\n');
      end
      if rz.type == 1 || rz.type == 4
          fprintf('- Spindle is rigid\n');
      else
          fprintf('- Spindle is flexible\n');
      end
      if micro_hexapod.type == 1 || micro_hexapod.type == 4
          fprintf('- Micro Hexapod is rigid\n');
      else
          fprintf('- Micro Hexapod is flexible\n');
      end

  end

  fprintf('\n');
#+end_src

** =computeReferencePose=
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/computeReferencePose.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

#+begin_src matlab
  function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
  % computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
  %
  % Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
  %
  % Inputs:
  %    - Dy - Reference of the Translation Stage [m]
  %    - Ry - Reference of the Tilt Stage [rad]
  %    - Rz - Reference of the Spindle [rad]
  %    - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad]
  %    - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad]
  %
  % Outputs:
  %    - WTr -

    %% Translation Stage
    Rty = [1 0 0 0;
           0 1 0 Dy;
           0 0 1 0;
           0 0 0 1];

    %% Tilt Stage - Pure rotating aligned with Ob
    Rry = [ cos(Ry) 0 sin(Ry) 0;
            0       1 0       0;
           -sin(Ry) 0 cos(Ry) 0;
            0       0 0       1];

    %% Spindle - Rotation along the Z axis
    Rrz = [cos(Rz) -sin(Rz) 0 0 ;
           sin(Rz)  cos(Rz) 0 0 ;
           0        0       1 0 ;
           0        0       0 1 ];


    %% Micro-Hexapod
    Rhx = [1 0           0;
           0 cos(Dh(4)) -sin(Dh(4));
           0 sin(Dh(4))  cos(Dh(4))];

    Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
           0           1 0;
           -sin(Dh(5)) 0 cos(Dh(5))];

    Rhz = [cos(Dh(6)) -sin(Dh(6)) 0;
           sin(Dh(6))  cos(Dh(6)) 0;
           0           0          1];

    Rh = [1 0 0 Dh(1) ;
          0 1 0 Dh(2) ;
          0 0 1 Dh(3) ;
          0 0 0 1 ];

    Rh(1:3, 1:3) = Rhz*Rhy*Rhx;

    %% Nano-Hexapod
    Rnx = [1 0           0;
           0 cos(Dn(4)) -sin(Dn(4));
           0 sin(Dn(4))  cos(Dn(4))];

    Rny = [ cos(Dn(5)) 0 sin(Dn(5));
           0           1 0;
           -sin(Dn(5)) 0 cos(Dn(5))];

    Rnz = [cos(Dn(6)) -sin(Dn(6)) 0;
           sin(Dn(6))  cos(Dn(6)) 0;
           0           0          1];

    Rn = [1 0 0 Dn(1) ;
          0 1 0 Dn(2) ;
          0 0 1 Dn(3) ;
          0 0 0 1 ];

    Rn(1:3, 1:3) = Rnz*Rny*Rnx;

    %% Total Homogeneous transformation
    WTr = Rty*Rry*Rrz*Rh*Rn;
  end
#+end_src

** =circlefit=
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/circlefit.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

#+begin_src matlab
function   [xc,yc,R,a] = circlefit(x,y)
%
%   [xc yx R] = circfit(x,y)
%
%   fits a circle  in x,y plane in a more accurate
%   (less prone to ill condition )
%  procedure than circfit2 but using more memory
%  x,y are column vector where (x(i),y(i)) is a measured point
%
%  result is center point (yc,xc) and radius R
%  an optional output is the vector of coeficient a
% describing the circle's equation
%
%   x^2+y^2+a(1)*x+a(2)*y+a(3)=0
%
%  By:  Izhak bucher 25/oct /1991,
    x=x(:); y=y(:);
    a=[x y ones(size(x))]\[-(x.^2+y.^2)];
    xc = -.5*a(1);
    yc = -.5*a(2);
    R  =  sqrt((a(1)^2+a(2)^2)/4-a(3));
#+end_src

** Initialize Micro-Station Stages
*** =initializeGround=: Ground
#+begin_src matlab :tangle matlab/src/initializeGround.m :comments none :mkdirp yes :eval no
function [ground] = initializeGround(args)

    arguments
        args.type char {mustBeMember(args.type,{'none', 'rigid'})} = 'rigid'
        args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) % Rotation point for the ground motion [m]
    end

    ground = struct();

    switch args.type
      case 'none'
        ground.type = 0;
      case 'rigid'
        ground.type = 1;
    end

    ground.shape   = [2, 2, 0.5]; % [m]
    ground.density = 2800;        % [kg/m3]

    ground.rot_point = args.rot_point;

    if exist('./mat', 'dir')
        if exist('./mat/nass_model_stages.mat', 'file')
            save('mat/nass_model_stages.mat', 'ground', '-append');
        else
            save('mat/nass_model_stages.mat', 'ground');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_stages.mat', 'file')
            save('matlab/mat/nass_model_stages.mat', 'ground', '-append');
        else
            save('matlab/mat/nass_model_stages.mat', 'ground');
        end
    end
end
#+end_src

*** =initializeGranite=: Granite
#+begin_src matlab :tangle matlab/src/initializeGranite.m :comments none :mkdirp yes :eval no
function [granite] = initializeGranite(args)

    arguments
        args.type          char    {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = 'flexible'
        args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
        args.K  (6,1) double {mustBeNumeric, mustBeNonnegative} = [5e9;   5e9;   5e9;   2.5e7; 2.5e7; 1e7] % [N/m]
        args.C  (6,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5; 2e4;   2e4; 1e4] % [N/(m/s)]
        args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m]
        args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m]
        args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
        args.sample_pos (1,1) double {mustBeNumeric} = 0.775 % Height of the measurment point [m]
    end

    granite = struct();

    switch args.type
      case 'none'
        granite.type = 0;
      case 'rigid'
        granite.type = 1;
      case 'flexible'
        granite.type = 2;
    end

    granite.density = args.density; % [kg/m3]
    granite.STEP    = 'granite.STEP';

    % Z-offset for the initial position of the sample with respect to the granite top surface.
    granite.sample_pos = args.sample_pos; % [m]

    granite.K = args.K; % [N/m]
    granite.C = args.C; % [N/(m/s)]

    if exist('./mat', 'dir')
        if exist('./mat/nass_model_stages.mat', 'file')
            save('mat/nass_model_stages.mat', 'granite', '-append');
        else
            save('mat/nass_model_stages.mat', 'granite');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_stages.mat', 'file')
            save('matlab/mat/nass_model_stages.mat', 'granite', '-append');
        else
            save('matlab/mat/nass_model_stages.mat', 'granite');
        end
    end

end
#+end_src

*** =initializeTy=: Translation Stage
#+begin_src matlab :tangle matlab/src/initializeTy.m :comments none :mkdirp yes :eval no
function [ty] = initializeTy(args)

    arguments
        args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
    end

    ty = struct();

    switch args.type
      case 'none'
        ty.type = 0;
      case 'rigid'
        ty.type = 1;
      case 'flexible'
        ty.type = 2;
    end

    % Ty Granite frame
    ty.granite_frame.density = 7800; % [kg/m3] => 43kg
    ty.granite_frame.STEP    = 'Ty_Granite_Frame.STEP';

    % Guide Translation Ty
    ty.guide.density         = 7800; % [kg/m3] => 76kg
    ty.guide.STEP            = 'Ty_Guide.STEP';

    % Ty - Guide_Translation12
    ty.guide12.density       = 7800; % [kg/m3]
    ty.guide12.STEP          = 'Ty_Guide_12.STEP';

    % Ty - Guide_Translation11
    ty.guide11.density       = 7800; % [kg/m3]
    ty.guide11.STEP          = 'Ty_Guide_11.STEP';

    % Ty - Guide_Translation22
    ty.guide22.density       = 7800; % [kg/m3]
    ty.guide22.STEP          = 'Ty_Guide_22.STEP';

    % Ty - Guide_Translation21
    ty.guide21.density       = 7800; % [kg/m3]
    ty.guide21.STEP          = 'Ty_Guide_21.STEP';

    % Ty - Plateau translation
    ty.frame.density         = 7800; % [kg/m3]
    ty.frame.STEP            = 'Ty_Stage.STEP';

    % Ty Stator Part
    ty.stator.density        = 5400; % [kg/m3]
    ty.stator.STEP           = 'Ty_Motor_Stator.STEP';

    % Ty Rotor Part
    ty.rotor.density         = 5400; % [kg/m3]
    ty.rotor.STEP            = 'Ty_Motor_Rotor.STEP';

    ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
    ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 1e4]; % [N/(m/s), N*m/(rad/s)]

    if exist('./mat', 'dir')
        if exist('./mat/nass_model_stages.mat', 'file')
            save('mat/nass_model_stages.mat', 'ty', '-append');
        else
            save('mat/nass_model_stages.mat', 'ty');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_stages.mat', 'file')
            save('matlab/mat/nass_model_stages.mat', 'ty', '-append');
        else
            save('matlab/mat/nass_model_stages.mat', 'ty');
        end
    end

end
#+end_src

*** =initializeRy=: Tilt Stage
#+begin_src matlab :tangle matlab/src/initializeRy.m :comments none :mkdirp yes :eval no
function [ry] = initializeRy(args)

    arguments
        args.type          char    {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
        args.Ry_init (1,1) double  {mustBeNumeric} = 0
    end

    ry = struct();

    switch args.type
      case 'none'
        ry.type = 0;
      case 'rigid'
        ry.type = 1;
      case 'flexible'
        ry.type = 2;
    end

    % Ry - Guide for the tilt stage
    ry.guide.density = 7800; % [kg/m3]
    ry.guide.STEP    = 'Tilt_Guide.STEP';

    % Ry - Rotor of the motor
    ry.rotor.density = 2400; % [kg/m3]
    ry.rotor.STEP    = 'Tilt_Motor_Axis.STEP';

    % Ry - Motor
    ry.motor.density = 3200; % [kg/m3]
    ry.motor.STEP    = 'Tilt_Motor.STEP';

    % Ry - Plateau Tilt
    ry.stage.density = 7800; % [kg/m3]
    ry.stage.STEP    = 'Tilt_Stage.STEP';

    % Z-Offset so that the center of rotation matches the sample center;
    ry.z_offset = 0.58178; % [m]

    ry.Ry_init = args.Ry_init; % [rad]

    ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
    ry.C = [1e5;   1e5; 1e5;   3e4;   1e3; 3e4];

    if exist('./mat', 'dir')
        if exist('./mat/nass_model_stages.mat', 'file')
            save('mat/nass_model_stages.mat', 'ry', '-append');
        else
            save('mat/nass_model_stages.mat', 'ry');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_stages.mat', 'file')
            save('matlab/mat/nass_model_stages.mat', 'ry', '-append');
        else
            save('matlab/mat/nass_model_stages.mat', 'ry');
        end
    end

end
#+end_src

*** =initializeRz=: Spindle
#+begin_src matlab :tangle matlab/src/initializeRz.m :comments none :mkdirp yes :eval no
function [rz] = initializeRz(args)

    arguments
        args.type    char    {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
    end

    rz = struct();

    switch args.type
      case 'none'
        rz.type = 0;
      case 'rigid'
        rz.type = 1;
      case 'flexible'
        rz.type = 2;
    end

    % Spindle - Slip Ring
    rz.slipring.density = 7800; % [kg/m3]
    rz.slipring.STEP    = 'Spindle_Slip_Ring.STEP';

    % Spindle - Rotor
    rz.rotor.density    = 7800; % [kg/m3]
    rz.rotor.STEP       = 'Spindle_Rotor.STEP';

    % Spindle - Stator
    rz.stator.density   = 7800; % [kg/m3]
    rz.stator.STEP      = 'Spindle_Stator.STEP';

    rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
    rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];

    if exist('./mat', 'dir')
        if exist('./mat/nass_model_stages.mat', 'file')
            save('mat/nass_model_stages.mat', 'rz', '-append');
        else
            save('mat/nass_model_stages.mat', 'rz');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_stages.mat', 'file')
            save('matlab/mat/nass_model_stages.mat', 'rz', '-append');
        else
            save('matlab/mat/nass_model_stages.mat', 'rz');
        end
    end

end
#+end_src

*** =initializeMicroHexapod=: Micro Hexapod

#+begin_src matlab :tangle matlab/src/initializeMicroHexapod.m :comments none :mkdirp yes :eval no
function [micro_hexapod] = initializeMicroHexapod(args)

    arguments
        args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
        % initializeFramesPositions
        args.H    (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
        args.MO_B (1,1) double {mustBeNumeric} = 270e-3
        % generateGeneralConfiguration
        args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
        args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-3
        args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
        args.MH  (1,1) double {mustBeNumeric, mustBePositive} = 45e-3
        args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 118e-3
        args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
        % initializeStrutDynamics
        args.Ki (1,1) double {mustBeNumeric, mustBeNonnegative} = 2e7
        args.Ci (1,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3
        % initializeCylindricalPlatforms
        args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10
        args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
        args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3
        args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10
        args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
        args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
        % initializeCylindricalStruts
        args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1
        args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
        args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
        args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1
        args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
        args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
        % inverseKinematics
        args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
        args.ARB (3,3) double {mustBeNumeric} = eye(3)
    end

    stewart = initializeStewartPlatform();

    stewart = initializeFramesPositions(stewart, ...
                                        'H', args.H, ...
                                        'MO_B', args.MO_B);

    stewart = generateGeneralConfiguration(stewart, ...
                                           'FH', args.FH, ...
                                           'FR', args.FR, ...
                                           'FTh', args.FTh, ...
                                           'MH', args.MH, ...
                                           'MR', args.MR, ...
                                           'MTh', args.MTh);

    stewart = computeJointsPose(stewart);

    stewart = initializeStrutDynamics(stewart, ...
                                      'k', args.Ki, ...
                                      'c', args.Ci);

    stewart = initializeJointDynamics(stewart, ...
                                      'type_F', '2dof', ...
                                      'type_M', '3dof');

    stewart = initializeCylindricalPlatforms(stewart, ...
                                             'Fpm', args.Fpm, ...
                                             'Fph', args.Fph, ...
                                             'Fpr', args.Fpr, ...
                                             'Mpm', args.Mpm, ...
                                             'Mph', args.Mph, ...
                                             'Mpr', args.Mpr);

    stewart = initializeCylindricalStruts(stewart, ...
                                          'Fsm', args.Fsm, ...
                                          'Fsh', args.Fsh, ...
                                          'Fsr', args.Fsr, ...
                                          'Msm', args.Msm, ...
                                          'Msh', args.Msh, ...
                                          'Msr', args.Msr);

    stewart = computeJacobian(stewart);

    stewart = initializeStewartPose(stewart, ...
                                    'AP', args.AP, ...
                                    'ARB', args.ARB);

    stewart = initializeInertialSensor(stewart, 'type', 'none');

    switch args.type
      case 'none'
        stewart.type = 0;
      case 'rigid'
        stewart.type = 1;
      case 'flexible'
        stewart.type = 2;
    end

    micro_hexapod = stewart;
    if exist('./mat', 'dir')
        if exist('./mat/nass_model_stages.mat', 'file')
            save('mat/nass_model_stages.mat', 'micro_hexapod', '-append');
        else
            save('mat/nass_model_stages.mat', 'micro_hexapod');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_stages.mat', 'file')
            save('matlab/mat/nass_model_stages.mat', 'micro_hexapod', '-append');
        else
            save('matlab/mat/nass_model_stages.mat', 'micro_hexapod');
        end
    end
end
#+end_src

*** =initializeSimplifiedNanoHexapod=: Nano Hexapod

#+begin_src matlab :tangle matlab/src/initializeSimplifiedNanoHexapod.m :comments none :mkdirp yes :eval no
function [nano_hexapod] = initializeSimplifiedNanoHexapod(args)

    arguments
        args.type char {mustBeMember(args.type,{'none', 'stewart'})} = 'stewart'
        %% initializeFramesPositions
        args.H    (1,1) double {mustBeNumeric, mustBePositive} = 95e-3 % Height of the nano-hexapod [m]
        args.MO_B (1,1) double {mustBeNumeric} = 150e-3  % Height of {B} w.r.t. {M} [m]
        %% generateGeneralConfiguration
        args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 % Height of fixed joints [m]
        args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 120e-3 % Radius of fixed joints [m]
        args.FTh (6,1) double {mustBeNumeric} = [220, 320, 340, 80, 100, 200]*(pi/180) % Angles of fixed joints [rad]
        args.MH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 % Height of mobile joints [m]
        args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius of mobile joints [m]
        args.MTh (6,1) double {mustBeNumeric} = [255, 285, 15, 45, 135, 165]*(pi/180) % Angles of fixed joints [rad]
        %% Actuators
        args.actuator_type char {mustBeMember(args.actuator_type,{'1dof', '2dof', 'flexible'})} = '1dof'
        args.actuator_k   (1,1) double {mustBeNumeric, mustBePositive} = 1e6
        args.actuator_kp  (1,1) double {mustBeNumeric, mustBeNonnegative} = 5e4
        args.actuator_ke  (1,1) double {mustBeNumeric, mustBePositive} = 4952605
        args.actuator_ka  (1,1) double {mustBeNumeric, mustBePositive} = 2476302
        args.actuator_c   (1,1) double {mustBeNumeric, mustBePositive} = 50
        args.actuator_cp  (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.actuator_ce  (1,1) double {mustBeNumeric, mustBePositive} = 100
        args.actuator_ca  (1,1) double {mustBeNumeric, mustBePositive} = 50
        %% initializeCylindricalPlatforms
        args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the fixed plate [kg]
        args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the fixed plate [m]
        args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the fixed plate [m]
        args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the mobile plate [kg]
        args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the mobile plate [m]
        args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the mobile plate [m]
        %% initializeCylindricalStruts
        args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the fixed part of the strut [kg]
        args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the fixed part of the struts [m]
        args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m]
        args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the mobile part of the strut [kg]
        args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the mobile part of the struts [m]
        args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m]
        %% Bottom and Top Flexible Joints
        args.flex_type_F char {mustBeMember(args.flex_type_F,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof'
        args.flex_type_M char {mustBeMember(args.flex_type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof'
        args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        %% inverseKinematics
        args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
        args.ARB (3,3) double {mustBeNumeric} = eye(3)
    end

    stewart = initializeStewartPlatform();

    switch args.type
      case 'none'
        stewart.type = 0;
      case 'stewart'
        stewart.type = 1;
    end

    stewart = initializeFramesPositions(stewart, ...
                                        'H', args.H, ...
                                        'MO_B', args.MO_B);

    stewart = generateGeneralConfiguration(stewart, ...
                                           'FH', args.FH, ...
                                           'FR', args.FR, ...
                                           'FTh', args.FTh, ...
                                           'MH', args.MH, ...
                                           'MR', args.MR, ...
                                           'MTh', args.MTh);

    stewart = computeJointsPose(stewart);

    stewart = initializeStrutDynamics(stewart, ...
                                      'type', args.actuator_type, ...
                                      'k',  args.actuator_k, ...
                                      'kp',  args.actuator_kp, ...
                                      'ke', args.actuator_ke, ...
                                      'ka', args.actuator_ka, ...
                                      'c',  args.actuator_c, ...
                                      'cp',  args.actuator_cp, ...
                                      'ce', args.actuator_ce, ...
                                      'ca', args.actuator_ca);

    stewart = initializeJointDynamics(stewart, ...
                                      'type_F', args.flex_type_F, ...
                                      'type_M', args.flex_type_M, ...
                                      'Kf_M', args.Kf_M, ...
                                      'Cf_M', args.Cf_M, ...
                                      'Kt_M', args.Kt_M, ...
                                      'Ct_M', args.Ct_M, ...
                                      'Kf_F', args.Kf_F, ...
                                      'Cf_F', args.Cf_F, ...
                                      'Kt_F', args.Kt_F, ...
                                      'Ct_F', args.Ct_F, ...
                                      'Ka_F', args.Ka_F, ...
                                      'Ca_F', args.Ca_F, ...
                                      'Kr_F', args.Kr_F, ...
                                      'Cr_F', args.Cr_F, ...
                                      'Ka_M', args.Ka_M, ...
                                      'Ca_M', args.Ca_M, ...
                                      'Kr_M', args.Kr_M, ...
                                      'Cr_M', args.Cr_M);

    stewart = initializeCylindricalPlatforms(stewart, ...
                                             'Fpm', args.Fpm, ...
                                             'Fph', args.Fph, ...
                                             'Fpr', args.Fpr, ...
                                             'Mpm', args.Mpm, ...
                                             'Mph', args.Mph, ...
                                             'Mpr', args.Mpr);

    stewart = initializeCylindricalStruts(stewart, ...
                                          'Fsm', args.Fsm, ...
                                          'Fsh', args.Fsh, ...
                                          'Fsr', args.Fsr, ...
                                          'Msm', args.Msm, ...
                                          'Msh', args.Msh, ...
                                          'Msr', args.Msr);

    stewart = computeJacobian(stewart);

    stewart = initializeStewartPose(stewart, ...
                                    'AP', args.AP, ...
                                    'ARB', args.ARB);

    nano_hexapod = stewart;
    if exist('./mat', 'dir')
        if exist('./mat/nass_model_stages.mat', 'file')
            save('mat/nass_model_stages.mat', 'nano_hexapod', '-append');
        else
            save('mat/nass_model_stages.mat', 'nano_hexapod');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_stages.mat', 'file')
            save('matlab/mat/nass_model_stages.mat', 'nano_hexapod', '-append');
        else
            save('matlab/mat/nass_model_stages.mat', 'nano_hexapod');
        end
    end
end
#+end_src

*** =initializeSample=: Sample

#+begin_src matlab :tangle matlab/src/initializeSample.m :comments none :mkdirp yes :eval no
function [sample] = initializeSample(args)

    arguments
        args.type char {mustBeMember(args.type,{'none', 'cylindrical'})} = 'none'
        args.H (1,1) double {mustBeNumeric, mustBePositive} = 250e-3 % Height [m]
        args.R (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius [m]
        args.m (1,1) double {mustBeNumeric, mustBePositive} = 1 % Mass [kg]
    end

    sample = struct();

    switch args.type
      case 'none'
        sample.type = 0;
        sample.m = 0;
      case 'cylindrical'
        sample.type = 1;

        sample.H = args.H;
        sample.R = args.R;
        sample.m = args.m;
    end

    if exist('./mat', 'dir')
        if exist('./mat/nass_model_stages.mat', 'file')
            save('mat/nass_model_stages.mat', 'sample', '-append');
        else
            save('mat/nass_model_stages.mat', 'sample');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_stages.mat', 'file')
            save('matlab/mat/nass_model_stages.mat', 'sample', '-append');
        else
            save('matlab/mat/nass_model_stages.mat', 'sample');
        end
    end

end
#+end_src

** Initialize Nano-Hexapod
*** =initializeStewartPlatform=: Initialize the Stewart Platform structure

#+begin_src matlab :tangle matlab/src/initializeStewartPlatform.m :comments none :mkdirp yes :eval no
function [stewart] = initializeStewartPlatform()
% initializeStewartPlatform - Initialize the stewart structure
%
% Syntax: [stewart] = initializeStewartPlatform(args)
%
% Outputs:
%    - stewart - A structure with the following sub-structures:
%      - platform_F -
%      - platform_M -
%      - joints_F   -
%      - joints_M   -
%      - struts_F   -
%      - struts_M   -
%      - actuators  -
%      - geometry   -
%      - properties -

    stewart = struct();
    stewart.platform_F = struct();
    stewart.platform_M = struct();
    stewart.joints_F   = struct();
    stewart.joints_M   = struct();
    stewart.struts_F   = struct();
    stewart.struts_M   = struct();
    stewart.actuators  = struct();
    stewart.sensors    = struct();
    stewart.sensors.inertial = struct();
    stewart.sensors.force    = struct();
    stewart.sensors.relative = struct();
    stewart.geometry   = struct();
    stewart.kinematics = struct();

end
#+end_src

*** =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M}

#+begin_src matlab :tangle matlab/src/initializeFramesPositions.m :comments none :mkdirp yes :eval no
function [stewart] = initializeFramesPositions(stewart, args)
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
%
% Syntax: [stewart] = initializeFramesPositions(stewart, args)
%
% Inputs:
%    - args - Can have the following fields:
%        - H    [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
%        - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
%
% Outputs:
%    - stewart - A structure with the following fields:
%        - geometry.H      [1x1] - Total Height of the Stewart Platform [m]
%        - geometry.FO_M   [3x1] - Position of {M} with respect to {F} [m]
%        - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]
%        - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]

    arguments
        stewart
        args.H    (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
        args.MO_B (1,1) double {mustBeNumeric} = 50e-3
    end

    H = args.H; % Total Height of the Stewart Platform [m]

    FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]

    MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]

    FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]

    stewart.geometry.H      = H;
    stewart.geometry.FO_M   = FO_M;
    stewart.platform_M.MO_B = MO_B;
    stewart.platform_F.FO_A = FO_A;

end
#+end_src

*** =generateGeneralConfiguration=: Generate a Very General Configuration

#+begin_src matlab :tangle matlab/src/generateGeneralConfiguration.m :comments none :mkdirp yes :eval no
function [stewart] = generateGeneralConfiguration(stewart, args)
% generateGeneralConfiguration - Generate a Very General Configuration
%
% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
%
% Inputs:
%    - args - Can have the following fields:
%        - FH  [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
%        - FR  [1x1] - Radius of the position of the fixed joints in the X-Y [m]
%        - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
%        - MH  [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
%        - FR  [1x1] - Radius of the position of the mobile joints in the X-Y [m]
%        - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
%
% Outputs:
%    - stewart - updated Stewart structure with the added fields:
%        - platform_F.Fa  [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
%        - platform_M.Mb  [3x6] - Its i'th column is the position vector of joint bi with respect to {M}

    arguments
        stewart
        args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
        args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 115e-3;
        args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
        args.MH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
        args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
        args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
    end

    Fa = zeros(3,6);
    Mb = zeros(3,6);

    for i = 1:6
        Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i));  args.FH];
        Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
    end

    stewart.platform_F.Fa = Fa;
    stewart.platform_M.Mb = Mb;

end
#+end_src

*** =computeJointsPose=: Compute the Pose of the Joints

#+begin_src matlab :tangle matlab/src/computeJointsPose.m :comments none :mkdirp yes :eval no
function [stewart] = computeJointsPose(stewart)
% computeJointsPose -
%
% Syntax: [stewart] = computeJointsPose(stewart)
%
% Inputs:
%    - stewart - A structure with the following fields
%        - platform_F.Fa   [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
%        - platform_M.Mb   [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
%        - platform_F.FO_A [3x1] - Position of {A} with respect to {F}
%        - platform_M.MO_B [3x1] - Position of {B} with respect to {M}
%        - geometry.FO_M   [3x1] - Position of {M} with respect to {F}
%
% Outputs:
%    - stewart - A structure with the following added fields
%        - geometry.Aa    [3x6]   - The i'th column is the position of ai with respect to {A}
%        - geometry.Ab    [3x6]   - The i'th column is the position of bi with respect to {A}
%        - geometry.Ba    [3x6]   - The i'th column is the position of ai with respect to {B}
%        - geometry.Bb    [3x6]   - The i'th column is the position of bi with respect to {B}
%        - geometry.l     [6x1]   - The i'th element is the initial length of strut i
%        - geometry.As    [3x6]   - The i'th column is the unit vector of strut i expressed in {A}
%        - geometry.Bs    [3x6]   - The i'th column is the unit vector of strut i expressed in {B}
%        - struts_F.l     [6x1]   - Length of the Fixed part of the i'th strut
%        - struts_M.l     [6x1]   - Length of the Mobile part of the i'th strut
%        - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
%        - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}

    assert(isfield(stewart.platform_F, 'Fa'),   'stewart.platform_F should have attribute Fa')
    Fa = stewart.platform_F.Fa;

    assert(isfield(stewart.platform_M, 'Mb'),   'stewart.platform_M should have attribute Mb')
    Mb = stewart.platform_M.Mb;

    assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
    FO_A = stewart.platform_F.FO_A;

    assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
    MO_B = stewart.platform_M.MO_B;

    assert(isfield(stewart.geometry,   'FO_M'), 'stewart.geometry should have attribute FO_M')
    FO_M = stewart.geometry.FO_M;

    Aa = Fa - repmat(FO_A, [1, 6]);
    Bb = Mb - repmat(MO_B, [1, 6]);

    Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]);
    Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);

    As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As

    l = vecnorm(Ab - Aa)';

    Bs = (Bb - Ba)./vecnorm(Bb - Ba);

    FRa = zeros(3,3,6);
    MRb = zeros(3,3,6);

    for i = 1:6
        FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)];
        FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i));

        MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
        MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
    end

    stewart.geometry.Aa = Aa;
    stewart.geometry.Ab = Ab;
    stewart.geometry.Ba = Ba;
    stewart.geometry.Bb = Bb;
    stewart.geometry.As = As;
    stewart.geometry.Bs = Bs;
    stewart.geometry.l  = l;

    stewart.struts_F.l  = l/2;
    stewart.struts_M.l  = l/2;

    stewart.platform_F.FRa = FRa;
    stewart.platform_M.MRb = MRb;

end
#+end_src

*** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms

#+begin_src matlab :tangle matlab/src/initializeCylindricalPlatforms.m :comments none :mkdirp yes :eval no
function [stewart] = initializeCylindricalPlatforms(stewart, args)
% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
%
% Syntax: [stewart] = initializeCylindricalPlatforms(args)
%
% Inputs:
%    - args - Structure with the following fields:
%        - Fpm [1x1] - Fixed Platform Mass [kg]
%        - Fph [1x1] - Fixed Platform Height [m]
%        - Fpr [1x1] - Fixed Platform Radius [m]
%        - Mpm [1x1] - Mobile Platform Mass [kg]
%        - Mph [1x1] - Mobile Platform Height [m]
%        - Mpr [1x1] - Mobile Platform Radius [m]
%
% Outputs:
%    - stewart - updated Stewart structure with the added fields:
%      - platform_F [struct] - structure with the following fields:
%        - type = 1
%        - M [1x1] - Fixed Platform Mass [kg]
%        - I [3x3] - Fixed Platform Inertia matrix [kg*m^2]
%        - H [1x1] - Fixed Platform Height [m]
%        - R [1x1] - Fixed Platform Radius [m]
%      - platform_M [struct] - structure with the following fields:
%        - M [1x1] - Mobile Platform Mass [kg]
%        - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]
%        - H [1x1] - Mobile Platform Height [m]
%        - R [1x1] - Mobile Platform Radius [m]

    arguments
        stewart
        args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
        args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
        args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3
        args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
        args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
        args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
    end

    I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
                1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
                1/2 *args.Fpm * args.Fpr^2]);

    I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
                1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
                1/2 *args.Mpm * args.Mpr^2]);

    stewart.platform_F.type = 1;

    stewart.platform_F.I = I_F;
    stewart.platform_F.M = args.Fpm;
    stewart.platform_F.R = args.Fpr;
    stewart.platform_F.H = args.Fph;

    stewart.platform_M.type = 1;

    stewart.platform_M.I = I_M;
    stewart.platform_M.M = args.Mpm;
    stewart.platform_M.R = args.Mpr;
    stewart.platform_M.H = args.Mph;

end
#+end_src

*** =initializeCylindricalStruts=: Define the inertia of cylindrical struts

#+begin_src matlab :tangle matlab/src/initializeCylindricalStruts.m :comments none :mkdirp yes :eval no
function [stewart] = initializeCylindricalStruts(stewart, args)
% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
%
% Syntax: [stewart] = initializeCylindricalStruts(args)
%
% Inputs:
%    - args - Structure with the following fields:
%        - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
%        - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
%        - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
%        - Msm [1x1] - Mass of the Mobile part of the struts [kg]
%        - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
%        - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
%
% Outputs:
%    - stewart - updated Stewart structure with the added fields:
%      - struts_F [struct] - structure with the following fields:
%        - M [6x1]   - Mass of the Fixed part of the struts [kg]
%        - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
%        - H [6x1]   - Height of cylinder for the Fixed part of the struts [m]
%        - R [6x1]   - Radius of cylinder for the Fixed part of the struts [m]
%      - struts_M [struct] - structure with the following fields:
%        - M [6x1]   - Mass of the Mobile part of the struts [kg]
%        - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
%        - H [6x1]   - Height of cylinder for the Mobile part of the struts [m]
%        - R [6x1]   - Radius of cylinder for the Mobile part of the struts [m]

    arguments
        stewart
        args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
        args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
        args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
        args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
        args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
        args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
    end

    stewart.struts_M.type = 1;

    %% Compute the properties of the cylindrical struts
    Fsm = args.Fsm;
    Fsh = args.Fsh;
    Fsr = args.Fsr;

    Msm = args.Msm;
    Msh = args.Msh;
    Msr = args.Msr;

    I_F = [1/12 * Fsm * (3*Fsr^2 + Fsh^2), ...
           1/12 * Fsm * (3*Fsr^2 + Fsh^2), ...
           1/2  * Fsm * Fsr^2];

    I_M = [1/12 * Msm * (3*Msr^2 + Msh^2), ...
           1/12 * Msm * (3*Msr^2 + Msh^2), ...
           1/2  * Msm * Msr^2];
    stewart.struts_M.I = I_M;
    stewart.struts_F.I = I_F;

    stewart.struts_M.M = args.Msm;
    stewart.struts_M.R = args.Msr;
    stewart.struts_M.H = args.Msh;

    stewart.struts_F.type = 1;

    stewart.struts_F.M = args.Fsm;
    stewart.struts_F.R = args.Fsr;
    stewart.struts_F.H = args.Fsh;

end
#+end_src

*** =initializeStrutDynamics=: Add Stiffness and Damping properties of each strut

#+begin_src matlab :tangle matlab/src/initializeStrutDynamics.m :comments none :mkdirp yes :eval no
function [stewart] = initializeStrutDynamics(stewart, args)
% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
%
% Syntax: [stewart] = initializeStrutDynamics(args)
%
% Inputs:
%    - args - Structure with the following fields:
%        - K [6x1] - Stiffness of each strut [N/m]
%        - C [6x1] - Damping of each strut [N/(m/s)]
%
% Outputs:
%    - stewart - updated Stewart structure with the added fields:
%      - actuators.type = 1
%      - actuators.K [6x1] - Stiffness of each strut [N/m]
%      - actuators.C [6x1] - Damping of each strut [N/(m/s)]

    arguments
        stewart
        args.type char {mustBeMember(args.type,{'1dof', '2dof', 'flexible'})} = '1dof'
        args.k  (1,1) double {mustBeNumeric, mustBeNonnegative} = 20e6
        args.kp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.ke (1,1) double {mustBeNumeric, mustBeNonnegative} = 5e6
        args.ka (1,1) double {mustBeNumeric, mustBeNonnegative} = 60e6
        args.c  (1,1) double {mustBeNumeric, mustBeNonnegative} = 2e1
        args.cp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.ce (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e6
        args.ca (1,1) double {mustBeNumeric, mustBeNonnegative} = 10

        args.F_gain (1,1) double {mustBeNumeric} = 1
        args.me (1,1) double {mustBeNumeric} = 0.01
        args.ma (1,1) double {mustBeNumeric} = 0.01
    end

    if strcmp(args.type, '1dof')
        stewart.actuators.type = 1;
    elseif strcmp(args.type, '2dof')
        stewart.actuators.type = 2;
    elseif strcmp(args.type, 'flexible')
        stewart.actuators.type = 3;
    end

    stewart.actuators.k = args.k;
    stewart.actuators.c = args.c;

    % Parallel stiffness
    stewart.actuators.kp = args.kp;
    stewart.actuators.cp = args.cp;

    stewart.actuators.ka = args.ka;
    stewart.actuators.ca = args.ca;

    stewart.actuators.ke = args.ke;
    stewart.actuators.ce = args.ce;

    stewart.actuators.F_gain = args.F_gain;

    stewart.actuators.ma = args.ma;
    stewart.actuators.me = args.me;

end
#+end_src

*** =initializeJointDynamics=: Add Stiffness and Damping properties for spherical joints

#+begin_src matlab :tangle matlab/src/initializeJointDynamics.m :comments none :mkdirp yes :eval no
function [stewart] = initializeJointDynamics(stewart, args)
% initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints
%
% Syntax: [stewart] = initializeJointDynamics(args)
%
% Inputs:
%    - args - Structure with the following fields:
%        - type_F - 'universal', 'spherical', 'universal_p', 'spherical_p'
%        - type_M - 'universal', 'spherical', 'universal_p', 'spherical_p'
%        - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad]
%        - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad]
%        - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)]
%        - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)]
%        - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad]
%        - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad]
%        - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)]
%        - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)]
%
% Outputs:
%    - stewart - updated Stewart structure with the added fields:
%      - stewart.joints_F and stewart.joints_M:
%        - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect)
%        - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m]
%        - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad]
%        - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad]
%        - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)]
%        - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)]
%        - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)]

    arguments
        stewart
        args.type_F     char   {mustBeMember(args.type_F,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof'
        args.type_M     char   {mustBeMember(args.type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof'
        args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
        args.K_M        double {mustBeNumeric} = zeros(6,6)
        args.M_M        double {mustBeNumeric} = zeros(6,6)
        args.n_xyz_M    double {mustBeNumeric} = zeros(2,3)
        args.xi_M       double {mustBeNumeric} = 0.1
        args.step_file_M char {} = ''
        args.K_F        double {mustBeNumeric} = zeros(6,6)
        args.M_F        double {mustBeNumeric} = zeros(6,6)
        args.n_xyz_F    double {mustBeNumeric} = zeros(2,3)
        args.xi_F       double {mustBeNumeric} = 0.1
        args.step_file_F char {} = ''
    end

    switch args.type_F
      case '2dof'
        stewart.joints_F.type = 1;
      case '3dof'
        stewart.joints_F.type = 2;
      case '4dof'
        stewart.joints_F.type = 3;
      case '6dof'
        stewart.joints_F.type = 4;
      case 'flexible'
        stewart.joints_F.type = 5;
      otherwise
        error("joints_F are not correctly defined")
    end

    switch args.type_M
      case '2dof'
        stewart.joints_M.type = 1;
      case '3dof'
        stewart.joints_M.type = 2;
      case '4dof'
        stewart.joints_M.type = 3;
      case '6dof'
        stewart.joints_M.type = 4;
      case 'flexible'
        stewart.joints_M.type = 5;
      otherwise
        error("joints_M are not correctly defined")
    end

    stewart.joints_M.Ka = args.Ka_M;
    stewart.joints_M.Kr = args.Kr_M;

    stewart.joints_F.Ka = args.Ka_F;
    stewart.joints_F.Kr = args.Kr_F;

    stewart.joints_M.Ca = args.Ca_M;
    stewart.joints_M.Cr = args.Cr_M;

    stewart.joints_F.Ca = args.Ca_F;
    stewart.joints_F.Cr = args.Cr_F;

    stewart.joints_M.Kf = args.Kf_M;
    stewart.joints_M.Kt = args.Kt_M;

    stewart.joints_F.Kf = args.Kf_F;
    stewart.joints_F.Kt = args.Kt_F;

    stewart.joints_M.Cf = args.Cf_M;
    stewart.joints_M.Ct = args.Ct_M;

    stewart.joints_F.Cf = args.Cf_F;
    stewart.joints_F.Ct = args.Ct_F;

    stewart.joints_F.M = args.M_F;
    stewart.joints_F.K = args.K_F;
    stewart.joints_F.n_xyz = args.n_xyz_F;
    stewart.joints_F.xi = args.xi_F;
    stewart.joints_F.xi = args.xi_F;
    stewart.joints_F.step_file = args.step_file_F;

    stewart.joints_M.M = args.M_M;
    stewart.joints_M.K = args.K_M;
    stewart.joints_M.n_xyz = args.n_xyz_M;
    stewart.joints_M.xi = args.xi_M;
    stewart.joints_M.step_file = args.step_file_M;

end
#+end_src

*** =initializeStewartPose=: Determine the initial stroke in each leg to have the wanted pose

#+begin_src matlab :tangle matlab/src/initializeStewartPose.m :comments none :mkdirp yes :eval no
function [stewart] = initializeStewartPose(stewart, args)
% initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose
%                         It uses the inverse kinematic
%
% Syntax: [stewart] = initializeStewartPose(stewart, args)
%
% Inputs:
%    - stewart - A structure with the following fields
%        - Aa   [3x6] - The positions ai expressed in {A}
%        - Bb   [3x6] - The positions bi expressed in {B}
%    - args - Can have the following fields:
%        - AP   [3x1] - The wanted position of {B} with respect to {A}
%        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
%
% Outputs:
%    - stewart - updated Stewart structure with the added fields:
%      - actuators.Leq [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}

    arguments
        stewart
        args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
        args.ARB (3,3) double {mustBeNumeric} = eye(3)
    end

    [Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);

    stewart.actuators.Leq = dLi;

end
#+end_src

*** =computeJacobian=: Compute the Jacobian Matrix

#+begin_src matlab :tangle matlab/src/computeJacobian.m :comments none :mkdirp yes :eval no
function [stewart] = computeJacobian(stewart)
% computeJacobian -
%
% Syntax: [stewart] = computeJacobian(stewart)
%
% Inputs:
%    - stewart - With at least the following fields:
%      - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
%      - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
%      - actuators.K [6x1] - Total stiffness of the actuators
%
% Outputs:
%    - stewart - With the 3 added field:
%        - geometry.J [6x6] - The Jacobian Matrix
%        - geometry.K [6x6] - The Stiffness Matrix
%        - geometry.C [6x6] - The Compliance Matrix

    assert(isfield(stewart.geometry, 'As'),   'stewart.geometry should have attribute As')
    As = stewart.geometry.As;

    assert(isfield(stewart.geometry, 'Ab'),   'stewart.geometry should have attribute Ab')
    Ab = stewart.geometry.Ab;

    assert(isfield(stewart.actuators, 'k'),   'stewart.actuators should have attribute k')
    Ki = stewart.actuators.k;

    J = [As' , cross(Ab, As)'];

    K = J'*diag(Ki)*J;

    C = inv(K);

    stewart.geometry.J = J;
    stewart.geometry.K = K;
    stewart.geometry.C = C;

end
#+end_src

*** =inverseKinematics=: Compute Inverse Kinematics

#+begin_src matlab :tangle matlab/src/inverseKinematics.m :comments none :mkdirp yes :eval no
function [Li, dLi] = inverseKinematics(stewart, args)
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
%
% Syntax: [stewart] = inverseKinematics(stewart)
%
% Inputs:
%    - stewart - A structure with the following fields
%        - geometry.Aa   [3x6] - The positions ai expressed in {A}
%        - geometry.Bb   [3x6] - The positions bi expressed in {B}
%        - geometry.l    [6x1] - Length of each strut
%    - args - Can have the following fields:
%        - AP   [3x1] - The wanted position of {B} with respect to {A}
%        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
%
% Outputs:
%    - Li   [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
%    - dLi  [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}

    arguments
        stewart
        args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
        args.ARB (3,3) double {mustBeNumeric} = eye(3)
    end

    assert(isfield(stewart.geometry, 'Aa'),   'stewart.geometry should have attribute Aa')
    Aa = stewart.geometry.Aa;

    assert(isfield(stewart.geometry, 'Bb'),   'stewart.geometry should have attribute Bb')
    Bb = stewart.geometry.Bb;

    assert(isfield(stewart.geometry, 'l'),   'stewart.geometry should have attribute l')
    l = stewart.geometry.l;

    Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));

    dLi = Li-l;

end
#+end_src

*** =displayArchitecture=: 3D plot of the Stewart platform architecture
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/displayArchitecture.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:displayArchitecture>>

This Matlab function is accessible [[file:../src/displayArchitecture.m][here]].

**** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
function [] = displayArchitecture(stewart, args)
% displayArchitecture - 3D plot of the Stewart platform architecture
%
% Syntax: [] = displayArchitecture(args)
%
% Inputs:
%    - stewart
%    - args - Structure with the following fields:
%        - AP   [3x1] - The wanted position of {B} with respect to {A}
%        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
%        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
%        - F_color [color] - Color used for the Fixed elements
%        - M_color [color] - Color used for the Mobile elements
%        - L_color [color] - Color used for the Legs elements
%        - frames    [true/false] - Display the Frames
%        - legs      [true/false] - Display the Legs
%        - joints    [true/false] - Display the Joints
%        - labels    [true/false] - Display the Labels
%        - platforms [true/false] - Display the Platforms
%        - views     ['all', 'xy', 'yz', 'xz', 'default'] -
%
% Outputs:
#+end_src

**** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
arguments
    stewart
    args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
    args.ARB (3,3) double {mustBeNumeric} = eye(3)
    args.F_color = [0 0.4470 0.7410]
    args.M_color = [0.8500 0.3250 0.0980]
    args.L_color = [0 0 0]
    args.frames    logical {mustBeNumericOrLogical} = true
    args.legs      logical {mustBeNumericOrLogical} = true
    args.joints    logical {mustBeNumericOrLogical} = true
    args.labels    logical {mustBeNumericOrLogical} = true
    args.platforms logical {mustBeNumericOrLogical} = true
    args.views     char    {mustBeMember(args.views,{'all', 'xy', 'xz', 'yz', 'default'})} = 'default'
end
#+end_src

**** Check the =stewart= structure elements
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
FO_A = stewart.platform_F.FO_A;

assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
MO_B = stewart.platform_M.MO_B;

assert(isfield(stewart.geometry, 'H'),   'stewart.geometry should have attribute H')
H = stewart.geometry.H;

assert(isfield(stewart.platform_F, 'Fa'),   'stewart.platform_F should have attribute Fa')
Fa = stewart.platform_F.Fa;

assert(isfield(stewart.platform_M, 'Mb'),   'stewart.platform_M should have attribute Mb')
Mb = stewart.platform_M.Mb;
#+end_src


**** Figure Creation, Frames and Homogeneous transformations
:PROPERTIES:
:UNNUMBERED: t
:END:

The reference frame of the 3d plot corresponds to the frame $\{F\}$.
#+begin_src matlab
if ~strcmp(args.views, 'all')
    figure;
else
    f = figure('visible', 'off');
end

hold on;
#+end_src

We first compute homogeneous matrices that will be useful to position elements on the figure where the reference frame is $\{F\}$.
#+begin_src matlab
FTa = [eye(3), FO_A; ...
       zeros(1,3), 1];
ATb = [args.ARB, args.AP; ...
       zeros(1,3), 1];
BTm = [eye(3), -MO_B; ...
       zeros(1,3), 1];

FTm = FTa*ATb*BTm;
#+end_src

Let's define a parameter that define the length of the unit vectors used to display the frames.
#+begin_src matlab
d_unit_vector = H/4;
#+end_src

Let's define a parameter used to position the labels with respect to the center of the element.
#+begin_src matlab
d_label = H/20;
#+end_src

**** Fixed Base elements
:PROPERTIES:
:UNNUMBERED: t
:END:
Let's first plot the frame $\{F\}$.
#+begin_src matlab
Ff = [0, 0, 0];
if args.frames
    quiver3(Ff(1)*ones(1,3), Ff(2)*ones(1,3), Ff(3)*ones(1,3), ...
            [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)

    if args.labels
        text(Ff(1) + d_label, ...
             Ff(2) + d_label, ...
             Ff(3) + d_label, '$\{F\}$', 'Color', args.F_color);
    end
end
#+end_src

Now plot the frame $\{A\}$ fixed to the Base.
#+begin_src matlab
if args.frames
    quiver3(FO_A(1)*ones(1,3), FO_A(2)*ones(1,3), FO_A(3)*ones(1,3), ...
            [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)

    if args.labels
        text(FO_A(1) + d_label, ...
             FO_A(2) + d_label, ...
             FO_A(3) + d_label, '$\{A\}$', 'Color', args.F_color);
    end
end
#+end_src

Let's then plot the circle corresponding to the shape of the Fixed base.
#+begin_src matlab
if args.platforms && stewart.platform_F.type == 1
    theta = [0:0.01:2*pi+0.01]; % Angles [rad]
    v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal
    center = [0; 0; 0]; % Center of the circle
    radius = stewart.platform_F.R; % Radius of the circle [m]

    points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));

    plot3(points(1,:), ...
          points(2,:), ...
          points(3,:), '-', 'Color', args.F_color);
end
#+end_src

Let's now plot the position and labels of the Fixed Joints
#+begin_src matlab
if args.joints
    scatter3(Fa(1,:), ...
             Fa(2,:), ...
             Fa(3,:), 'MarkerEdgeColor', args.F_color);
    if args.labels
        for i = 1:size(Fa,2)
            text(Fa(1,i) + d_label, ...
                 Fa(2,i), ...
                 Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color);
        end
    end
end
#+end_src

**** Mobile Platform elements
:PROPERTIES:
:UNNUMBERED: t
:END:

Plot the frame $\{M\}$.
#+begin_src matlab
Fm = FTm*[0; 0; 0; 1]; % Get the position of frame {M} w.r.t. {F}

if args.frames
    FM_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
    quiver3(Fm(1)*ones(1,3), Fm(2)*ones(1,3), Fm(3)*ones(1,3), ...
            FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', args.M_color)

    if args.labels
        text(Fm(1) + d_label, ...
             Fm(2) + d_label, ...
             Fm(3) + d_label, '$\{M\}$', 'Color', args.M_color);
    end
end
#+end_src

Plot the frame $\{B\}$.
#+begin_src matlab
FB = FO_A + args.AP;

if args.frames
    FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
    quiver3(FB(1)*ones(1,3), FB(2)*ones(1,3), FB(3)*ones(1,3), ...
            FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', args.M_color)

    if args.labels
        text(FB(1) - d_label, ...
             FB(2) + d_label, ...
             FB(3) + d_label, '$\{B\}$', 'Color', args.M_color);
    end
end
#+end_src

Let's then plot the circle corresponding to the shape of the Mobile platform.
#+begin_src matlab
if args.platforms && stewart.platform_M.type == 1
    theta = [0:0.01:2*pi+0.01]; % Angles [rad]
    v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal
    center = Fm(1:3); % Center of the circle
    radius = stewart.platform_M.R; % Radius of the circle [m]

    points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));

    plot3(points(1,:), ...
          points(2,:), ...
          points(3,:), '-', 'Color', args.M_color);
end
#+end_src

Plot the position and labels of the rotation joints fixed to the mobile platform.
#+begin_src matlab
if args.joints
    Fb = FTm*[Mb;ones(1,6)];

    scatter3(Fb(1,:), ...
             Fb(2,:), ...
             Fb(3,:), 'MarkerEdgeColor', args.M_color);

    if args.labels
        for i = 1:size(Fb,2)
            text(Fb(1,i) + d_label, ...
                 Fb(2,i), ...
                 Fb(3,i), sprintf('$b_{%i}$', i), 'Color', args.M_color);
        end
    end
end
#+end_src

**** Legs
:PROPERTIES:
:UNNUMBERED: t
:END:
Plot the legs connecting the joints of the fixed base to the joints of the mobile platform.
#+begin_src matlab
if args.legs
    for i = 1:6
        plot3([Fa(1,i), Fb(1,i)], ...
              [Fa(2,i), Fb(2,i)], ...
              [Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color);

        if args.labels
            text((Fa(1,i)+Fb(1,i))/2 + d_label, ...
                 (Fa(2,i)+Fb(2,i))/2, ...
                 (Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color);
        end
    end
end
#+end_src

**** Figure parameters
#+begin_src matlab
switch args.views
  case 'default'
    view([1 -0.6 0.4]);
  case 'xy'
    view([0 0 1]);
  case 'xz'
    view([0 -1 0]);
  case 'yz'
    view([1 0 0]);
end
axis equal;
axis off;
#+end_src

**** Subplots
#+begin_src matlab
if strcmp(args.views, 'all')
    hAx = findobj('type', 'axes');

    figure;
    s1 = subplot(2,2,1);
    copyobj(get(hAx(1), 'Children'), s1);
    view([0 0 1]);
    axis equal;
    axis off;
    title('Top')

    s2 = subplot(2,2,2);
    copyobj(get(hAx(1), 'Children'), s2);
    view([1 -0.6 0.4]);
    axis equal;
    axis off;

    s3 = subplot(2,2,3);
    copyobj(get(hAx(1), 'Children'), s3);
    view([1 0 0]);
    axis equal;
    axis off;
    title('Front')

    s4 = subplot(2,2,4);
    copyobj(get(hAx(1), 'Children'), s4);
    view([0 -1 0]);
    axis equal;
    axis off;
    title('Side')

    close(f);
end
#+end_src


*** =describeStewartPlatform=: Display some text describing the current defined Stewart Platform
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/describeStewartPlatform.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:describeStewartPlatform>>

This Matlab function is accessible [[file:../src/describeStewartPlatform.m][here]].

**** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
function [] = describeStewartPlatform(stewart)
% describeStewartPlatform - Display some text describing the current defined Stewart Platform
%
% Syntax: [] = describeStewartPlatform(args)
%
% Inputs:
%    - stewart
%
% Outputs:
#+end_src

**** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
arguments
    stewart
end
#+end_src

**** Geometry
#+begin_src matlab
fprintf('GEOMETRY:\n')
fprintf('- The height between the fixed based and the top platform is %.3g [mm].\n', 1e3*stewart.geometry.H)

if stewart.platform_M.MO_B(3) > 0
    fprintf('- Frame {A} is located %.3g [mm] above the top platform.\n',  1e3*stewart.platform_M.MO_B(3))
else
    fprintf('- Frame {A} is located %.3g [mm] below the top platform.\n', - 1e3*stewart.platform_M.MO_B(3))
end

fprintf('- The initial length of the struts are:\n')
fprintf('\t %.3g, %.3g, %.3g, %.3g, %.3g, %.3g [mm]\n', 1e3*stewart.geometry.l)
fprintf('\n')
#+end_src

**** Actuators
#+begin_src matlab
fprintf('ACTUATORS:\n')
if stewart.actuators.type == 1
    fprintf('- The actuators are modelled as 1DoF.\n')
    fprintf('- The Stiffness and Damping of each actuators is:\n')
    fprintf('\t k = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.k(1), stewart.actuators.c(1))
    if stewart.actuators.kp > 0
    fprintf('\t Added parallel stiffness: kp = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.kp(1))
    end
elseif stewart.actuators.type == 2
    fprintf('- The actuators are modelled as 2DoF (APA).\n')
    fprintf('- The vertical stiffness and damping contribution of the piezoelectric stack is:\n')
    fprintf('\t ka = %.0e [N/m] \t ca = %.0e [N/(m/s)]\n', stewart.actuators.ka(1), stewart.actuators.ca(1))
    fprintf('- Vertical stiffness when the piezoelectric stack is removed is:\n')
    fprintf('\t kr = %.0e [N/m] \t cr = %.0e [N/(m/s)]\n', stewart.actuators.kr(1), stewart.actuators.cr(1))
elseif stewart.actuators.type == 3
    fprintf('- The actuators are modelled with a flexible element (FEM).\n')
end
fprintf('\n')
#+end_src

**** Joints
#+begin_src matlab
fprintf('JOINTS:\n')
#+end_src

Type of the joints on the fixed base.
#+begin_src matlab
switch stewart.joints_F.type
  case 1
    fprintf('- The joints on the fixed based are universal joints (2DoF)\n')
  case 2
    fprintf('- The joints on the fixed based are spherical joints (3DoF)\n')
end
#+end_src

Type of the joints on the mobile platform.
#+begin_src matlab
switch stewart.joints_M.type
  case 1
    fprintf('- The joints on the mobile based are universal joints (2DoF)\n')
  case 2
    fprintf('- The joints on the mobile based are spherical joints (3DoF)\n')
end
#+end_src

Position of the fixed joints
#+begin_src matlab
fprintf('- The position of the joints on the fixed based with respect to {F} are (in [mm]):\n')
fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_F.Fa)
#+end_src

Position of the mobile joints
#+begin_src matlab
fprintf('- The position of the joints on the mobile based with respect to {M} are (in [mm]):\n')
fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_M.Mb)
fprintf('\n')
#+end_src

**** Kinematics
#+begin_src matlab
fprintf('KINEMATICS:\n')

if isfield(stewart.kinematics, 'K')
    fprintf('- The Stiffness matrix K is (in [N/m]):\n')
    fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.K)
end

if isfield(stewart.kinematics, 'C')
    fprintf('- The Damping matrix C is (in [m/N]):\n')
    fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.C)
end
#+end_src

