phd-test-bench-nano-hexapod/test-bench-nano-hexapod.org

#+TITLE: Nano-Hexapod - Test Bench
: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{test-bench-nano-hexapod.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:

#+begin_export html
  <hr>
  <p>This report is also available as a <a href="./test-bench-nano-hexapod.pdf">pdf</a>.</p>
  <hr>
#+end_export

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

Add these documents:
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-nano-hexapod-assembly/nass-nano-hexapod-assembly.org][nass-nano-hexapod-assembly]]
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/test-bench-vibration-table/vibration-table.org][test-bench-vibration-table]]
- [ ] *Use corrected APA parameters in the initialization script*


Suspended table EPDM: ID00/test_bench/table_dyn

*Goal of this test bench*:
- Properly mount the nano-hexapod
- Verify all is working
- Tune the complete nano-hexapod model

*Basic outline*:
1. [ ] Mounting procedure
   - [ ] Goal
   - [ ] Procedure
   - [ ] Results
2. [ ] Suspended table:
   - [ ] Goal: identify dynamics of the nano-hexapod not coupled with the outside world
   - [ ] Experimental modal analysis (first mode at 700 Hz => rigid body in Simscape))
   - [ ] Simscape model of the table, comparison of the obtained modes
3. [ ] Simscape model of the Nano-Hexapod? (Maybe already presented in second chapter, maybe this [[file:~/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/B6-nass-design/nass-design.org::+TITLE: Nano Hexapod - Obtained Design][document]])
   Yes, but now the model is updated with the tuned models of the APA, Flexible joints, etc..
4. [ ] Nano-Hexapod Dynamics
   - Identified dynamics
   - Comparison with the simscape model
   - Effect of the payload mass

Maybe the rest is not so interesting here as it will be presented again in the next sections.
- Robust Integral Force Feedback (LAC)
- High Authority Controller HAC
  - Decoupling Strategy

** TODO [#B] Rework captions

** DONE [#C] See if the FEM in Simscape can model the struts modes
CLOSED: [2024-11-14 Thu 11:17]
Yes !

** TODO [#C] Add nice pictures

[[file:~/Cloud/pictures/work/nano-hexapod/vibration-table]]

** TODO [#B] If possible, correlate the modal analysis with FEM

This could just be used to show that experimental measure of the flexible mode of the top plate has been done:
- [X] *This test was made using encoder fixed to the struts, is it relevant to put it here?*
- [ ] Also compare with the FEM
  - [[file:/home/thomas/Cloud/work-projects/ID31-NASS/nass-fem/Assembly 20201020/Modal t=0.50mm]]
  - [[file:/home/thomas/Cloud/work-projects/ID31-NASS/nass-fem/GitLab_nass-fem/dynamic-modal/assy-hexapod-20201022/t_0.25mm]]
  - [[file:/home/thomas/Cloud/work-projects/ID31-NASS/nass-fem/GitLab_nass-fem/dynamic-modal/assy-hexapod-20201022/t_0.5mm]]
  - [[file:/home/thomas/Cloud/work-projects/ID31-NASS/nass-fem/GitLab_nass-fem/plateau-superelement]]

** DONE [#B] Proper analysis of the identified dynamics
CLOSED: [2024-10-28 Mon 11:13]

- [ ] Top plate flexible modes (2 modes)
- [ ] Modes of the encoder supports
- [ ] ...

** TODO [#C] Remove un-used matlab scripts and src files

** DONE [#B] Make nice subfigures for identified modes
CLOSED: [2024-10-27 Sun 15:58] SCHEDULED: <2024-10-26 Sat>

Maybe try to do similar thing as for the micro station: [[file:~/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/A3-micro-station-modal-analysis/mode_shapes-gif-to-jpg/gen_mode_1.sh]]

- [X] Table: 6 rigid body modes + 3 flexible modes
  [[file:figs/modal-analysis-table]]
- [X] Nano hexapod: 6 rigid body modes + 2 flexible modes
  [[file:figs/modal-analysis-hexapod]]

** DONE [#A] Update the default APA parameters to have good match
CLOSED: [2024-10-26 Sat 15:25]

initializeNanoHexapodFinal

#+begin_src matlab
    args.actuator_k   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*380000
    args.actuator_ke  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*4952605
    args.actuator_ka  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*2476302
    args.actuator_c   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*20
    args.actuator_ce  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*200
    args.actuator_ca  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*100
#+end_src

#+begin_src matlab
%% Actuator gain [N/V]
if all(args.actuator_Ga == 0)
    switch args.actuator_type
      case '2dof'
        nano_hexapod.actuator.Ga = ones(6,1)*(-2.5796);
      case 'flexible frame'
        nano_hexapod.actuator.Ga = ones(6,1); % TODO
      case 'flexible'
        nano_hexapod.actuator.Ga = ones(6,1)*23.2;
    end
else
    nano_hexapod.actuator.Ga = args.actuator_Ga; % Actuator gain [N/V]
end

%% Sensor gain [V/m]
if all(args.actuator_Gs == 0)
    switch args.actuator_type
      case '2dof'
        nano_hexapod.actuator.Gs = ones(6,1)*466664;
      case 'flexible frame'
        nano_hexapod.actuator.Gs = ones(6,1); % TODO
      case 'flexible'
        nano_hexapod.actuator.Gs = ones(6,1)*(-4898341);
    end
else
    nano_hexapod.actuator.Gs = args.actuator_Gs; % Sensor gain [V/m]
end

#+end_src

** DONE [#A] Check why the model has more damping now
CLOSED: [2024-10-26 Sat 15:26]

- Probably because damping on the FJ bench was overestimated (the damping linked to the suspended mass was maybe underestimated)

** DONE [#A] Determine how to manage the Simscape model of the hexapod
CLOSED: [2024-10-26 Sat 15:26]

- git submodule?
- Maybe just copy paste the directory as it will not change a lot now

* Introduction                                                        :ignore:

Prior to the nano-hexapod assembly, all the struts were mounted and individually characterized.
In Section ref:sec:test_nhexa_mounting, the assembly procedure of the nano-hexapod is presented.

To identify the dynamics of the nano-hexapod, a special suspended table was developed, which consisted of a stiff "optical breadboard" suspended on top of four soft springs.
The Nano-Hexapod was then mounted on top of the suspended table such that its dynamics is not affected by complex dynamics except from the suspension modes of the table that can be well characterized and modeled (Section ref:sec:test_nhexa_table).

The obtained nano-hexapod dynamics is analyzed in Section ref:sec:test_nhexa_dynamics, and compared with the multi-body model in Section ref:sec:test_nhexa_model.

* Nano-Hexapod Assembly Procedure
<<sec:test_nhexa_mounting>>
The assembly of the nano-hexapod is critical for both avoiding additional stress in the flexible joints (that would result in a loss of stroke) and for precisely determining the Jacobian matrix.
The goal was to fix the six struts to the two nano-hexapod plates (shown in Figure ref:fig:test_nhexa_nano_hexapod_plates) while the two plates were parallel and aligned vertically so that all the flexible joints did not experience any stress.
To do so, a precisely machined mounting tool (Figure ref:fig:test_nhexa_center_part_hexapod_mounting) is used to position the two nano-hexapod plates during the assembly procedure.

#+name: fig:test_nhexa_received_parts
#+caption: Nano-Hexapod plates (\subref{fig:test_nhexa_nano_hexapod_plates}) and mounting tool used to position the two plates during assembly (\subref{fig:test_nhexa_center_part_hexapod_mounting})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_nhexa_nano_hexapod_plates}Top and bottom plates}
#+attr_latex: :options {0.59\textwidth}
#+begin_subfigure
#+attr_org: :width 800px
#+attr_latex: :height 4cm
[[file:figs/test_nhexa_nano_hexapod_plates.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_nhexa_center_part_hexapod_mounting}Mounting tool}
#+attr_latex: :options {0.39\textwidth}
#+begin_subfigure
#+attr_org: :width 800px
#+attr_latex: :height 4cm
[[file:figs/test_nhexa_center_part_hexapod_mounting.jpg]]
#+end_subfigure
#+end_figure

The mechanical tolerances of the received plates were checked using a FARO arm[fn:1] (Figure ref:fig:test_nhexa_plates_tolerances) and were found to comply with the requirements[fn:2].
The same was done for the mounting tool[fn:3].
The two plates were then fixed to the mounting tool, as shown in Figure ref:fig:test_nhexa_mounting_tool_hexapod_top_view.
The main goal of this "mounting tool" is to position the flexible joint interfaces (the "V" shapes) of both plates so that a cylinder can rest on the 4 flat interfaces at the same time.

#+name: fig:test_nhexa_dimensional_check
#+caption: A FARO arm is used to dimensionally check the received parts (\subref{fig:test_nhexa_plates_tolerances}) and after mounting the two plates with the mounting part (\subref{fig:test_nhexa_mounting_tool_hexapod_top_view})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_nhexa_plates_tolerances}Dimensional check of the bottom plate}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_org: :width 800px
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_nhexa_plates_tolerances.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_nhexa_mounting_tool_hexapod_top_view}Wanted coaxiality between interfaces}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_org: :width 800px
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_nhexa_mounting_tool_hexapod_top_view.png]]
#+end_subfigure
#+end_figure

The quality of the positioning can be estimated by measuring the "straightness" of the top and bottom "V" interfaces.
This corresponds to the diameter of the smallest cylinder which contains all points along the measured axis.
This was again done using the FARO arm, and the results for all six struts are summarized in Table ref:tab:measured_straightness.
The straightness was found to be better than $15\,\mu m$ for all struts[fn:4], which is sufficiently good to not induce significant stress of the flexible joint during assembly.

#+name: tab:measured_straightness
#+caption: Measured straightness between the two "V" shapes for the six struts. These measurements were performed twice for each strut.
#+attr_latex: :environment tabularx :width 0.35\linewidth :align Xcc
#+attr_latex: :center t :booktabs t
| *Strut* | *Meas 1*     | *Meas 2*     |
|---------+--------------+--------------|
|       1 | $7\,\mu m$   | $3\, \mu m$  |
|       2 | $11\, \mu m$ | $11\, \mu m$ |
|       3 | $15\, \mu m$ | $14\, \mu m$ |
|       4 | $6\, \mu m$  | $6\, \mu m$  |
|       5 | $7\, \mu m$  | $5\, \mu m$  |
|       6 | $6\, \mu m$  | $7\, \mu m$  |

The encoder rulers and heads were then fixed to the top and bottom plates, respectively (Figure ref:fig:test_nhexa_mount_encoder), and the encoder heads were aligned to maximize the received contrast.

#+name: fig:test_nhexa_mount_encoder
#+caption: Mounting of the encoders to the Nano-hexapod. The rulers are fixed to the top plate (\subref{fig:test_nhexa_mount_encoder_rulers}) while encoders heads are fixed to the bottom plate (\subref{fig:test_nhexa_mount_encoder_heads})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_nhexa_mount_encoder_rulers}Encoder rulers}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_org: :width 800px
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_nhexa_mount_encoder_rulers.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_nhexa_mount_encoder_heads}Encoder heads}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_org: :width 800px
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_nhexa_mount_encoder_heads.jpg]]
#+end_subfigure
#+end_figure

The six struts were then fixed to the bottom and top plates one by one.
First, the top flexible joint is fixed so that its flat reference surface is in contact with the top plate.
This step precisely determines the position of the flexible joint with respect to the top plate.
The bottom flexible joint is then fixed.
After mounting all six struts, the mounting tool (Figure ref:fig:test_nhexa_center_part_hexapod_mounting) can be disassembled, and the nano-hexapod as shown in Figure ref:fig:test_nhexa_nano_hexapod_mounted is fully assembled.

#+name: fig:test_nhexa_nano_hexapod_mounted
#+caption: Mounted Nano-Hexapod
#+attr_org: :width 800px
#+attr_latex: :width \linewidth
[[file:figs/test_nhexa_mounted_hexapod.jpg]]

* Suspended Table
:PROPERTIES:
:header-args:matlab+: :tangle matlab/test_nhexa_1_suspended_table.m
:END:
<<sec:test_nhexa_table>>
** Introduction

When a dynamical system is fixed to a support (such as a granite or an optical table), its dynamics will couple to the support dynamics.
This may results in additional modes appearing in the system dynamics, which are difficult to predict and model.
To prevent this issue, the strategy adopted here is to mount the nano-hexapod on top a suspended table with low frequency suspension modes.

In this case, the modes of the suspended table were chosen to be at much lower frequency than those of the nano-hexapod such that good decoupling is obtained.
Another key advantage is that the suspension modes of the table can be easily represented using a multi-body model.
Therefore, the measured dynamics of the nano-hexapod on top of the suspended table can be compared to a multi-body model representing the same experimental conditions.
The model of the Nano-Hexapod can thus be precisely tuned to match the measured dynamics.

The developed suspended table is described in Section ref:ssec:test_nhexa_table_setup.
The modal analysis of the table is done in ref:ssec:test_nhexa_table_identification.
Finally, the multi-body model representing the suspended table was tuned to match the measured modes (Section ref:ssec:test_nhexa_table_model).

** Matlab Init                                              :noexport:ignore:
#+begin_src matlab
%% test_nhexa_1_suspended_table.m
#+end_src

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

** Experimental Setup
<<ssec:test_nhexa_table_setup>>

The design of the suspended table is quite straightforward.
First, an optical table with high frequency flexible mode was selected[fn:5].
Then, four springs[fn:6] were selected with low spring rate such that the suspension modes are below 10Hz.
Finally, some interface elements were designed, and mechanical lateral mechanical stops were added (Figure ref:fig:test_nhexa_suspended_table_cad).

#+name: fig:test_nhexa_suspended_table_cad
#+caption: CAD View of the vibration table. The purple cylinders are representing the soft springs.
#+attr_latex: :width 0.7\linewidth
[[file:figs/test_nhexa_suspended_table_cad.jpg]]

** Modal analysis of the suspended table
<<ssec:test_nhexa_table_identification>>

In order to perform a modal analysis of the suspended table, a total of 15 3-axis accelerometers[fn:7] were fixed to the breadboard.
Using an instrumented hammer, the first 9 modes could be identified and are summarized in Table ref:tab:test_nhexa_suspended_table_modes.
The first 6 modes are suspension modes (i.e. rigid body mode of the breadboard) and are located below 10Hz.
The next modes are the flexible modes of the breadboard as shown in Figure ref:fig:test_nhexa_table_flexible_modes, and are located above 700Hz.

#+attr_latex: :options [t]{0.45\linewidth}
#+begin_minipage
#+name: fig:test_nhexa_suspended_table
#+caption: Mounted suspended table. Only 1 or the 15 accelerometer is mounted on top
#+attr_latex: :width 0.99\linewidth :float nil
[[file:figs/test_nhexa_suspended_table.jpg]]
#+end_minipage
\hfill
#+attr_latex: :options [b]{0.45\linewidth}
#+begin_minipage
#+begin_scriptsize
#+name: tab:test_nhexa_suspended_table_modes
#+caption: Obtained modes of the suspended table
#+attr_latex: :environment tabularx :width 0.9\linewidth :placement [b] :align clX
#+attr_latex: :booktabs t :float nil :center t
#+RESULTS:
| *Modes* | *Frequency* | *Description*    |
|---------+-------------+------------------|
|     1,2 | 1.3 Hz      | X-Y translations |
|       3 | 2.0 Hz      | Z rotation       |
|       4 | 6.9 Hz      | Z translation    |
|     5,6 | 9.5 Hz      | X-Y rotations    |
|---------+-------------+------------------|
|       7 | 701 Hz      | "Membrane" Mode  |
|       8 | 989 Hz      | Complex mode     |
|       9 | 1025 Hz     | Complex mode     |
#+end_scriptsize
#+end_minipage


#+name: fig:test_nhexa_table_flexible_modes
#+caption: Three identified flexible modes of the suspended table
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_nhexa_table_flexible_mode_1}Flexible mode at 701Hz}
#+attr_latex: :options {\textwidth}
#+begin_subfigure
#+attr_latex: :width \linewidth
[[file:figs/test_nhexa_table_flexible_mode_1.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_nhexa_table_flexible_mode_2}Flexible mode at 989Hz}
#+attr_latex: :options {\textwidth}
#+begin_subfigure
#+attr_latex: :width \linewidth
[[file:figs/test_nhexa_table_flexible_mode_2.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_nhexa_table_flexible_mode_3}Flexible mode at 1025Hz}
#+attr_latex: :options {\textwidth}
#+begin_subfigure
#+attr_latex: :width \linewidth
[[file:figs/test_nhexa_table_flexible_mode_3.jpg]]
#+end_subfigure
#+end_figure

