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

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

** TODO [#A] Merge the micro-station model with the nano-hexapod model
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
- [ ] Remove nano-hexapod model and add simplified model
- [ ] Add "cylindrical" payloads (configurable in mass)

** TODO [#B] Discuss the necessity of estimated Rz?

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.

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

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

From last sections:
- Uniaxial: No stiff nano-hexapod (should also demonstrate that here)
- Rotating: No soft nano-hexapod, Decentralized IFF can be used robustly by adding parallel stiffness

In this section:
- Take the model of the nano-hexapod with stiffness 1um/N
- Apply decentralized IFF
- Apply HAC-LAC
- Check robustness to payload change
- Simulation of experiments

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

- 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

- Schematic with micro-station + nass + metrology + control system
- Zoom in the control system with blocs
- Then explain all the blocs
- Say that there are many control strategies.
  It will be the topic of chapter 2.3.
  Here, we start with something simple: control in the frame of the struts

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

- from ref:ssec:ustation_kinematics, computation of the wanted sample pose from the setpoint of each stage.

** Computation of the sample's pose error

From metrology (here supposed to be perfect 6-DoF), compute the sample's pose error.
Has to invert the homogeneous transformation.

** Position error in the frame of the nano-hexapod

Explain how to compute the errors in the frame of the struts (rotating)

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

- How to apply/optimize IFF on an hexapod? ()
- Robustness to payload mass
- Root Locus
- Damping optimization

- [ ][[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]]
- [ ][[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/bibliography.org::*Vibration Control and Active Damping][Vibration Control and Active Damping]]

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

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

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

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 [V]
% io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Vs');  io_i = io_i + 1; % Force Sensors voltages [V]
% io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Position Errors [m]

% % With no payload
% Gm = exp(-1e-4*s)*linearize(mdl, io);
% Gm.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
% Gm.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
%                  'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
#+end_src

- Show how it changes with the payload mass (1, 25, 50)
- Effect of rotation (1rpm, 60rpm)

** Controller Design

- Apply IFF
- Show Root Locus
- Choose optimal gain.
  Here in MIMO, cannot have optimal damping for all modes. (there is a paper that tries to optimize that)
- Show robustness to change of payload (loci?)
- Reference to paper showing stability in MIMO for decentralized IFF

** Sensitivity to disturbances

- Compute transfer functions from spindle vertical error to sample vertical error with IFF (and compare without the NASS)
- Same for horizontal
- Maybe noise budgeting, but may be complex in MIMO...

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

- [ ] [[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...)
- Effect of micro-station compliance
- Effect of IFF
- Effect of payload mass
- Decoupled plant
- Controller design

From control kinematics:
- Talk about issue of not estimating Rz from external metrology? (maybe could be nice to discuss that during the experiments!)
- Show what happens is Rz is not estimated (for instance supposed equaled to zero => increased coupling)

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

- Compute transfer function from u to dL (with IFF applied)

** Effect of Payload mass

- Show effect of payload mass + rotation

** Controller design

- Show robustness with Loci

** Sensitivity to disturbances

- Compute transfer functions from spindle vertical error to sample vertical error with HAC-IFF
  Compare without the NASS, and with just IFF
- Same for horizontal
- Maybe noise budgeting, but may be complex in MIMO...

** Tomography experiment

- With HAC-IFF, perform tomography experiment, and compare with open-loop

- 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

* Conclusion
<<sec:nass_conclusion>>


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

* 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
        %% 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} = 1e4
        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();

    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

*** TODO [#A] =initializeSample=: Sample

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

    arguments
        args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3 % Height [m]
        args.R (1,1) double {mustBeNumeric, mustBePositive} = 350e-3 % Radius [m]
        args.m (1,1) double {mustBeNumeric, mustBePositive} = 1 % Mass [kg]
    end

    sample = struct();

    switch args.type
      case '0'
        sample.type = 0;
      case '1'
        sample.type = 1;
      case '2'
        sample.type = 2;
      case '3'
        sample.type = 3;
    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

* 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;
#+END_SRC