** Multi-body Model of the suspended table
<<ssec:test_nhexa_table_model>>

The multi-body model of the suspended table consists simply of two solid bodies connected by 4 springs.
The 4 springs are here modeled with "bushing joints" that have stiffness and damping properties in x, y, and z directions.

The model order is 12, which corresponds to the 6 suspension modes.
The inertia properties of the parts were determined from the geometry and material densities.
The stiffness of the springs was initially set from the datasheet nominal value of $17.8\,N/mm$ and then reduced down to $14\,N/mm$ to better match the measured suspension modes.
The stiffness of the springs in the horizontal plane is set at $0.5\,N/mm$.
The obtained suspension modes of the multi-body model are compared with the measured modes in Table ref:tab:test_nhexa_suspended_table_simscape_modes.

#+begin_src matlab
%% Configure Simscape Model
table_type = 'Suspended'; % On top of vibration table
device_type = 'None'; % No device on the vibration table
payload_num = 0; % No Payload

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'],      1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '/F_v'],  1, 'openoutput'); io_i = io_i + 1;

%% Run the linearization
G = linearize(mdl, io);
G.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G.OutputName  = {'Vdx', 'Vdy', 'Vdz', 'Vrx', 'Vry', 'Vrz'};

%% Compute the resonance frequencies
ws = eig(G.A);
ws = ws(imag(ws) > 0);
#+end_src

#+name: tab:test_nhexa_suspended_table_simscape_modes
#+caption: Comparison of the identified suspension modes with the Simscape model and measured experimentally
#+attr_latex: :environment tabularx :width 0.6\linewidth :align Xcccc
#+attr_latex: :center t :booktabs t
| Directions   | $D_x$, $D_y$ | $R_z$  | $D_z$  | $R_x$, $R_y$ |
|--------------+--------------+--------+--------+--------------|
| Experimental | 1.3 Hz       | 2.0 Hz | 6.9 Hz | 9.5 Hz       |
| Multi-body   | 1.3 Hz       | 1.8 Hz | 6.8 Hz | 9.5 Hz       |

* Nano-Hexapod Measured Dynamics
:PROPERTIES:
:header-args:matlab+: :tangle matlab/test_nhexa_2_dynamics.m
:END:
<<sec:test_nhexa_dynamics>>

** Introduction                                                      :ignore:

The Nano-Hexapod was then mounted on top of the suspended table, as shown in Figure ref:fig:test_nhexa_hexa_suspended_table.
All instrumentation (Speedgoat with ADC, DAC, piezoelectric voltage amplifiers and digital interfaces for the encoder) were configured and connected to the nano-hexapod using many cables.

#+name: fig:test_nhexa_hexa_suspended_table
#+caption: Mounted Nano-Hexapod on top of the suspended table
#+attr_latex: :width 0.7\linewidth
[[file:figs/test_nhexa_hexa_suspended_table.jpg]]

A modal analysis of the nano-hexapod is first performed in Section ref:ssec:test_nhexa_enc_struts_modal_analysis.
The results of the modal analysis will be useful to better understand the measured dynamics from actuators to sensors.

A block diagram of the (open-loop) system is shown in Figure ref:fig:test_nhexa_nano_hexapod_signals.
The frequency response functions from controlled signals $\mathbf{u}$ to the force sensors voltages $\mathbf{V}_s$ and to the encoders measured displacements $\mathbf{d}_e$ are experimentally identified in Section ref:ssec:test_nhexa_identification.
The effect of the payload mass on the dynamics is discussed in Section ref:ssec:test_nhexa_added_mass.

#+begin_src latex :file test_nhexa_nano_hexapod_signals.pdf
\definecolor{instrumentation}{rgb}{0, 0.447, 0.741}
\definecolor{mechanics}{rgb}{0.8500, 0.325, 0.098}

\begin{tikzpicture}
  % Blocs
  \node[block={4.0cm}{3.0cm}, fill=mechanics!20!white] (nano_hexapod) {Mechanics};
  \coordinate[] (inputF)  at (nano_hexapod.west);
  \coordinate[] (outputL) at ($(nano_hexapod.south east)!0.8!(nano_hexapod.north east)$);
  \coordinate[] (outputF) at ($(nano_hexapod.south east)!0.2!(nano_hexapod.north east)$);

  \node[block, left= 0.8 of inputF,  fill=instrumentation!20!white, align=center] (F_stack) {\tiny Actuator \\ \tiny stacks};
  \node[block, left= 0.8 of F_stack, fill=instrumentation!20!white] (PD200)   {PD200};
  \node[DAC,   left= 0.8 of PD200,   fill=instrumentation!20!white] (F_DAC)   {DAC};
  \node[block, right=0.8 of outputF, fill=instrumentation!20!white, align=center] (Fm_stack){\tiny Sensor \\ \tiny stack};
  \node[ADC,   right=0.8 of Fm_stack,fill=instrumentation!20!white] (Fm_ADC)  {ADC};
  \node[block, right=0.8 of outputL, fill=instrumentation!20!white] (encoder) {\tiny Encoder};

  % Connections and labels
  \draw[->] ($(F_DAC.west)+(-0.8,0)$) node[above right]{$\mathbf{u}$} node[below right]{$[V]$} -- node[sloped]{$/$} (F_DAC.west);
  \draw[->] (F_DAC.east)   -- node[midway, above]{$\tilde{\mathbf{u}}$}node[midway, below]{$[V]$} (PD200.west);
  \draw[->] (PD200.east)   -- node[midway, above]{$\mathbf{V}_a$}node[midway, below]{$[V]$} (F_stack.west);
  \draw[->] (F_stack.east) -- (inputF) node[above left]{$\mathbf{\tau}$}node[below left]{$[N]$};

  \draw[->] (outputF)       -- (Fm_stack.west) node[above left]{$\mathbf{\epsilon}$} node[below left]{$[m]$};
  \draw[->] (Fm_stack.east) -- node[midway, above]{$\tilde{\mathbf{V}}_s$}node[midway, below]{$[V]$} (Fm_ADC.west);
  \draw[->] (Fm_ADC.east)   -- node[sloped]{$/$} ++(0.8, 0)coordinate(end) node[above left]{$\mathbf{V}_s$}node[below left]{$[V]$};

  \draw[->] (outputL)      -- (encoder.west) node[above left]{$\mathbf{d}_e$} node[below left]{$[m]$};
  \draw[->] (encoder.east) -- node[sloped]{$/$} (encoder-|end) node[above left]{$\mathbf{d}_{e}$}node[below left]{$[m]$};

  % Nano-Hexapod
  \begin{scope}[on background layer]
    \node[fit={(F_stack.west|-nano_hexapod.south) (Fm_stack.east|-nano_hexapod.north)}, fill=black!20!white, draw, inner sep=2pt] (system) {};
    \node[above] at (system.north) {Nano-Hexapod};
  \end{scope}
\end{tikzpicture}
#+end_src

#+name: fig:test_nhexa_nano_hexapod_signals
#+caption: Block diagram of the studied system. The command signal generated by the speedgoat is $\mathbf{u}$, and the measured dignals are $\mathbf{d}_{e}$ and $\mathbf{V}_s$. Units are indicated in square brackets.
#+attr_latex: :width \linewidth
#+RESULTS:
[[file:figs/test_nhexa_nano_hexapod_signals.png]]

** Matlab Init                                              :noexport:ignore:
#+begin_src matlab
%% test_nhexa_2_dynamics.m
% Identification of the nano-hexapod dynamics from u to de and to Vs
% Encoders are fixed to the plates
#+end_src

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

** Modal analysis
<<ssec:test_nhexa_enc_struts_modal_analysis>>

To facilitate the future analysis of the measured plant dynamics, a basic modal analysis of the nano-hexapod is performed.
Five 3-axis accelerometers were fixed on the top platform of the nano-hexapod (Figure ref:fig:test_nhexa_modal_analysis) and the top platform was excited using an instrumented hammer.

#+name: fig:test_nhexa_modal_analysis
#+caption: Five accelerometers fixed on top of the nano-hexapod to perform a modal analysis
#+attr_latex: :width 0.7\linewidth
[[file:figs/test_nhexa_modal_analysis.jpg]]

Between 100Hz and 200Hz, 6 suspension modes (i.e. rigid body modes of the top platform) were identified.
At around 700Hz, two flexible modes of the top plate were observed (see Figure ref:fig:test_nhexa_hexa_flexible_modes).
These modes are summarized in Table ref:tab:test_nhexa_hexa_modal_modes_list.

#+name: tab:test_nhexa_hexa_modal_modes_list
#+caption: Description of the identified modes of the Nano-Hexapod
#+attr_latex: :environment tabularx :width 0.7\linewidth :align ccX
#+attr_latex: :center t :booktabs t
| *Mode* | *Frequency* | *Description*                                |
|--------+-------------+----------------------------------------------|
|      1 | 120 Hz      | Suspension Mode: Y-translation               |
|      2 | 120 Hz      | Suspension Mode: X-translation               |
|      3 | 145 Hz      | Suspension Mode: Z-translation               |
|      4 | 165 Hz      | Suspension Mode: Y-rotation                  |
|      5 | 165 Hz      | Suspension Mode: X-rotation                  |
|      6 | 190 Hz      | Suspension Mode: Z-rotation                  |
|      7 | 692 Hz      | (flexible) Membrane mode of the top platform |
|      8 | 709 Hz      | Second flexible mode of the top platform     |

#+name: fig:test_nhexa_hexa_flexible_modes
#+caption: Two identified flexible modes of the top plate of the Nano-Hexapod
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_nhexa_hexa_flexible_mode_1}Flexible mode at 692Hz}
#+attr_latex: :options {\textwidth}
#+begin_subfigure
#+attr_latex: :width \linewidth
[[file:figs/test_nhexa_hexa_flexible_mode_1.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_nhexa_hexa_flexible_mode_2}Flexible mode at 709Hz}
#+attr_latex: :options {\textwidth}
#+begin_subfigure
#+attr_latex: :width \linewidth
[[file:figs/test_nhexa_hexa_flexible_mode_2.jpg]]
#+end_subfigure
#+end_figure

** Identification of the dynamics
<<ssec:test_nhexa_identification>>

The dynamics of the nano-hexapod from the six command signals ($u_1$ to $u_6$) to the six measured displacement by the encoders ($d_{e1}$ to $d_{e6}$) and to the six force sensors ($V_{s1}$ to $V_{s6}$) were identified by generating low-pass filtered white noise for each command signal, one by one.

The $6 \times 6$ FRF matrix from $\mathbf{u}$ ot $\mathbf{d}_e$ is shown in Figure ref:fig:test_nhexa_identified_frf_de.
The diagonal terms are displayed using colored lines, and all the 30 off-diagonal terms are displayed by gray lines.

All six diagonal terms are well superimposed up to at least $1\,kHz$, indicating good manufacturing and mounting uniformity.
Below the first suspension mode, good decoupling can be observed (the amplitude of all off-diagonal terms are $\approx 20$ times smaller than the diagonal terms), indicating the correct assembly of all parts.

From 10Hz up to 1kHz, around 10 resonance frequencies can be observed.
The first 4 are suspension modes (at 122Hz, 143Hz, 165Hz and 191Hz) which correlate the modes measured during the modal analysis in Section ref:ssec:test_nhexa_enc_struts_modal_analysis.
Three modes at 237Hz, 349Hz and 395Hz are attributed to the internal strut resonances (this will be checked in Section ref:ssec:test_nhexa_comp_model_coupling).
Except for the mode at 237Hz, their impact on the dynamics is small.
The two modes at 665Hz and 695Hz are attributed to the flexible modes of the top platform.
Other modes can be observed above 1kHz, which can be attributed to flexible modes of the encoder supports or to flexible modes of the top platform.

Up to at least 1kHz, an alternating pole/zero pattern is observed, which makes the control easier to tune.
This would not have occurred if the encoders were fixed to the struts.

#+begin_src matlab
%% Identification of the transfer function from u to de and from u to Vs without payload
% Load identification data
load('test_nhexa_identification_data_mass_0.mat', 'data');

% Setup useful variables
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(1/Ts); % Number of points for the FFT computation
win = hanning(Nfft); % Hanning window
Noverlap = floor(Nfft/2); % Overlap between frequency analysis

% And we get the frequency vector
[~, f] = tfestimate(data{1}.u, data{1}.de, win, Noverlap, Nfft, 1/Ts);

% Transfer function from u to de
G_de = zeros(length(f), 6, 6);

for i = 1:6
    G_de(:,:,i) = tfestimate(data{i}.u, data{i}.de, win, Noverlap, Nfft, 1/Ts);
end

% Transfer function from u to Vs
G_Vs = zeros(length(f), 6, 6);

for i = 1:6
    G_Vs(:,:,i) = tfestimate(data{i}.u, data{i}.Vs, win, Noverlap, Nfft, 1/Ts);
end
#+end_src

#+begin_src matlab :exports none
%% Bode plot for the transfer function from u to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(f, abs(G_de(:, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f, abs(G_de(:,i, i)), ...
         'DisplayName', sprintf('$d_{e,%i}/u_%i$', i, i));
end
plot(f, abs(G_de(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$d_{e,i}/u_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 5e-4]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 4);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f, 180/pi*angle(G_de(:,i, i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);

linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
#+end_src

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

#+name: fig:test_nhexa_identified_frf_de
#+caption: Measured FRF for the transfer function from $\mathbf{u}$ to $\mathbf{d}_e$. The 6 diagonal terms are the colored lines (all superimposed), and the 30 off-diagonal terms are the gray lines.
#+attr_latex: :width \linewidth
#+RESULTS:
[[file:figs/test_nhexa_identified_frf_de.png]]


Similarly, the $6 \times 6$ FRF matrix from $\mathbf{u}$ to $\mathbf{V}_s$ is shown in Figure ref:fig:test_nhexa_identified_frf_Vs.
Alternating poles and zeros can be observed up to at least 2kHz, which is a necessary characteristics for applying decentralized IFF.
Similar to what was observed for the encoder outputs, all the "diagonal" terms are well superimposed, indicating that the same controller can be applied to all the struts.
The first flexible mode of the struts as 235Hz has large amplitude, and therefore, it should be possible to add some damping to this mode using IFF.

#+begin_src matlab :exports none
%% Bode plot of the IFF Plant (transfer function from u to Vs)
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(f, abs(G_Vs(:, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f, abs(G_Vs(:,i , i)), ...
         'DisplayName', sprintf('$V_{s%i}/u_%i$', i, i));
end
plot(f, abs(G_Vs(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$V_{si}/u_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 4);
leg.ItemTokenSize(1) = 15;
ylim([1e-3, 6e1]);

ax2 = nexttile;
hold on;
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f, 180/pi*angle(G_Vs(:,i, i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);

linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
#+end_src

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

#+name: fig:test_nhexa_identified_frf_Vs
#+caption: Measured FRF for the transfer function from $\mathbf{u}$ to $\mathbf{V}_s$. The 6 diagonal terms are the colored lines (all superimposed), and the 30 off-diagonal terms are the shaded black lines.
#+attr_latex: :width \linewidth
#+RESULTS:
[[file:figs/test_nhexa_identified_frf_Vs.png]]

** Effect of payload mass on the dynamics
<<ssec:test_nhexa_added_mass>>

One major challenge for controlling the NASS is the wanted robustness to a variation of payload mass; therefore, it is necessary to understand how the dynamics of the nano-hexapod changes with a change in payload mass.

To study how the dynamics changes with the payload mass, up to three "cylindrical masses" of $13\,kg$ each can be added for a total of $\approx 40\,kg$.
These three cylindrical masses on top of the nano-hexapod are shown in Figure ref:fig:test_nhexa_table_mass_3.

#+name: fig:test_nhexa_table_mass_3
#+caption: Picture of the nano-hexapod with the added three cylindrical masses for a total of $\approx 40\,kg$
#+attr_org: :width 800px
#+attr_latex: :width 0.8\linewidth
[[file:figs/test_nhexa_table_mass_3.jpg]]

#+begin_src matlab
%% Set to true only if all the FRF matrices should again computed
% from the experimental data
compute_frf = false;
if compute_frf
#+end_src

#+begin_src matlab
    %% Identification of the FRF matrices from u to de and to Vs
    % Load identification Data
    meas_added_mass = {...
        load('test_nhexa_identification_data_mass_0.mat', 'data'), ....
        load('test_nhexa_identification_data_mass_1.mat', 'data'), ....
        load('test_nhexa_identification_data_mass_2.mat', 'data'), ....
        load('test_nhexa_identification_data_mass_3.mat', 'data')};

    % Setup useful variables
    Ts = 1e-4; % Sampling Time [s]
    Nfft = floor(1/Ts); % Number of points for the FFT computation
    win = hanning(Nfft); % Hanning window
    Noverlap = floor(Nfft/2); % Overlap between frequency analysis

    % And we get the frequency vector
    [~, f] = tfestimate(meas_added_mass{1}.data{1}.u, meas_added_mass{1}.data{1}.de, win, Noverlap, Nfft, 1/Ts);

    % FRF from u to de
    G_de = {};

    for i_mass = [0:3]
        G_de(i_mass+1) = {zeros(length(f), 6, 6)};
        for i_strut = 1:6
            G_de{i_mass+1}(:,:,i_strut) = tfestimate(meas_added_mass{i_mass+1}.data{i_strut}.u, meas_added_mass{i_mass+1}.data{i_strut}.de, win, Noverlap, Nfft, 1/Ts);
        end
    end

    % FRF from u to Vs
    G_Vs = {};

    for i_mass = [0:3]
        G_Vs(i_mass+1) = {zeros(length(f), 6, 6)};
        for i_strut = 1:6
            G_Vs{i_mass+1}(:,:,i_strut) = tfestimate(meas_added_mass{i_mass+1}.data{i_strut}.u, meas_added_mass{i_mass+1}.data{i_strut}.Vs, win, Noverlap, Nfft, 1/Ts);
        end
    end
#+end_src

#+begin_src matlab :exports none :tangle no
    % The identified dynamics are then saved for further use.
    save('matlab/mat/test_nhexa_identified_frf_masses.mat', 'f', 'G_Vs', 'G_de')
#+end_src

#+begin_src matlab :eval no
    % The identified dynamics are then saved for further use.
    save('./mat/test_nhexa_identified_frf_masses.mat', 'f', 'G_Vs', 'G_de')
#+end_src

#+begin_src matlab
end
#+end_src

#+begin_src matlab :exports none
%% Load the identified transfer functions
frf_ol = load('test_nhexa_identified_frf_masses.mat', 'f', 'G_Vs', 'G_de');
#+end_src

The obtained frequency response functions from actuator signal $u_i$ to the associated encoder $d_{ei}$ for the four payload conditions (no mass, 13kg, 26kg and 39kg) are shown in Figure ref:fig:test_nhexa_identified_frf_de_masses.
As expected, the frequency of the suspension modes decreased with increasing payload mass.
The low frequency gain does not change because it is linked to the stiffness property of the nano-hexapod and not to its mass property.

The frequencies of the two flexible modes of the top plate first decreased significantly when the first mass was added (from $\approx 700\,Hz$ to $\approx 400\,Hz$).
This is because the added mass is composed of two half cylinders that are not fixed together.
Therefore, it adds a lot of mass to the top plate without increasing stiffness in one direction.
When more than one "mass layer" is added, the half cylinders are added at some angles such that rigidity is added in all directions (see how the three mass "layers" are positioned in Figure ref:fig:test_nhexa_table_mass_3).
In this case, the frequency of these flexible modes is increased.
In practice, the payload should be one solid body, and no decrease in the frequency of this flexible mode should be observed.
The apparent amplitude of the flexible mode of the strut at 237Hz becomes smaller as the payload mass increased.

The measured FRFs from $u_i$ to $V_{si}$ are shown in Figure ref:fig:test_nhexa_identified_frf_Vs_masses.
For all tested payloads, the measured FRF always have alternating poles and zeros, indicating that IFF can be applied in a robust manner.

#+begin_src matlab :exports none
%% Bode plot for the transfer function from u to de - Several payloads
masses = [0, 13, 26, 39];
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i_mass = [0:3]
    % Diagonal terms
    plot(frf_ol.f, abs(frf_ol.G_de{i_mass+1}(:,1, 1)), 'color', [colors(i_mass+1,:), 0.5], ...
         'DisplayName', sprintf('$d_{ei}/u_i$ - %i kg', masses(i_mass+1)));
    for i = 2:6
        plot(frf_ol.f, abs(frf_ol.G_de{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.5], ...
             'HandleVisibility', 'off');
    end
    % % Off-Diagonal terms
    % for i = 1:5
    %     for j = i+1:6
    %         plot(frf_ol.f, abs(frf_ol.G_de{i_mass+1}(:,i,j)), 'color', [colors(i_mass+1,:), 0.2], ...
    %              'HandleVisibility', 'off');
    %     end
    % end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 5e-4]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i_mass = [0:3]
    for i =1:6
        plot(frf_ol.f, 180/pi*angle(frf_ol.G_de{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.5]);
    end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-90, 180])

linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
#+end_src

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

#+begin_src matlab :exports none
%% Bode plot for the transfer function from u to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i_mass = [0:3]
    % Diagonal terms
    plot(frf_ol.f, abs(frf_ol.G_Vs{i_mass+1}(:,1, 1)), 'color', [colors(i_mass+1,:), 0.5], ...
         'DisplayName', sprintf('$V_{si}/u_i$ - %i kg', masses(i_mass+1)));
    for i = 2:6
        plot(frf_ol.f, abs(frf_ol.G_Vs{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.5], ...
             'HandleVisibility', 'off');
    end
    % % Off-Diagonal terms
    % for i = 1:5
    %     for j = i+1:6
    %         plot(frf_ol.f, abs(frf_ol.G_Vs{i_mass+1}(:,i,j)), 'color', [colors(i_mass+1,:), 0.2], ...
    %              'HandleVisibility', 'off');
    %     end
    % end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-2, 1e2]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i_mass = [0:3]
    for i =1:6
        plot(frf_ol.f, 180/pi*angle(frf_ol.G_Vs{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.5]);
    end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);

linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
#+end_src

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

#+name: fig:test_struts_mounting
#+caption: Measured Frequency Response Functions from $u_i$ to $d_{ei}$ (\subref{fig:test_nhexa_identified_frf_de_masses}) and from $u_i$ to $V_{si}$ (\subref{fig:test_nhexa_identified_frf_Vs_masses}) for all 4 payload conditions. Only diagonal terms are shown.
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_nhexa_identified_frf_de_masses}$u_i$ to $d_{ei}$}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width \linewidth
[[file:figs/test_nhexa_identified_frf_de_masses.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_nhexa_identified_frf_Vs_masses}$u_i$ to $V_{si}$}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width \linewidth
[[file:figs/test_nhexa_identified_frf_Vs_masses.png]]
#+end_subfigure
#+end_figure

* Nano-Hexapod Model Dynamics
:PROPERTIES:
:header-args:matlab+: :tangle matlab/test_nhexa_3_model.m
:END:
<<sec:test_nhexa_model>>

** Introduction                                                      :ignore:

In this section, the dynamics measured in Section ref:sec:test_nhexa_dynamics is compared with those estimated from the multi-body model.
The nano-hexapod multi-body model was therefore added on top of the vibration table multi-body model, as shown in Figure ref:fig:test_nhexa_hexa_simscape.

#+name: fig:test_nhexa_hexa_simscape
#+caption: 3D representation of the multi-body model with the nano-hexapod on top of the suspended table. Three mass "layers" are here added
#+attr_latex: :width 0.8\linewidth
[[file:figs/test_nhexa_hexa_simscape.png]]

The model should exhibit certain characteristics that are verified in this section.
First, it should match the measured system dynamics from actuators to sensors presented in Section ref:sec:test_nhexa_dynamics.
Both the "direct" terms (Section ref:ssec:test_nhexa_comp_model) and "coupling" terms (Section ref:ssec:test_nhexa_comp_model_coupling) of the multi-body model are compared with the measured dynamics.
Second, it should also represents how the system dynamics changes when a payload is fixed to the top platform.
This is checked in Section ref:ssec:test_nhexa_comp_model_masses.

** Matlab Init                                              :noexport:ignore:
#+begin_src matlab
%% test_nhexa_3_model.m
% Compare the measured dynamics from u to de and to Vs with the Simscape model
#+end_src

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

** Extract transfer function matrices from the Simscape Model      :noexport:
#+begin_src matlab :eval no
%% Set to true only if all the dynamics should again computed
% from the simscape model
extract_simscape_dynamics = false;
if extract_simscape_dynamics
#+end_src

#+begin_src matlab
    %% Extract the transfer function matrix from the Simscape model
    % Initialization of the Simscape model
    table_type = 'Suspended'; % On top of vibration table
    device_type = 'Hexapod'; % Nano-Hexapod
    payload_num = 0; % No Payload

    n_hexapod = initializeNanoHexapod('flex_bot_type', '4dof', ...
                                      'flex_top_type', '4dof', ...
                                      'motion_sensor_type', 'plates', ...
                                      'actuator_type', '2dof');

    % Identify the FRF matrix from u to [de,Vs]
    clear io; io_i = 1;
    io(io_i) = linio([mdl, '/u'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
    io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
    io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Encoders

    G_de = {};
    G_Vs = {};

    for i = [0:3]
        payload_num = i; % Change the payload on the nano-hexapod
        G = exp(-s*1e-4)*linearize(mdl, io, 0.0);
        G.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
        G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ...
                        'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'};
        G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)};
        G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)};
    end
#+end_src

#+begin_src matlab :exports none :tangle no
    % Save the identified plants
    save('matlab/mat/test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de')
#+end_src

#+begin_src matlab :eval no
    % Save the identified plants
    save('./mat/test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de')
#+end_src

#+begin_src matlab
    %% The same identification is performed, but this time with
    % "flexible" model of the APA
    table_type = 'Suspended'; % On top of vibration table
    device_type = 'Hexapod'; % Nano-Hexapod
    payload_num = 0; % No Payload

    n_hexapod = initializeNanoHexapod('flex_bot_type', '4dof', ...
                                      'flex_top_type', '4dof', ...
                                      'motion_sensor_type', 'plates', ...
                                      'actuator_type', 'flexible');

    G_de = {};
    G_Vs = {};

    for i = [0:3]
        payload_num = i; % Change the payload on the nano-hexapod
        G = exp(-s*1e-4)*linearize(mdl, io, 0.0);
        G.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
        G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ...
                        'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'};
        G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)};
        G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)};
    end
#+end_src

#+begin_src matlab :exports none :tangle no
    % Save the identified plants
    save('matlab/mat/test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de')
#+end_src

#+begin_src matlab :eval no
    % Save the identified plants
    save('./mat/test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de')
#+end_src

#+begin_src matlab :eval no
end
#+end_src

** Nano-Hexapod model dynamics
<<ssec:test_nhexa_comp_model>>

#+begin_src matlab :exports none
%% Load Simscape Model and measured FRF
sim_ol  = load('test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de');
frf_ol  = load('test_nhexa_identified_frf_masses.mat', 'f', 'G_Vs', 'G_de');
#+end_src

The multi-body model of the nano-hexapod was first configured with 4-DoF flexible joints, 2-DoF APA, and rigid top and bottom plates.
The stiffness values of the flexible joints were chosen based on the values estimated using the test bench and on the FEM.
The parameters of the APA model were determined from the test bench of the APA.
The $6 \times 6$ transfer function matrices from $\mathbf{u}$ to $\mathbf{d}_e$ and from $\mathbf{u}$ to $\mathbf{V}_s$ are then extracted from the multi-body model.

First, is it evaluated how well the models matches the "direct" terms of the measured FRF matrix.
To do so, the diagonal terms of the extracted transfer function matrices are compared with the measured FRF in Figure ref:fig:test_nhexa_comp_simscape_diag.
It can be seen that the 4 suspension modes of the nano-hexapod (at 122Hz, 143Hz, 165Hz and 191Hz) are well modeled.
The three resonances that were attributed to "internal" flexible modes of the struts (at 237Hz, 349Hz and 395Hz) cannot be seen in the model, which is reasonable because the APAs are here modeled as a simple uniaxial 2-DoF system.
At higher frequencies, no resonances can be observed in the model, as the top plate and the encoder supports are modeled as rigid bodies.

#+begin_src matlab :exports none
%% Diagonal elements of the FRF matrix from u to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:,1, 1)), 'color', [colors(1,:),0.5], ...
     'DisplayName', '$d_{ei}/u_i$ - FRF')
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(1,1), freqs, 'Hz'))), 'color', [colors(2,:),0.5], ...
     'DisplayName', '$d_{ei}/u_i$ - Model')
for i = 2:6
    plot(frf_ol.f, abs(frf_ol.G_de{1}(:,i, i)), 'color', [colors(1,:),0.5], ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(i,i), freqs, 'Hz'))), 'color', [colors(2,:),0.5], ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 5e-4]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(frf_ol.f, 180/pi*angle(frf_ol.G_de{1}(:,i, i)), 'color', [colors(1,:),0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(sim_ol.G_de{1}(i,i), freqs, 'Hz'))), 'color', [colors(2,:),0.5]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);

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

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

#+begin_src matlab :exports none
%% Diagonal elements of the FRF matrix from u to Vs
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(frf_ol.f, abs(frf_ol.G_Vs{1}(:,1, 1)), 'color', [colors(1,:),0.5], ...
     'DisplayName', '$V_{si}/u_i$ - FRF')
plot(freqs, abs(squeeze(freqresp(sim_ol.G_Vs{1}(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
     'DisplayName', '$V_{si}/u_i$ - Model')
for i = 2:6
    plot(frf_ol.f, abs(frf_ol.G_Vs{1}(:,i, i)), 'color', [colors(1,:),0.5], ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(sim_ol.G_Vs{1}(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
legend('location', 'southeast');

ax2 = nexttile;
hold on;
for i = 1:6
    plot(frf_ol.f, 180/pi*angle(frf_ol.G_Vs{1}(:,i, i)), 'color', [colors(1,:),0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(sim_ol.G_Vs{1}(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);

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

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

#+name: fig:test_nhexa_comp_simscape_diag
#+caption: Comparison of the diagonal elements (i.e. "direct" terms) of the measured FRF matrix and the dynamics identified from the multi-body model. Both for the dynamics from $u$ to $d_e$ (\subref{fig:test_nhexa_comp_simscape_de_diag}) and from $u$ to $V_s$ (\subref{fig:test_nhexa_comp_simscape_Vs_diag})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_nhexa_comp_simscape_de_diag}from $u$ to $d_e$}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_nhexa_comp_simscape_de_diag.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_nhexa_comp_simscape_Vs_diag}from $u$ to $V_s$}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_nhexa_comp_simscape_Vs_diag.png]]
#+end_subfigure
#+end_figure

** Dynamical coupling
<<ssec:test_nhexa_comp_model_coupling>>

Another desired feature of the model is that it effectively represents coupling in the system, as this is often the limiting factor for the control of MIMO systems.
Instead of comparing the full 36 elements of the $6 \times 6$ FFR matrix from $\mathbf{u}$ to $\mathbf{d}_e$, only the first "column" is compared (Figure ref:fig:test_nhexa_comp_simscape_de_all), which corresponds to the transfer function from the command $u_1$ to the six measured encoder displacements $d_{e1}$ to $d_{e6}$.
It can be seen that the coupling in the model matches the measurements well up to the first un-modeled flexible mode at 237Hz.
Similar results are observed for all other coupling terms and for the transfer function from $\mathbf{u}$ to $\mathbf{V}_s$.

#+begin_src matlab :exports none
%% Comparison of the plants (encoder output) when tuning the misalignment
i_input = 1;

figure;
tiledlayout(2, 3, 'TileSpacing', 'tight', 'Padding', 'tight');

ax1 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 1, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(1, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e1}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');

ax2 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 2, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(2, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e2}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);

ax3 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 3, i_input)), ...
     'DisplayName', 'Measurements');
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(3, i_input), freqs, 'Hz'))), ...
     'DisplayName', 'Model (2-DoF APA)');
text(54, 4e-4, '$d_{e3}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax4 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 4, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(4, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e4}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
xticks([50, 100, 200, 400])

ax5 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 5, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(5, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e5}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([50, 100, 200, 400])

ax6 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 6, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(6, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e6}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([50, 100, 200, 400])

linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
xlim([50, 5e2]); ylim([1e-8, 5e-4]);
#+end_src

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

#+name: fig:test_nhexa_comp_simscape_de_all
#+caption: Comparison of the measured (in blue) and modeled (in red) frequency transfer functions from the first control signal $u_1$ to the six encoders $d_{e1}$ to $d_{e6}$. The APA are here modeled with a 2-DoF mass-spring-damper system.
#+RESULTS:
[[file:figs/test_nhexa_comp_simscape_de_all.png]]

The APA300ML was then modeled with a /super-element/ extracted from a FE-software.
The obtained transfer functions from $u_1$ to the six measured encoder displacements $d_{e1}$ to $d_{e6}$ are compared with the measured FRF in Figure ref:fig:test_nhexa_comp_simscape_de_all_flex.
While the damping of the suspension modes for the /super-element/ is underestimated (which could be solved by properly tuning the proportional damping coefficients), the flexible modes of the struts at 237Hz and 349Hz are well modeled.
Even the mode 395Hz can be observed in the model.
Therefore, if the modes of the struts are to be modeled, the /super-element/ of the APA300ML can be used at the cost of obtaining a much higher order model.

#+begin_src matlab
%% Load the plant model with Flexible APA
flex_ol = load('test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de');
#+end_src

#+begin_src matlab :exports none
%% Comparison of the plants (encoder output) when tuning the misalignment
i_input = 1;

figure;
tiledlayout(2, 3, 'TileSpacing', 'tight', 'Padding', 'tight');

ax1 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 1, i_input)));
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(1, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e1}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');

ax2 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 2, i_input)));
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(2, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e2}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);

ax3 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 3, i_input)), ...
     'DisplayName', 'Measurements');
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(3, i_input), freqs, 'Hz'))), ...
     'DisplayName', 'Model (Flexible APA)');
text(54, 4e-4, '$d_{e3}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax4 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 4, i_input)));
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(4, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e4}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
xticks([50, 100, 200, 400])

ax5 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 5, i_input)));
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(5, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e5}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([50, 100, 200, 400])

ax6 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 6, i_input)));
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(6, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e6}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([50, 100, 200, 400])

linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
xlim([50, 5e2]); ylim([1e-8, 5e-4]);
#+end_src

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

#+name: fig:test_nhexa_comp_simscape_de_all_flex
#+caption: Comparison of the measured (in blue) and modeled (in red) frequency transfer functions from the first control signal $u_1$ to the six encoders $d_{e1}$ to $d_{e6}$. The APA are here modeled with a "super-element".
#+RESULTS:
[[file:figs/test_nhexa_comp_simscape_de_all_flex.png]]

** Effect of payload mass
<<ssec:test_nhexa_comp_model_masses>>

Another important characteristic of the model is that it should represents the dynamics of the system well for all considered payloads.
The model dynamics is therefore compared with the measured dynamics for 4 payloads (no payload, 13kg, 26kg and 39kg) in Figure ref:fig:test_nhexa_comp_simscape_diag_masses.
The observed shift of the suspension modes to lower frequencies with increased payload mass is well represented by the multi-body model.
The complex conjugate zeros also well match the experiments both for the encoder outputs (Figure ref:fig:test_nhexa_comp_simscape_de_diag_masses) and the force sensor outputs (Figure ref:fig:test_nhexa_comp_simscape_Vs_diag_masses).

Note that the model displays smaller damping than that observed experimentally for high values of the payload mass.
One option could be to tune the damping as a function of the mass (similar to what is done with the Rayleigh damping).
However, as decentralized IFF will be applied, the damping is actively brought, and the open-loop damping value should have very little impact on the obtained plant dynamics.

#+begin_src matlab :exports none
%% Bode plot for the transfer function from u to de
masses = [0, 13, 26, 39];
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i_mass = [0:3]
    plot(frf_ol.f, abs(frf_ol.G_de{i_mass+1}(:,1, 1)), 'color', [colors(i_mass+1,:), 0.2], ...
         'DisplayName', sprintf('Meas (%i kg)', masses(i_mass+1)));
    for i = 2:6
        plot(frf_ol.f, abs(frf_ol.G_de{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.2], ...
             'HandleVisibility', 'off');
    end
    set(gca, 'ColorOrderIndex', i_mass+1)
    plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{i_mass+1}(1,1), freqs, 'Hz'))), '--', ...
         'DisplayName', 'Simscape');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/u$ [m/V]'); set(gca, 'XTickLabel',[]);
ylim([5e-8, 1e-3]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i_mass = [0:3]
    for i =1:6
        plot(frf_ol.f, 180/pi*angle(frf_ol.G_de{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.2]);
    end
    set(gca, 'ColorOrderIndex', i_mass+1)
    plot(freqs, 180/pi*angle(squeeze(freqresp(sim_ol.G_de{i_mass+1}(1,1), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([-45, 180]);

linkaxes([ax1,ax2],'x');
xlim([20, 2e2]);
xticks([20, 50, 100, 200])
#+end_src

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

#+begin_src matlab :exports none
%% Bode plot for the transfer function from u to Vs
masses = [0, 13, 26, 39];
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i_mass = 0:3
    plot(frf_ol.f, abs(frf_ol.G_Vs{i_mass+1}(:,1, 1)), 'color', [colors(i_mass+1,:), 0.2], ...
         'DisplayName', sprintf('Meas (%i kg)', masses(i_mass+1)));
    for i = 2:6
        plot(frf_ol.f, abs(frf_ol.G_Vs{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.2], ...
             'HandleVisibility', 'off');
    end
    plot(freqs, abs(squeeze(freqresp(sim_ol.G_Vs{i_mass+1}(1,1), freqs, 'Hz'))), '--', 'color', colors(i_mass+1,:), ...
         'DisplayName', 'Simscape');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-3, 1e2]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i_mass = 0:3
    for i =1:6
        plot(frf_ol.f, 180/pi*angle(frf_ol.G_Vs{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.2]);
    end
    plot(freqs, 180/pi*angle(squeeze(freqresp(sim_ol.G_Vs{i_mass+1}(i,i), freqs, 'Hz'))), '--', 'color', colors(i_mass+1,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);

linkaxes([ax1,ax2],'x');
xlim([20, 2e2]);
xticks([20, 50, 100, 200])
#+end_src

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

#+name: fig:test_nhexa_comp_simscape_diag_masses
#+caption: Comparison of the diagonal elements (i.e. "direct" terms) of the measured FRF matrix and the dynamics identified from the multi-body model. Both for the dynamics from $u$ to $d_e$ (\subref{fig:test_nhexa_comp_simscape_de_diag}) and from $u$ to $V_s$ (\subref{fig:test_nhexa_comp_simscape_Vs_diag})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_nhexa_comp_simscape_de_diag_masses}from $u$ to $d_e$}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_nhexa_comp_simscape_de_diag_masses.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_nhexa_comp_simscape_Vs_diag_masses}from $u$ to $V_s$}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_nhexa_comp_simscape_Vs_diag_masses.png]]
#+end_subfigure
#+end_figure

In order to also check if the model well represents the coupling when high payload masses are used, the transfer functions from $u_1$ to $d_{e1}$ to $d_{e6}$ are compared in the case of the 39kg payload in Figure ref:fig:test_nhexa_comp_simscape_de_all_high_mass.
Excellent match between experimental and model coupling is observed.
Therefore, the model effectively represents the system coupling for different payloads.

#+begin_src matlab :exports none
%% Comparison of the plants (encoder output) when tuning the misalignment
i_input = 1;

figure;
tiledlayout(2, 3, 'TileSpacing', 'tight', 'Padding', 'tight');

ax1 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{4}(:, 1, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(1, i_input), freqs, 'Hz'))));
text(12, 4e-4, '$d_{e1}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');

ax2 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{4}(:, 2, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(2, i_input), freqs, 'Hz'))));
text(12, 4e-4, '$d_{e2}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);

ax3 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{4}(:, 3, i_input)), ...
     'DisplayName', 'Measurements');
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(3, i_input), freqs, 'Hz'))), ...
     'DisplayName', 'Model (2-DoF APA)');
text(12, 4e-4, '$d_{e3}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax4 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{4}(:, 4, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(4, i_input), freqs, 'Hz'))));
text(12, 4e-4, '$d_{e4}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
xticks([10, 50, 100, 200, 400])

ax5 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{4}(:, 5, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(5, i_input), freqs, 'Hz'))));
text(12, 4e-4, '$d_{e5}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([10, 50, 100, 200, 400])

ax6 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{4}(:, 6, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(6, i_input), freqs, 'Hz'))));
text(12, 4e-4, '$d_{e6}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([10, 50, 100, 200, 400])

linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
xlim([10, 5e2]); ylim([1e-8, 5e-4]);
#+end_src

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

#+name: fig:test_nhexa_comp_simscape_de_all_high_mass
#+caption: Comparison of the measured (in blue) and modeled (in red) frequency transfer functions from the first control signal $u_1$ to the six encoders $d_{e1}$ to $d_{e6}$
#+RESULTS:
[[file:figs/test_nhexa_comp_simscape_de_all_high_mass.png]]

* Conclusion
The goal of this test bench was to obtain an accurate model of the nano-hexapod that could then be included on top of the micro-station model.
The adopted strategy was to identify the nano-hexapod dynamics under conditions in which all factors that could have affected the nano-hexapod dynamics were considered.
This was achieved by developing a suspended table with low frequency suspension modes that can be accurately modeled (Section ref:sec:test_nhexa_table).
Although the dynamics of the nano-hexapod was indeed impacted by the dynamics of the suspended platform, this impact was also considered in the multi-body model.

The dynamics of the nano-hexapod was then identified in Section ref:sec:test_nhexa_dynamics.
Below the first suspension mode, good decoupling could be observed for the transfer function from $\bm{u}$ to $\bm{d}_e$, which enables the design of a decentralized positioning controller based on the encoders for relative positioning purposes.
Many other modes were present above 700Hz, which will inevitably limit the achievable bandwidth.
The observed effect of the payload's mass on the dynamics was quite large, which also represents a complex control challenge.

The frequency response functions from the six DAC voltages $\bm{u}$ to the six force sensors voltages $\bm{V}_s$ all have alternating complex conjugate poles and complex conjugate zeros for all the tested payloads (Figure ref:fig:test_nhexa_comp_simscape_Vs_diag_masses).
This indicates that it is possible to implement decentralized Integral Force Feedback in a robust manner.

The developed multi-body model of the nano-hexapod was found to accurately represents the suspension modes of the Nano-Hexapod (Section ref:sec:test_nhexa_model).
Both FRF matrices from $\mathbf{u}$ to $\mathbf{V}_s$ and from $\mathbf{u}$ to $\mathbf{d}_e$ are well matching with the measurements, even when considering coupling (i.e. off-diagonal) terms, which are very important from a control perspective.
At frequencies above the suspension modes, the Nano-Hexapod model became inaccurate because the flexible modes were not modeled.
It was found that modeling the APA300ML using a /super-element/ allows to model the internal resonances of the struts.
The same can be done with the top platform and the encoder supports; however, the model order would be higher and may become unpractical for simulation.

Obtaining a model that accurately represents the complex dynamics of the Nano-Hexapod was made possible by the modeling approach used in this study.
This approach involved tuning and validating models of individual components (such as the APA and flexible joints) using dedicated test benches.
The different models could then be combined to form the Nano-Hexapod dynamical model.
If a model of the nano-hexapod was developed in one time, it would be difficult to tune all the model parameters to match the measured dynamics, or even to know if the model "structure" would be adequate to represent the system dynamics.

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

* Glossary                                                            :ignore:
[[printglossaries:]]

# #+latex: \printglossary[type=\acronymtype]
# #+latex: \printglossary[type=\glossarytype]
# #+latex: \printglossary

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

** Initialize Simscape Model
#+NAME: m-init-simscape
#+begin_src matlab
%% Initialize Parameters for Simscape model
table_type = 'Rigid'; % On top of vibration table
device_type = 'None'; % On top of vibration table
payload_num = 0; % No Payload

% Simulink Model name
mdl = 'test_nhexa_simscape';
#+end_src

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

%% Frequency Vector
freqs = logspace(log10(10), log10(2e3), 1000);
#+END_SRC

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

arguments
    %% Bottom Flexible Joints
    args.flex_bot_type      char {mustBeMember(args.flex_bot_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
    args.flex_bot_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
    args.flex_bot_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
    args.flex_bot_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
    args.flex_bot_kz  (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
    args.flex_bot_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
    args.flex_bot_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
    args.flex_bot_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
    args.flex_bot_cz  (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]

    %% Top Flexible Joints
    args.flex_top_type      char {mustBeMember(args.flex_top_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
    args.flex_top_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
    args.flex_top_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
    args.flex_top_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
    args.flex_top_kz  (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
    args.flex_top_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
    args.flex_top_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
    args.flex_top_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
    args.flex_top_cz  (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]

    %% Jacobian - Location of frame {A} and {B}
    args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]

    %% Relative Motion Sensor
    args.motion_sensor_type char {mustBeMember(args.motion_sensor_type,{'struts', 'plates'})} = 'struts'

    %% Top Plate
    args.top_plate_type char {mustBeMember(args.top_plate_type,{'rigid', 'flexible'})} = 'rigid'
    args.top_plate_xi  (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio

    %% Actuators
    args.actuator_type char {mustBeMember(args.actuator_type,{'2dof', 'flexible frame', 'flexible'})} = 'flexible'
    args.actuator_Ga  (6,1) double {mustBeNumeric} = zeros(6,1) % Actuator gain [N/V]
    args.actuator_Gs  (6,1) double {mustBeNumeric} = zeros(6,1) % Sensor gain [V/m]
    % For 2DoF
    args.actuator_k   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*380000
    args.actuator_ke  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*4952605
    args.actuator_ka  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*2476302
    args.actuator_c   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*5
    args.actuator_ce  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*100
    args.actuator_ca  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*50
    args.actuator_Leq (6,1) double {mustBeNumeric} = ones(6,1)*0.056 % [m]
    % For Flexible Frame
    args.actuator_ks (6,1) double {mustBeNumeric} = ones(6,1)*235e6 % Stiffness of one stack [N/m]
    args.actuator_cs (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % Stiffness of one stack [N/m]
    % Misalignment
    args.actuator_d_align (6,3) double {mustBeNumeric} = zeros(6,3) % [m]

    args.actuator_xi  (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio

    %% Controller
    args.controller_type char {mustBeMember(args.controller_type,{'none', 'iff', 'dvf', 'hac-iff-struts'})} = 'none'
end

nano_hexapod = struct();

nano_hexapod.flex_bot = struct();

switch args.flex_bot_type
  case '2dof'
    nano_hexapod.flex_bot.type = 1;
  case '3dof'
    nano_hexapod.flex_bot.type = 2;
  case '4dof'
    nano_hexapod.flex_bot.type = 3;
  case 'flexible'
    nano_hexapod.flex_bot.type = 4;
end

nano_hexapod.flex_bot.kRx = args.flex_bot_kRx; % X bending stiffness [Nm/rad]
nano_hexapod.flex_bot.kRy = args.flex_bot_kRy; % Y bending stiffness [Nm/rad]
nano_hexapod.flex_bot.kRz = args.flex_bot_kRz; % Torsionnal stiffness [Nm/rad]
nano_hexapod.flex_bot.kz  = args.flex_bot_kz;  % Axial stiffness [N/m]

nano_hexapod.flex_bot.cRx = args.flex_bot_cRx; % [Nm/(rad/s)]
nano_hexapod.flex_bot.cRy = args.flex_bot_cRy; % [Nm/(rad/s)]
nano_hexapod.flex_bot.cRz = args.flex_bot_cRz; % [Nm/(rad/s)]
nano_hexapod.flex_bot.cz  = args.flex_bot_cz;  %[N/(m/s)]

nano_hexapod.flex_top = struct();

switch args.flex_top_type
  case '2dof'
    nano_hexapod.flex_top.type = 1;
  case '3dof'
    nano_hexapod.flex_top.type = 2;
  case '4dof'
    nano_hexapod.flex_top.type = 3;
  case 'flexible'
    nano_hexapod.flex_top.type = 4;
end

nano_hexapod.flex_top.kRx = args.flex_top_kRx; % X bending stiffness [Nm/rad]
nano_hexapod.flex_top.kRy = args.flex_top_kRy; % Y bending stiffness [Nm/rad]
nano_hexapod.flex_top.kRz = args.flex_top_kRz; % Torsionnal stiffness [Nm/rad]
nano_hexapod.flex_top.kz  = args.flex_top_kz;  % Axial stiffness [N/m]

nano_hexapod.flex_top.cRx = args.flex_top_cRx; % [Nm/(rad/s)]
nano_hexapod.flex_top.cRy = args.flex_top_cRy; % [Nm/(rad/s)]
nano_hexapod.flex_top.cRz = args.flex_top_cRz; % [Nm/(rad/s)]
nano_hexapod.flex_top.cz  = args.flex_top_cz;  %[N/(m/s)]

nano_hexapod.motion_sensor = struct();

switch args.motion_sensor_type
  case 'struts'
    nano_hexapod.motion_sensor.type = 1;
  case 'plates'
    nano_hexapod.motion_sensor.type = 2;
end

nano_hexapod.actuator = struct();

switch args.actuator_type
  case '2dof'
    nano_hexapod.actuator.type = 1;
  case 'flexible frame'
    nano_hexapod.actuator.type = 2;
  case 'flexible'
    nano_hexapod.actuator.type = 3;
end

%% Actuator gain [N/V]
if all(args.actuator_Ga == 0)
    switch args.actuator_type
      case '2dof'
        nano_hexapod.actuator.Ga = ones(6,1)*(-2.5796);
      case 'flexible frame'
        nano_hexapod.actuator.Ga = ones(6,1); % TODO
      case 'flexible'
        nano_hexapod.actuator.Ga = ones(6,1)*23.2;
    end
else
    nano_hexapod.actuator.Ga = args.actuator_Ga; % Actuator gain [N/V]
end

%% Sensor gain [V/m]
if all(args.actuator_Gs == 0)
    switch args.actuator_type
      case '2dof'
        nano_hexapod.actuator.Gs = ones(6,1)*466664;
      case 'flexible frame'
        nano_hexapod.actuator.Gs = ones(6,1); % TODO
      case 'flexible'
        nano_hexapod.actuator.Gs = ones(6,1)*(-4898341);
    end
else
    nano_hexapod.actuator.Gs = args.actuator_Gs; % Sensor gain [V/m]
end

switch args.actuator_type
  case '2dof'
    nano_hexapod.actuator.k  = args.actuator_k;  % [N/m]
    nano_hexapod.actuator.ke = args.actuator_ke; % [N/m]
    nano_hexapod.actuator.ka = args.actuator_ka; % [N/m]

    nano_hexapod.actuator.c  = args.actuator_c;  % [N/(m/s)]
    nano_hexapod.actuator.ce = args.actuator_ce; % [N/(m/s)]
    nano_hexapod.actuator.ca = args.actuator_ca; % [N/(m/s)]

    nano_hexapod.actuator.Leq = args.actuator_Leq; % [m]

  case 'flexible frame'
    nano_hexapod.actuator.K = readmatrix('APA300ML_b_mat_K.CSV'); % Stiffness Matrix
    nano_hexapod.actuator.M = readmatrix('APA300ML_b_mat_M.CSV'); % Mass Matrix
    nano_hexapod.actuator.P = extractNodes('APA300ML_b_out_nodes_3D.txt'); % Node coordinates [m]

    nano_hexapod.actuator.ks = args.actuator_ks; % Stiffness of one stack [N/m]
    nano_hexapod.actuator.cs = args.actuator_cs; % Damping of one stack [N/m]
    nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio

  case 'flexible'
    nano_hexapod.actuator.K = readmatrix('full_APA300ML_K.CSV'); % Stiffness Matrix
    nano_hexapod.actuator.M = readmatrix('full_APA300ML_M.CSV'); % Mass Matrix
    nano_hexapod.actuator.P = extractNodes('full_APA300ML_out_nodes_3D.txt'); % Node coordiantes [m]

    nano_hexapod.actuator.d_align = args.actuator_d_align; % Misalignment
    nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio

end

nano_hexapod.geometry = struct();

Fa = [[-86.05,  -74.78, 22.49],
      [ 86.05,  -74.78, 22.49],
      [ 107.79, -37.13, 22.49],
      [ 21.74,  111.91, 22.49],
      [-21.74,  111.91, 22.49],
      [-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m]

Mb = [[-28.47, -106.25, -22.50],
      [ 28.47, -106.25, -22.50],
      [ 106.25, 28.47,  -22.50],
      [ 77.78,  77.78,  -22.50],
      [-77.78,  77.78,  -22.50],
      [-106.25, 28.47,  -22.50]]'*1e-3; % Bi w.r.t. {M} [m]

Fb = Mb + [0; 0; 95e-3]; % Bi w.r.t. {F} [m]

si = Fb - Fa;
si = si./vecnorm(si); % Normalize

Fc = [[-29.362, -105.765, 52.605]
      [ 29.362, -105.765, 52.605]
      [ 106.276, 27.454,  52.605]
      [ 76.914,  78.31,   52.605]
      [-76.914,  78.31,   52.605]
      [-106.276, 27.454,  52.605]]'*1e-3; % Meas pos w.r.t. {F}
Mc = Fc - [0; 0; 95e-3]; % Meas pos w.r.t. {M}

nano_hexapod.geometry.Fa = Fa;
nano_hexapod.geometry.Fb = Fb;
nano_hexapod.geometry.Fc = Fc;
nano_hexapod.geometry.Mb = Mb;
nano_hexapod.geometry.Mc = Mc;
nano_hexapod.geometry.si = si;
nano_hexapod.geometry.MO_B = args.MO_B;

Bb = Mb - [0; 0; args.MO_B];

nano_hexapod.geometry.J = [nano_hexapod.geometry.si', cross(Bb, nano_hexapod.geometry.si)'];

switch args.motion_sensor_type
  case 'struts'
    nano_hexapod.geometry.Js = nano_hexapod.geometry.J;
  case 'plates'
    Bc = Mc - [0; 0; args.MO_B];
    nano_hexapod.geometry.Js = [nano_hexapod.geometry.si', cross(Bc, nano_hexapod.geometry.si)'];
end

nano_hexapod.top_plate = struct();

switch args.top_plate_type
  case 'rigid'
    nano_hexapod.top_plate.type = 1;
  case 'flexible'
    nano_hexapod.top_plate.type = 2;

    nano_hexapod.top_plate.R_flex = ...
       {[ 0.53191886726305  0.4795690716524  0.69790817745892
         -0.29070157897799  0.8775041341865 -0.38141720787774
         -0.79533320729697  0                0.60617249143351 ],
        [ 0.53191886726305 -0.4795690716524 -0.69790817745892
          0.29070157897799  0.8775041341865 -0.38141720787774
          0.79533320729697  0                0.60617249143351 ],
        [-0.01420448131633 -0.9997254079576 -0.01863709726680
          0.60600604129104 -0.0234330681729  0.79511481512719
         -0.79533320729697  0                0.60617249143351 ],
        [-0.51771438594672 -0.5201563363051  0.67927108019212
          0.31530446231304 -0.8540710660135 -0.41369760724945
          0.79533320729697  0                0.60617249143351 ],
        [-0.51771438594671  0.5201563363052 -0.67927108019211
         -0.31530446231304 -0.8540710660135 -0.41369760724945
         -0.79533320729697  0                0.60617249143351 ],
        [-0.01420448131632  0.9997254079576  0.01863709726679
         -0.60600604129104 -0.0234330681729  0.79511481512719
          0.79533320729697  0                0.60617249143351 ] };


    nano_hexapod.top_plate.R_enc = ...
      { [-0.877504134186525 -0.479569071652412 0
         0.479569071652412 -0.877504134186525 0
                         0                  0 1 ],
        [ 0.877504134186525 -0.479569071652413 0
          0.479569071652413  0.877504134186525 0
                          0                  0 1 ],
        [ 0.023433068172945  0.999725407957606 0
         -0.999725407957606  0.023433068172945 0
                          0                  0 1 ],
        [-0.854071066013566 -0.520156336305202 0
          0.520156336305202 -0.854071066013566 0
                          0                  0 1 ],
        [ 0.854071066013574 -0.520156336305191 0
          0.520156336305191  0.854071066013574 0
                          0                  0 1 ],
        [-0.023433068172958  0.999725407957606 0
         -0.999725407957606 -0.023433068172958 0
                          0                  0 1 ] };

    nano_hexapod.top_plate.K = readmatrix('top_plate_K_6.CSV'); % Stiffness Matrix
    nano_hexapod.top_plate.M = readmatrix('top_plate_M_6.CSV'); % Mass Matrix
    nano_hexapod.top_plate.P = extractNodes('top_plate_out_nodes_3D_qua.txt'); % Node coordiantes [m]
    nano_hexapod.top_plate.xi = args.top_plate_xi; % Damping ratio

end
#+end_src

** =extractNodes=
#+begin_src matlab :tangle matlab/src/extractNodes.m :comments none :mkdirp yes :eval no
function [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes(filename)
% extractNodes -
%
% Syntax: [n_xyz, nodes] = extractNodes(filename)
%
% Inputs:
%    - filename - relative or absolute path of the file that contains the Matrix
%
% Outputs:
%    - n_xyz -
%    - nodes - table containing the node numbers and corresponding dof of the interfaced DoFs

arguments
    filename
end

fid = fopen(filename,'rt');

if fid == -1
    error('Error opening the file');
end

n_xyz = []; % Contains nodes coordinates
n_i = []; % Contains nodes indices

n_num = []; % Contains node numbers
n_dof = {}; % Contains node directions

while 1
    % Read a line
    nextline = fgetl(fid);

    % End of the file
    if ~isstr(nextline), break, end

    % Line just before the list of nodes coordinates
    if contains(nextline, 'NODE') && ...
            contains(nextline, 'X') && ...
            contains(nextline, 'Y') && ...
            contains(nextline, 'Z')

        while 1
            nextline = fgetl(fid);

            if nextline < 0, break, end

            c = sscanf(nextline, ' %f');

            if isempty(c), break, end

            n_xyz = [n_xyz; c(2:4)'];
            n_i = [n_i; c(1)];
        end
    end

    if nextline < 0, break, end

    % Line just before the list of node DOF
    if contains(nextline, 'NODE  LABEL')

        while 1
            nextline = fgetl(fid);

            if nextline < 0, break, end

            c = sscanf(nextline, ' %d %s');

            if isempty(c), break, end

            n_num = [n_num; c(1)];

            n_dof{length(n_dof)+1} = char(c(2:end)');
        end

        nodes = table(n_num, string(n_dof'), 'VariableNames', {'node_i', 'node_dof'});
    end

    if nextline < 0, break, end
end

fclose(fid);

int_i = unique(nodes.('node_i')); % indices of interface nodes

% Extract XYZ coordinates of only the interface nodes
if length(n_xyz) > 0 && length(n_i) > 0
    int_xyz = n_xyz(logical(sum(n_i.*ones(1, length(int_i)) == int_i', 2)), :);
else
    int_xyz = n_xyz;
end
#+end_src

* Footnotes

[fn:7]PCB 356B18. Sensitivity is $1\,V/g$, measurement range is $\pm 5\,g$ and bandwidth is $0.5$ to $5\,\text{kHz}$.
[fn:6]"SZ8005 20 x 044" from Steinel. The spring rate is specified at $17.8\,N/mm$
[fn:5]The 450 mm x 450 mm x 60 mm Nexus B4545A from Thorlabs.
[fn:4]As the accuracy of the FARO arm is $\pm 13\,\mu m$, the true straightness is probably better than the values indicated. The limitation of the instrument is here reached.
[fn:3]The height dimension is better than $40\,\mu m$. The diameter fitting of 182g6 and 24g6 with the two plates is verified.
[fn:2]Location of all the interface surfaces with the flexible joints were checked. The fittings (182H7 and 24H8) with the interface element were also checked.
[fn:1]FARO Arm Platinum 4ft, specified accuracy of $\pm 13\mu m$