#+TITLE: Nano Hexapod - Optimal Geometry
: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{nass-geometry.bib}

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

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

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

#+latex: \clearpage

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

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

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

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

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

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

* Notes                                                             :noexport:
** Notes
Prefix is =detail_kinematics=

Talk about the optimization of the nano-hexapod: geometry, stiffness, etc...
- [X] [[file:~/Cloud/work-projects/ID31-NASS/documents/state-of-thesis-2020/index.org::*Optimal Nano-Hexapod Design][Optimal Nano-Hexapod Design]]
- [X] file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/kinematic-study.org
- [X] file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/flexible-stewart-platform.org
  Not so interesting

- [ ] Talk about what will influence the dynamics
  It will influence the mechanical design.
  For instance we want to precisely position =bi= with respect to the top platform

Optimal geometry?
- [X] *Cubic architecture*?
  Cubic configuration file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/cubic-configuration.org
  https://tdehaeze.github.io/stewart-simscape/cubic-configuration.html
- [X] Kinematics
- [X] Trade-off for the strut orientation
- [X] Requirements in terms of positioning of the joints
- [X] Not a lot of differences, no specificity of cubic architecture, no specific positioning

- [X] https://research.tdehaeze.xyz/stewart-simscape/docs/bibliography.html
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/kinematic-study.org::*Estimated required actuator stroke from specified platform mobility][Estimated required actuator stroke from specified platform mobility]]
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/kinematic-study.org::*Estimation of the Joint required Stroke][Estimation of the Joint required Stroke]]

** Not used
*** Review table

#+name: tab:detail_kinematics_stewart_review
#+caption: Examples of Stewart platform developed. When not specifically indicated, sensors are included in the struts. All presented Stewart platforms are using flexible joints. The table is sorted by order of appearance in the literature
#+attr_latex: :environment tabularx :width 0.9\linewidth :align Xcccc
#+attr_latex: :center t :booktabs t :font \scriptsize
|                                          | *Geometry*   | *Actuators*                  | *Sensors*              | *Reference*                                                                                                                                                                                             |
|------------------------------------------+--------------+------------------------------+------------------------+---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
|                                          | Cubic        | Magnetostrictive             | Force, Accelerometers  | [[cite:&geng93_six_degree_of_freed_activ;&geng94_six_degree_of_freed_activ;&geng95_intel_contr_system_multip_degree]]                                                                                       |
| Figure ref:fig:detail_kinematics_jpl     | Cubic        | Voice Coil (0.5 mm)          | Force                  | [[cite:&spanos95_soft_activ_vibrat_isolat;&rahman98_multiax]]                                                                                                                                               |
|                                          | Cubic        | Voice Coil (10 mm)           | Force, LVDT, Geophones | [[cite:&thayer98_stewar;&thayer02_six_axis_vibrat_isolat_system;&hauge04_sensor_contr_space_based_six]]                                                                                                     |
| Figure ref:fig:detail_kinematics_uw_gsp  | Cubic        | Voice Coil                   | Force                  | [[cite:&mcinroy99_dynam;&mcinroy99_precis_fault_toler_point_using_stewar_platf;&mcinroy00_desig_contr_flexur_joint_hexap;&li01_simul_vibrat_isolat_point_contr;&jafari03_orthog_gough_stewar_platf_microm]] |
|                                          | Cubic        | Piezoelectric ($25\,\mu m$)  | Force                  | [[cite:&defendini00_techn]]                                                                                                                                                                                 |
| Figure ref:fig:detail_kinematics_ulb_pz  | Cubic        | APA  ($50\,\mu m$)           | Force                  | [[cite:&abu02_stiff_soft_stewar_platf_activ]]                                                                                                                                                               |
| Figure ref:fig:detail_kinematics_pph     | Non-Cubic    | Voice Coil                   | Accelerometers         | [[cite:&chen03_payload_point_activ_vibrat_isolat]]                                                                                                                                                          |
|                                          | Cubic        | Voice Coil                   | Force                  | [[cite:&hanieh03_activ_stewar;&preumont07_six_axis_singl_stage_activ]]                                                                                                                                      |
| Figure ref:fig:detail_kinematics_uqp     | Cubic        | Piezoelectric ($50\,\mu m$)  | Geophone               | [[cite:&agrawal04_algor_activ_vibrat_isolat_spacec]]                                                                                                                                                        |
|                                          | Non-Cubic    | Piezoelectric ($16\,\mu m$)  | Eddy Current           | [[cite:&furutani04_nanom_cuttin_machin_using_stewar]]                                                                                                                                                       |
|                                          | Cubic        | Piezoelectric ($120\,\mu m$) | (External) Capacitive  | [[cite:&ting06_desig_stewar_nanos_platf;&ting13_compos_contr_desig_stewar_nanos_platf]]                                                                                                                     |
|                                          | Non-Cubic    | Piezoelectric ($160\,\mu m$) | (External) Capacitive  | [[cite:&ting07_measur_calib_stewar_microm_system]]                                                                                                                                                          |
| Figure ref:fig:detail_kinematics_zhang11 | Non-cubic    | Magnetostrictive             | Accelerometer          | [[cite:&zhang11_six_dof]]                                                                                                                                                                                   |
|                                          | Non-Cubic    | Piezoelectric                | Strain Gauge           | [[cite:&du14_piezo_actuat_high_precis_flexib]]                                                                                                                                                              |
|                                          | Cubic        | Voice Coil                   | Accelerometer          | [[cite:&chi15_desig_exper_study_vcm_based;&tang18_decen_vibrat_contr_voice_coil;&jiao18_dynam_model_exper_analy_stewar]]                                                                                    |
|                                          | Cubic        | Piezoelectric                | Force                  | [[cite:&wang16_inves_activ_vibrat_isolat_stewar]]                                                                                                                                                           |
|                                          | Almost cubic | Voice Coil                   | Force, Accelerometer   | [[cite:&beijen18_self_tunin_mimo_distur_feedf;&tjepkema12_activ_ph]]                                                                                                                                        |
| Figure ref:fig:detail_kinematics_yang19  | Almost cubic | Piezoelectric                | Force, Strain gauge    | [[cite:&yang19_dynam_model_decoup_contr_flexib]]                                                                                                                                                            |
| Figure ref:fig:detail_kinematics_naves   | Non-Cubic    | 3-phase rotary motor         | Rotary Encoder         | [[cite:&naves20_desig;&naves20_t_flex]]                                                                                                                                                                     |

*** Dynamic isotropy

[[cite:&afzali-far16_vibrat_dynam_isotr_hexap_analy_studies]]:
- proposes an architecture where the CoM can be above the top platform
- [ ] Try to find a stewart platform an a payload for witch all resonances are equal.
  Massless struts and plates.
  Cylindrical payload tuned to obtained such property.
  Show that all modes can be optimally damped?
  Compare with Stewart platform with different resonance frequencies. Also need to have parallel stiffness with the struts
- Same resonance frequencies for suspension modes?
  Maybe in one case: sphere at the CoM?
  Could be nice to show that.
  Say that this can be nice for optimal damping for instance (link to paper explaining that)


- Show examples where the dynamics can indeed be decoupled in the cartesian frame (i.e. decoupled K and M matrices)


- [ ] Try to find a stewart platform an a payload for witch all resonances are equal.
  It is not possible for a cubic architecture and a cylinder.
  But maybe we can propose to not use a cubic architecture and still have dynamic isotropy?
  For instance, let's consider a specific payload => give some rules to obtain dynamic isotropy:


  Inertia of a cylinder:
  Iz = 1/2mr^2
  Ix = Iy = 1/12 m (3r^2 + h^2)

We want M = alpha K

2k = alpha m

alpha = 2k/m

Krx = 3/2 k H^2 = Ix = 1/12 m

#+begin_src matlab
%% Cubic Stewart platform with payload above the top platform
H = 200e-3;
MO_B = -100e-3;  % Position {B} with respect to {M} [m]

stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeCylindricalPlatforms(stewart, ...
                                         'Mpm', 1e-6, ... % Massless platform
                                         'Fpm', 1e-6, ... % Massless platform
                                         'Mph', 20e-3, ... % Thin platform
                                         'Fph', 20e-3, ... % Thin platform
                                         'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
                                         'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
stewart = initializeCylindricalStruts(stewart, ...
    'Fsm', 1e-6, ... % Massless strut
    'Msm', 1e-6, ... % Massless strut
    'Fsh', stewart.geometry.l(1)/2, ...
    'Msh', stewart.geometry.l(1)/2 ...
);

% Sample at the Center of the cube
sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 1/9*H, 'H_offset', -H/2-1/9*H/2, 'R', sqrt(6)*H);

% Run the linearization
G_CoM_CoK = linearize(mdl, io)*inv(stewart.kinematics.J.');
G_CoM_CoK.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_CoM_CoK.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src

#+begin_src matlab :exports none
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$D_x/F_x$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$D_y/F_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ...
     'DisplayName', '$D_z/F_z$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ...
     'DisplayName', '$R_x/M_x$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ...
     'DisplayName', '$R_y/M_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ...
     'DisplayName', '$R_z/M_z$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$R_x/F_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$R_y/F_x$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$D_x/M_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$D_y/M_x$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]);
ylim([1e-10, 2e-3])
#+end_src

*** Cubic with centered payload                                   :noexport:

It seems having  the payload at the center of the cube makes things worst for decentralized control.

#+begin_src matlab
%% Cubic Stewart platform with payload above the top platform
H = 200e-3;
MO_B = -100e-3;  % Position {B} with respect to {M} [m]

stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeCylindricalPlatforms(stewart, ...
                                         'Mpm', 1e-6, ... % Massless platform
                                         'Fpm', 1e-6, ... % Massless platform
                                         'Mph', 20e-3, ... % Thin platform
                                         'Fph', 20e-3, ... % Thin platform
                                         'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
                                         'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
stewart = initializeCylindricalStruts(stewart, ...
    'Fsm', 1e-6, ... % Massless strut
    'Msm', 1e-6, ... % Massless strut
    'Fsh', stewart.geometry.l(1)/2, ...
    'Msh', stewart.geometry.l(1)/2 ...
);

% Sample at the Center of the cube
sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'H_offset', -H/2-50e-3);

% Run the linearization
G_CoM_CoK = linearize(mdl, io)*inv(stewart.kinematics.J.');
G_CoM_CoK.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_CoM_CoK.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
                      'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
#+end_src

#+begin_src matlab :exports none :results none
%% Coupling in the cartesian frame for a Non_cubic Stewart platform - Frame {B} is at the center of mass of the payload
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_non_cubic(sprintf('fn%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(sprintf('fn%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(3,:), 0.1], ...
             'HandleVisibility', 'off');
    end
end
plot(freqs, abs(squeeze(freqresp(G_non_cubic('fn1', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:)], 'linewidth', 2.5, ...
     'DisplayName', '$L_i/F_i$');
plot(freqs, abs(squeeze(freqresp(G_non_cubic('fn2', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
     'DisplayName', '$L_i/F_j$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK('fn1', 'f1'), freqs, 'Hz'))), 'color', [colors(3,:)], 'linewidth', 2.5, ...
     'DisplayName', '$L_i/F_i$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK('fn2', 'f1'), freqs, 'Hz'))), 'color', [colors(3,:), 0.1], ...
     'DisplayName', '$L_i/F_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [N/N]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]);
ylim([1e-3, 1e2])
#+end_src

#+begin_src matlab :exports none :results none
%% Coupling in the cartesian frame for a Non_cubic Stewart platform - Frame {B} is at the center of mass of the payload
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_non_cubic(sprintf('dL%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_cubic(sprintf('dL%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(sprintf('dL%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(3,:), 0.1], ...
             'HandleVisibility', 'off');
    end
end
plot(freqs, abs(squeeze(freqresp(G_non_cubic('dL1', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:)], 'linewidth', 2.5, ...
     'DisplayName', '$L_i/F_i$');
plot(freqs, abs(squeeze(freqresp(G_non_cubic('dL2', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
     'DisplayName', '$L_i/F_j$');
plot(freqs, abs(squeeze(freqresp(G_cubic('dL1', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:)], 'linewidth', 2.5, ...
     'DisplayName', '$L_i/F_i$');
plot(freqs, abs(squeeze(freqresp(G_cubic('dL2', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
     'DisplayName', '$L_i/F_j$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK('dL1', 'f1'), freqs, 'Hz'))), 'color', [colors(3,:)], 'linewidth', 2.5, ...
     'DisplayName', '$L_i/F_i$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK('dL2', 'f1'), freqs, 'Hz'))), 'color', [colors(3,:), 0.1], ...
     'DisplayName', '$L_i/F_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]);
ylim([1e-10, 2e-3])
#+end_src


** DONE [#B] Change review based on christophe's comments
CLOSED: [2025-04-04 Fri 21:11] SCHEDULED: <2025-04-04 Fri>

- [-] make sure that all papers are cited
  - [X] geng93_six_degree_of_freed_activ
  - [X] geng94_six_degree_of_freed_activ
  - [X] geng95_intel_contr_system_multip_degree
  - [X] spanos95_soft_activ_vibrat_isolat
  - [X] rahman98_multiax
  - [X] thayer98_stewar
  - [X] thayer02_six_axis_vibrat_isolat_system
  - [X] hauge04_sensor_contr_space_based_six
  - [X] mcinroy99_dynam
  - [ ] mcinroy99_precis_fault_toler_point_using_stewar_platf
  - [X] mcinroy00_desig_contr_flexur_joint_hexap
  - [X] li01_simul_vibrat_isolat_point_contr
  - [X] jafari03_orthog_gough_stewar_platf_microm
  - [X] defendini00_techn
  - [X] abu02_stiff_soft_stewar_platf_activ
  - [X] chen03_payload_point_activ_vibrat_isolat
  - [X] hanieh03_activ_stewar
  - [X] preumont07_six_axis_singl_stage_activ
  - [X] agrawal04_algor_activ_vibrat_isolat_spacec
  - [X] furutani04_nanom_cuttin_machin_using_stewar
  - [ ] ting06_desig_stewar_nanos_platf
  - [X] ting13_compos_contr_desig_stewar_nanos_platf
  - [X] ting07_measur_calib_stewar_microm_system
  - [X] zhang11_six_dof
  - [X] du14_piezo_actuat_high_precis_flexib
  - [X] chi15_desig_exper_study_vcm_based
  - [X] tang18_decen_vibrat_contr_voice_coil
  - [X] jiao18_dynam_model_exper_analy_stewar
  - [X] wang16_inves_activ_vibrat_isolat_stewar
  - [ ] beijen18_self_tunin_mimo_distur_feedf
  - [ ] tjepkema12_activ_ph
  - [X] yang19_dynam_model_decoup_contr_flexib
  - [ ] naves20_desig
  - [ ] naves20_t_flex
- [-] make sure that all figures are cited
  - [X] Figure ref:fig:detail_kinematics_jpl
  - [X] Figure ref:fig:detail_kinematics_uw_gsp
  - [X] Figure ref:fig:detail_kinematics_ulb_pz
  - [X] Figure ref:fig:detail_kinematics_pph
  - [X] Figure ref:fig:detail_kinematics_uqp
  - [X] Figure ref:fig:detail_kinematics_zhang11
  - [X] Figure ref:fig:detail_kinematics_yang19
  - [ ] Figure ref:fig:detail_kinematics_naves

** DONE [#B] Read everything again and check figures
CLOSED: [2025-04-01 Tue 18:17] SCHEDULED: <2025-04-01 Tue>

** DONE [#A] Copy relevant parts of reports
CLOSED: [2025-04-01 Tue 15:09]

** DONE [#A] Structure the review of Stewart platforms
CLOSED: [2025-03-30 Sun 11:38]

Focus on short stroke (<1 mm) stewart platforms with flexible joints.

- Actuators: voice coil, piezo
- Flexible joints
- Geometry:
  - Cubic, non cubic, ...
- Control ? Maybe in the control section ?

** DONE [#B] Finish table about Stewart platforms
CLOSED: [2025-03-30 Sun 11:54]

[[tab:detail_kinematics_stewart_review][tab:detail_kinematics_stewart_review]]

- [ ] Only keep integrated sensor and not external metrology
- [ ] Check for missing information

** DONE [#A] Maybe need to have the Simscape model for the stewart platform?
CLOSED: [2025-03-30 Sun 19:38]

For what use?
Study of coupling between strut sensor

** DONE [#A] Verify Mobility of Stewart platform
CLOSED: [2025-04-01 Tue 09:36]

*Mobility is not a sphere but a cube!*

From [[cite:&mcinroy00_desig_contr_flexur_joint_hexap]]:
#+begin_quote
This implies that, if all struts can move equal
amounts, then the translational part of the hexapod’s Cartesian
workspace forms a *sphere*. Thus, this is a useful geometry when
the ability to move equal amounts in all translational directions
is required.
#+end_quote
*This is wrong*

Maybe easy check by taking few example points.
#+begin_src matlab :exports none :results none
%% Example of one Stewart platform and associated translational mobility
L_max =  50e-6; % Maximum actuator stroke (+/-) [m]

stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', 100e-3, 'MO_B', -50e-3);
stewart = generateGeneralConfiguration(stewart, 'FH', 0, 'FR', 100e-3, 'MH', 0, 'MR', 100e-3, ...
                                      'FTh', [-5, 5, 120-5, 120+5, 240-5, 240+5]*(pi/180), ...
                                      'MTh', [-60+5, 60-5, 60+5, 180-5, 180+5, -60-5]*(pi/180));
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1);
stewart = computeJacobian(stewart);
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 110e-3, 'Mpr', 110e-3);

displayArchitecture(stewart, 'labels', false, 'frames', false, 'F_color', [0,0,0], 'M_color', [0,0,0], 'L_color', [0,0,0]);
#+end_src

#+begin_src matlab
thetas = linspace(0, pi, 100);
phis = linspace(0, 2*pi, 100);
rs = zeros(length(thetas), length(phis));

for i = 1:length(thetas)
    for j = 1:length(phis)
        % Unit vector pointing to "spherical direction"
        Tx = sin(thetas(i))*cos(phis(j));
        Ty = sin(thetas(i))*sin(phis(j));
        Tz = cos(thetas(i));

        % Compute Strut motion required for unit displacement in theta/phi direction
        dL = stewart.kinematics.J*[Tx; Ty; Tz; 0; 0; 0;];

        % Due to maximum strut stroke, maximum displacement in the considered direction is limited
        rs(i, j) = L_max/max(abs(dL));
    end
end

% Sphere with radius equal to actuator stroke
[Xs,Ys,Zs] = sphere;
Xs = 1e6*L_max*Xs;
Ys = 1e6*L_max*Ys;
Zs = 1e6*L_max*Zs;

[phi_grid, theta_grid] = meshgrid(phis, thetas);
X = 1e6 * rs .* sin(theta_grid) .* cos(phi_grid);
Y = 1e6 * rs .* sin(theta_grid) .* sin(phi_grid);
Z = 1e6 * rs .* cos(theta_grid);

figure('Renderer', 'painters');
hold on;
surf(X,  Y,  Z,  'FaceColor', 'none', 'LineWidth', 0.1, 'EdgeColor', [0, 0, 0]);
surf(Xs, Ys, Zs, 'FaceColor', colors(2,:), 'EdgeColor', 'none');
quiver3(0, 0, 0, 100, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.9);
quiver3(0, 0, 0, 0, 100, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.9);
quiver3(0, 0, 0, 0, 0, 100, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.9);
hold off;
axis equal;
grid off;
axis off;
view(55, 25);
#+end_src

#+begin_src matlab
for i = 1:length(thetas)
    for j = 1:length(phis)
        max(abs(stewart.kinematics.J*[X(i,j); Y(i,j); Z(i,j); 0; 0; 0]))
    end
end
#+end_src

#+begin_src matlab
inv(stewart.kinematics.J)*[42.8; 40.8; 29.3; 0; 0; 0]
stewart.kinematics.J*[-6.4; -0.89; 24.6; 136.3; -348.7; -89.2]
#+end_src

** DONE [#A] Compute all the figures
CLOSED: [2025-04-01 Tue 15:09] SCHEDULED: <2025-04-01 Tue>

** DONE [#B] Verify cubic architecture in literature
CLOSED: [2025-04-01 Tue 11:31]

[[cite:&yang19_dynam_model_decoup_contr_flexib]]: *It is not cubic*
#+begin_src matlab
a1 = [0.1829; 0.06039; -0.03515];
a2 = [-0.03916; 0.1886; -0.03515];
a3 = [-0.1438; 0.1282 ;-0.03515];
b1 = [0.02771; 0.1906; 0.03515];
b2 = [0.1512; 0.1193; 0.03515];
b3 = [-0.1789; -0.07131; 0.03515];

v1 = b1-a1;
v2 = b2-a2;
v3 = b3-a3;
angle_rad = acos(dot(v1,v3)/(norm(v1)*norm(v3)));
angle_deg = rad2deg(angle_rad)
#+end_src


[[cite:&furutani04_nanom_cuttin_machin_using_stewar]]
It says 35 degrees from the horizontal plane for the struts.
Check if this is the case for a cubic architecture.
#+begin_src matlab
H    = 200e-3; % height of the Stewart platform [m]
MO_B = 50e-3;  % Position {B} with respect to {M} [m]

stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeCylindricalPlatforms(stewart, ...
                                         'Mpm', 1e-6, ... % Massless platform
                                         'Fpm', 1e-6, ... % Massless platform
                                         'Mph', 20e-3, ... % Thin platform
                                         'Fph', 20e-3, ... % Thin platform
                                         'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
                                         'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
stewart = initializeCylindricalStruts(stewart, ...
    'Fsm', 1e-6, ... % Massless strut
    'Msm', 1e-6, ... % Massless strut
    'Fsh', stewart.geometry.l(1)/2, ...
    'Msh', stewart.geometry.l(1)/2 ...
);

% Verify 90 degrees between the struts
v1 = stewart.geometry.As(:,1);
v2 = stewart.geometry.As(:,2);
angle_rad = acos(dot(v1,v2)/(norm(v1)*norm(v2)));
angle_deg = rad2deg(angle_rad)

% Check angle with horizontal plane
v1 = stewart.geometry.As(:,1)
v2 = [1;0;0]
angle_rad = acos(dot(v1,v2)/(norm(v1)*norm(v2)));
angle_deg = rad2deg(angle_rad)
#+end_src

** DONE [#A] Get analytical formula of stiffness matrix for cubic architecture?
CLOSED: [2025-03-31 Mon 21:40]
*** Introduction
- [ ] Copy this analysis to relevant section

- Make some simplifying assumption:
  - Cube at the center of the Stewart platform
  - Cube size =
  - Height of the mobile joints =

#+begin_src latex :file detail_kinematics_cubic_schematic.pdf
  \begin{tikzpicture}
    \begin{scope}[rotate={45}, shift={(0, 0, -4)}]
      % We first define the coordinate of the points of the Cube
      \coordinate[] (bot) at (0,0,4);
      \coordinate[] (top) at (4,4,0);
      \coordinate[] (A1) at (0,0,0);
      \coordinate[] (A2) at (4,0,4);
      \coordinate[] (A3) at (0,4,4);
      \coordinate[] (B1) at (4,0,0);
      \coordinate[] (B2) at (4,4,4);
      \coordinate[] (B3) at (0,4,0);

      % Center of the Cube
      \node[] (cubecenter) at ($0.5*(bot) + 0.5*(top)$){$\bullet$};
      \node[above right] at (cubecenter){$C$};

      % We draw parts of the cube that corresponds to the Stewart platform
      \draw[] (A1)node[]{$\bullet$} -- (B1)node[]{$\bullet$} -- (A2)node[]{$\bullet$} -- (B2)node[]{$\bullet$} -- (A3)node[]{$\bullet$} -- (B3)node[]{$\bullet$} -- (A1);

      % ai and bi are computed
      \def\lfrom{0.1}
      \def\lto{0.8}

      \coordinate(a1) at ($(A1) - \lfrom*(A1) + \lfrom*(B1)$);
      \coordinate(b1) at ($(A1) - \lto*(A1)   + \lto*(B1)$);
      \coordinate(a2) at ($(A2) - \lfrom*(A2) + \lfrom*(B1)$);
      \coordinate(b2) at ($(A2) - \lto*(A2)   + \lto*(B1)$);
      \coordinate(a3) at ($(A2) - \lfrom*(A2) + \lfrom*(B2)$);
      \coordinate(b3) at ($(A2) - \lto*(A2)   + \lto*(B2)$);
      \coordinate(a4) at ($(A3) - \lfrom*(A3) + \lfrom*(B2)$);
      \coordinate(b4) at ($(A3) - \lto*(A3)   + \lto*(B2)$);
      \coordinate(a5) at ($(A3) - \lfrom*(A3) + \lfrom*(B3)$);
      \coordinate(b5) at ($(A3) - \lto*(A3)   + \lto*(B3)$);
      \coordinate(a6) at ($(A1) - \lfrom*(A1) + \lfrom*(B3)$);
      \coordinate(b6) at ($(A1) - \lto*(A1)   + \lto*(B3)$);

      % Center of the Stewart Platform
      % \node[color=colorblue] at ($0.25*(a1) + 0.25*(a6) + 0.25*(b3) + 0.25*(b4)$){$\bullet$};

      % We draw the fixed and mobiles platforms
      \path[fill=colorblue, opacity=0.2] (a1) -- (a2) -- (a3) -- (a4) -- (a5) -- (a6) -- cycle;
      \path[fill=colorblue, opacity=0.2] (b1) -- (b2) -- (b3) -- (b4) -- (b5) -- (b6) -- cycle;
      \draw[color=colorblue, dashed] (a1) -- (a2) -- (a3) -- (a4) -- (a5) -- (a6) -- cycle;
      \draw[color=colorblue, dashed] (b1) -- (b2) -- (b3) -- (b4) -- (b5) -- (b6) -- cycle;

      % The legs of the hexapod are drawn
      \draw[color=colorblue] (a1)node{$\bullet$} -- node[midway, right]{$3$} (b1)node{$\bullet$};
      \draw[color=colorblue] (a2)node{$\bullet$} -- node[midway, right]{$4$} (b2)node{$\bullet$};
      \draw[color=colorblue] (a3)node{$\bullet$} -- node[midway, right]{$5$} (b3)node{$\bullet$};
      \draw[color=colorblue] (a4)node{$\bullet$} -- node[midway, right]{$6$} (b4)node{$\bullet$};
      \draw[color=colorblue] (a5)node{$\bullet$} -- node[midway, right]{$1$} (b5)node{$\bullet$};
      \draw[color=colorblue] (a6)node{$\bullet$} -- node[midway, right]{$2$} (b6)node{$\bullet$};

      % Labels
      \node[left=0.1 of a5] {$a_i$};
      \node[left=0.1 of b5] {$b_i$};
    \end{scope}

    % Height of the Hexapod
    \coordinate[] (sizepos) at ($(a2)+(0.2, 0)$);
    \coordinate[] (origin) at (0,0,0);
    % \draw[<->, dashed] (a2-|sizepos) -- node[midway, right]{$H$} (b2-|sizepos);
    % Height offset
    % \draw[<->, dashed] (a2-|sizepos) -- node[midway, right]{$H_0$} (origin-|sizepos);

    \draw[->] (0,6,0) node[above right]{$\{M\}$} -- ++(0,0,1);
    \draw[->] (0,6,0) -- ++(1,0,0);
    \draw[->] (0,6,0) -- ++(0,1,0);

    \draw[->] (0,2,0) node[below right]{$\{F\}$}-- ++(0,0,1)node[left]{$x$};
    \draw[->] (0,2,0) -- ++(1,0,0)node[above]{$y$};
    \draw[->] (0,2,0) -- ++(0,1,0)node[right]{$z$};

    \draw[<->, dashed] (0.5,2,0) --node[near end, right]{${}^{F}O_{C}$} ++(0,2,0);
    \draw[<->, dashed] (-2,2,0) --node[midway, right]{${}^{F}H_{A}$} ++(0,1,0);
    \draw[<->, dashed] (-2,6,0) --node[midway, right]{${}^{M}H_{B}$} ++(0,-1.5,0);

    % Useful part of the cube
    \draw[<->, dashed] ($(A2)+(0.5,0)$) -- node[midway, right]{$H_{C}$} ($(B1)+(0.5,0)$);
  \end{tikzpicture}
#+end_src

#+RESULTS:
[[file:figs/detail_kinematics_cubic_schematic.png]]

Compute:
- [X] si : easy (always the same)
- [X] bi : should also be easy
  - Have to find intersection between struts and top platform (height of the joints)
  - Need the equation of the cube's sides?


\begin{equation}
\hat{\bm{s}}_1 = \begin{bmatrix}  \sqrt{2}/\sqrt{3} \\  0          \\ 1/\sqrt{3} \end{bmatrix} \quad
\hat{\bm{s}}_2 = \begin{bmatrix} -1/\sqrt{6}        \\ -1/\sqrt{2} \\ 1/\sqrt{3} \end{bmatrix} \quad
\hat{\bm{s}}_3 = \begin{bmatrix} -1/\sqrt{6}        \\  1/\sqrt{2} \\ 1/\sqrt{3} \end{bmatrix} \quad
\hat{\bm{s}}_4 = \begin{bmatrix}  \sqrt{2}/\sqrt{3} \\  0          \\ 1/\sqrt{3} \end{bmatrix} \quad
\hat{\bm{s}}_5 = \begin{bmatrix} -1/\sqrt{6}        \\ -1/\sqrt{2} \\ 1/\sqrt{3} \end{bmatrix} \quad
\hat{\bm{s}}_6 = \begin{bmatrix} -1/\sqrt{6}        \\  1/\sqrt{2} \\ 1/\sqrt{3} \end{bmatrix}
\end{equation}


\begin{equation}
  \sum_{i = 1}^{6} \hat{\bm{s}}_i \cdot \hat{\bm{s}}_i^{\intercal} = 2 \bm{I}_3
\end{equation}


Need to include size of the cube for bi:
This is wrong, check from matlab script

\begin{equation}
\bm{b}_1 = \begin{bmatrix}  \sqrt{2}/\sqrt{3} \\  0          \\ 1/\sqrt{3} \end{bmatrix} \quad
\bm{b}_2 = \begin{bmatrix} -1/\sqrt{6}        \\ -1/\sqrt{2} \\ 1/\sqrt{3} \end{bmatrix} \quad
\bm{b}_3 = \begin{bmatrix} -1/\sqrt{6}        \\  1/\sqrt{2} \\ 1/\sqrt{3} \end{bmatrix} \quad
\bm{b}_4 = \begin{bmatrix}  \sqrt{2}/\sqrt{3} \\  0          \\ 1/\sqrt{3} \end{bmatrix} \quad
\bm{b}_5 = \begin{bmatrix} -1/\sqrt{6}        \\ -1/\sqrt{2} \\ 1/\sqrt{3} \end{bmatrix} \quad
\bm{b}_6 = \begin{bmatrix} -1/\sqrt{6}        \\  1/\sqrt{2} \\ 1/\sqrt{3} \end{bmatrix}
\end{equation}


\begin{equation}
  \bm{K} = k \bm{J}^{\intercal} \bm{J} =
  k \left[
  \begin{array}{c|c}
      \Sigma_{i = 0}^{6} \hat{\bm{s}}_i \cdot \hat{\bm{s}}_i^{\intercal} & \Sigma_{i = 0}^{6} \bm{\hat{s}}_i \cdot ({}^A\bm{b}_i \times {}^A\hat{\bm{s}}_i)^{\intercal} \\
      \hline
      \Sigma_{i = 0}^{6} ({}^A\bm{b}_i \times {}^A\hat{\bm{s}}_i) \cdot \hat{\bm{s}}_i^{\intercal} & \Sigma_{i = 0}^{6} ({}^A\bm{b}_i \times {}^A\hat{\bm{s}}_i) \cdot ({}^A\bm{b}_i \times {}^A\hat{\bm{s}}_i)^{\intercal}\\
  \end{array}
  \right]
\end{equation}

#+begin_src matlab :exports none :results none
%% Cubic Architecture - Effect of the position of frame {A} and {B}
H = 20e-3;     % Height of the Stewart platform [m]
Hc = 100e-3;   % Size of the useful part of the cube [m]
FOc = 50e-3; % Center of the cube at the Stewart platform center

%% Frames {A} and {B} at the cube's center
MO_B = 0e-3;  % Position {B} with respect to {M} [m]
MHb = 10e-3;

k = 1.4;

stewart_center = initializeStewartPlatform();
stewart_center = initializeFramesPositions(stewart_center, 'H', H, 'MO_B', MO_B);
stewart_center = generateCubicConfiguration(stewart_center, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', MHb);
stewart_center = computeJointsPose(stewart_center);
stewart_center = initializeStrutDynamics(stewart_center, 'k', k);
stewart_center = computeJacobian(stewart_center);
stewart_center = initializeCylindricalPlatforms(stewart_center, 'Fpr', 150e-3, 'Mpr', 150e-3);

displayArchitecture(stewart_center, 'labels', true, 'frames', true);
plotCube(stewart_center, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,1], 'link_to_struts', true);

%% Si
si = 1/sqrt(3)*[
  [ sqrt(2);    0;       ; 1], ...
  [-sqrt(2)/2; -sqrt(3/2); 1], ...
  [-sqrt(2)/2;  sqrt(3/2); 1], ...
  [ sqrt(2);    0;       ; 1], ...
  [-sqrt(2)/2; -sqrt(3/2); 1], ...
  [-sqrt(2)/2;  sqrt(3/2); 1]  ...
  ];

% Verification
if norm(si - stewart_center.geometry.Bs) > 1e-9
    warning('wrong formulas')
end

%% Bi with Hc the "cube height"
% Position of the cube's top points for the six struts (centered cube)
ci = Hc * [
  [1/sqrt(2); -sqrt(3)/sqrt(2); 0.5], ...
  [1/sqrt(2); -sqrt(3)/sqrt(2); 0.5], ...
  [1/sqrt(2);  sqrt(3)/sqrt(2); 0.5], ...
  [1/sqrt(2);  sqrt(3)/sqrt(2); 0.5], ...
  [-sqrt(2);   0;               0.5], ...
  [-sqrt(2);   0;               0.5]  ...
];

% Position of ci with respect to {B}
ci = ci + (- H - MO_B + FOc) * [0;0;1];

% Position of bi with respect to {B}
bi = ci - si * (MHb + FOc + Hc/2 - H) ./ si(3,:);

% Compare from Bi computed from Script
if norm(bi - stewart_center.geometry.Bb) > 1e-9
    warning('wrong formulas')
end

%% Stiffness matrix
K = zeros(6,6);
K(1:3, 4:6) = si * cross(bi,si).';
K(4:6, 1:3) = cross(bi,si) * si.';
K(1:3,1:3) = si * si';
K(4:6,4:6) = cross(bi,si) * cross(bi,si).';
K = k*K;
if norm(K - stewart_center.kinematics.K) > 1e-9
    warning('wrong formulas')
end
#+end_src

*** Analytical equations

A and B at cube's center.
Position of bi can be anywhere along the cube's edges

#+begin_src matlab
% Define symbolic variables
syms k Hc alpha

% Define si matrix (support vectors)
si = 1/sqrt(3)*[
  [ sqrt(2),    0,        1]; ...
  [-sqrt(2)/2, -sqrt(3/2), 1]; ...
  [-sqrt(2)/2,  sqrt(3/2), 1]; ...
  [ sqrt(2),    0,        1]; ...
  [-sqrt(2)/2, -sqrt(3/2), 1]; ...
  [-sqrt(2)/2,  sqrt(3/2), 1]  ...
];

% Define ci matrix (connection vectors)
ci = Hc * [
  [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2),  sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2),  sqrt(3)/sqrt(2), 0.5]; ...
  [-sqrt(2),   0,               0.5]; ...
  [-sqrt(2),   0,               0.5]  ...
];

% Calculate bi vectors
bi = ci + alpha * si;

% Initialize stiffness matrix
K = sym(zeros(6,6));

% Calculate elements of the stiffness matrix
for i = 1:6
    % Extract vectors for each leg
    s_i = si(i,:)';
    b_i = bi(i,:)';

    % Calculate cross product vector
    cross_bs = cross(b_i, s_i);

    % Build matrix blocks
    K(1:3, 4:6) = K(1:3, 4:6) + s_i * cross_bs';
    K(4:6, 1:3) = K(4:6, 1:3) + cross_bs * s_i';
    K(1:3, 1:3) = K(1:3, 1:3) + s_i * s_i';
    K(4:6, 4:6) = K(4:6, 4:6) + cross_bs * cross_bs';
end

% Scale by stiffness coefficient
K = k * K;

% Simplify the expressions
K = simplify(K);

% Display the analytical stiffness matrix
disp('Analytical Stiffness Matrix:');
pretty(K);

% If you want to analyze specific components
disp('Top-left block (K₁₁):');
pretty(K(1:3, 1:3));

disp('Top-right block (K₁₂):');
pretty(K(1:3, 4:6));

disp('Bottom-left block (K₂₁):');
pretty(K(4:6, 1:3));

disp('Bottom-right block (K₂₂):');
pretty(K(4:6, 4:6));

% Eigenvalue analysis (optional)
disp('Eigenvalues:');
eig_vals = simplify(eig(K));
pretty(eig_vals);
#+end_src

*** Effect of un-centered frame with cube's center
#+begin_src matlab
% Define symbolic variables
syms k Hc alpha H

assume(k > 0); % k is positive real
assume(Hc, 'real'); % Hc is real
assume(H, 'real'); % H is real
assume(alpha, 'real'); % alpha is real


% Define si matrix (support vectors)
si = 1/sqrt(3)*[
  [ sqrt(2),    0,        1]; ...
  [-sqrt(2)/2, -sqrt(3/2), 1]; ...
  [-sqrt(2)/2,  sqrt(3/2), 1]; ...
  [ sqrt(2),    0,        1]; ...
  [-sqrt(2)/2, -sqrt(3/2), 1]; ...
  [-sqrt(2)/2,  sqrt(3/2), 1]  ...
];

% Define ci matrix (connection vectors)
ci = Hc * [
  [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2),  sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2),  sqrt(3)/sqrt(2), 0.5]; ...
  [-sqrt(2),   0,               0.5]; ...
  [-sqrt(2),   0,               0.5]  ...
];

% Apply vertical shift to ci
ci = ci + H * [0, 0, 1];

% Calculate bi vectors
bi = ci + alpha * si;

% Initialize stiffness matrix
K = sym(zeros(6,6));

% Calculate elements of the stiffness matrix
for i = 1:6
    % Extract vectors for each leg
    s_i = si(i,:)';
    b_i = bi(i,:)';

    % Calculate cross product vector
    cross_bs = cross(b_i, s_i);

    % Build matrix blocks
    K(1:3, 4:6) = K(1:3, 4:6) + s_i * cross_bs';
    K(4:6, 1:3) = K(4:6, 1:3) + cross_bs * s_i';
    K(1:3, 1:3) = K(1:3, 1:3) + s_i * s_i';
    K(4:6, 4:6) = K(4:6, 4:6) + cross_bs * cross_bs';
end

% Scale by stiffness coefficient
K = k * K;

% Simplify the expressions
K = simplify(K);

% Display the analytical stiffness matrix
disp('Analytical Stiffness Matrix:');
pretty(K);
#+end_src

*** Centered cube - joints at cube's vertices => diagonal stiffness matrix

#+begin_src matlab
%% Suppose centered cube : Hc = H, FOc = H/2, MO_B = -H/2
k = 1;
Hc = 200e-3

si = 1/sqrt(3)*[
  [ sqrt(2);    0;       ; 1], ...
  [-sqrt(2)/2; -sqrt(3/2); 1], ...
  [-sqrt(2)/2;  sqrt(3/2); 1], ...
  [ sqrt(2);    0;       ; 1], ...
  [-sqrt(2)/2; -sqrt(3/2); 1], ...
  [-sqrt(2)/2;  sqrt(3/2); 1]  ...
  ];

bi = Hc * [
  [1/sqrt(2); -sqrt(3)/sqrt(2); 0.5], ...
  [1/sqrt(2); -sqrt(3)/sqrt(2); 0.5], ...
  [1/sqrt(2);  sqrt(3)/sqrt(2); 0.5], ...
  [1/sqrt(2);  sqrt(3)/sqrt(2); 0.5], ...
  [-sqrt(2);   0;               0.5], ...
  [-sqrt(2);   0;               0.5]  ...
];

K = zeros(6,6);
K(1:3, 4:6) = si * cross(bi,si).';
K(4:6, 1:3) = cross(bi,si) * si.';
K(1:3,1:3) = si * si';
K(4:6,4:6) = cross(bi,si) * cross(bi,si).';
K = k*K

K_formula = diag([2*k, 2*k, 2*k, 1.5*k*Hc^2, 1.5*k*Hc^2, 6*k*Hc^2])
#+end_src

*** Verify general structure of the Stewart platform

#+begin_src matlab
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', 100e-3, 'MO_B', -50e-3);
stewart = generateGeneralConfiguration(stewart, 'FH', 0, 'FR', 100e-3, 'MH', 0, 'MR', 100e-3, ...
                                      'FTh', [-5, 5, 120-5, 120+5, 240-5, 240+5]*(pi/180), ...
                                      'MTh', [-60+5, 60-5, 60+5, 180-5, 180+5, -60-5]*(pi/180));
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1);
stewart = computeJacobian(stewart);
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 110e-3, 'Mpr', 110e-3);

displayArchitecture(stewart, 'labels', false, 'frames', false, 'F_color', [0,0,0], 'M_color', [0,0,0], 'L_color', [0,0,0]);
#+end_src

** DONE [#A] Make table for review of Stewart platforms
CLOSED: [2025-03-19 Wed 18:25]

[[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/bibliography.org::*Built Stewart PLatforms][Built Stewart PLatforms]]

Link to figures.

In figure legend: link to references, mention the university and the application.

** DONE [#C] Create a function to plot the mobility of the Stewart platform
CLOSED: [2025-03-30 Sun 18:31]

Arguments:
- choose to fix the orientation with ${}^{B}R_{A}$
- maximum stroke of each actuator (may be included in the Stewart object)

* Introduction                                                        :ignore:

The performance of a Stewart platform depends on its geometric configuration, especially the orientation of its struts and the positioning of its joints.
During the conceptual design phase of the nano-hexapod, a preliminary geometry was selected based on general principles without detailed optimization.
As the project advanced to the detailed design phase, a rigorous analysis of how geometry influences system performance became essential to ensure that the final design would meet the demanding requirements of the Nano Active Stabilization System (NASS).

In this chapter, the nano-hexapod geometry is optimized through careful analysis of how design parameters influence critical performance aspects: attainable workspace, mechanical stiffness, strut-to-strut coupling for decentralized control strategies, and dynamic response in Cartesian coordinates.

The chapter begins with a comprehensive review of existing Stewart platform designs in Section ref:sec:detail_kinematics_stewart_review, surveying various approaches to geometry, actuation, sensing, and joint design from the literature.
Section ref:sec:detail_kinematics_geometry develops the analytical framework that connects geometric parameters to performance characteristics, establishing quantitative relationships that guide the optimization process.
Section ref:sec:detail_kinematics_cubic examines the cubic configuration, a specific architecture that has gathered significant attention, to evaluate its suitability for the nano-hexapod application.
Finally, Section ref:sec:detail_kinematics_nano_hexapod presents the optimized nano-hexapod geometry derived from these analyses and demonstrates how it addresses the specific requirements of the NASS.

* Review of Stewart platforms
<<sec:detail_kinematics_stewart_review>>

The first parallel platform similar to the Stewart platform was built in 1954 by Gough [[cite:&gough62_univer_tyre_test_machin]], for a tyre test machine (shown in Figure ref:fig:detail_geometry_gough_paper).
Subsequently, Stewart proposed a similar design for a flight simulator (shown in Figure ref:fig:detail_geometry_stewart_flight_simulator) in a 1965 publication [[cite:&stewart65_platf_with_six_degrees_freed]].
Since then, the Stewart platform (sometimes referred to as the Stewart-Gough platform) has been utilized across diverse applications [[cite:&dasgupta00_stewar_platf_manip]], including large telescopes [[cite:&kazezkhan14_dynam_model_stewar_platf_nansh_radio_teles;&yun19_devel_isotr_stewar_platf_teles_secon_mirror]], machine tools [[cite:&russo24_review_paral_kinem_machin_tools]], and Synchrotron instrumentation [[cite:&marion04_hexap_esrf;&villar18_nanop_esrf_id16a_nano_imagin_beaml]].

#+name: fig:detail_geometry_stewart_origins
#+caption: Two of the earliest developments of Stewart platforms
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:detail_geometry_gough_paper}Tyre test machine proposed by Gough \cite{gough62_univer_tyre_test_machin}}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :height 5.5cm
[[file:figs/detail_geometry_gough_paper.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_geometry_stewart_flight_simulator}Flight simulator proposed by Stewart \cite{stewart65_platf_with_six_degrees_freed}}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :height 5.5cm
[[file:figs/detail_geometry_stewart_flight_simulator.jpg]]
#+end_subfigure
#+end_figure

# TODO - Section ref:sec:nhexa_stewart_platform
As explained in the conceptual phase, Stewart platforms comprise the following key elements: two plates connected by six struts, with each strut composed of a joint at each end, an actuator, and one or several sensors.

# TODO - ref:sec:detail_fem_joint
The specific geometry (i.e., position of joints and orientation of the struts) can be selected based on the application requirements, resulting in numerous designs throughout the literature.
This discussion focuses primarily on Stewart platforms designed for nano-positioning and vibration control, which necessitates the use of flexible joints.
The implementation of these flexible joints, will be discussed when designing the nano-hexapod flexible joints.
Long stroke Stewart platforms are not addressed here as their design presents different challenges, such as singularity-free workspace and complex kinematics [[cite:&merlet06_paral_robot]].

In terms of actuation, mainly two types are used: voice coil actuators and piezoelectric actuators.
Voice coil actuators, providing stroke ranges from $0.5\,mm$ to $10\,mm$, are commonly implemented in cubic architectures (as illustrated in Figures ref:fig:detail_kinematics_jpl, ref:fig:detail_kinematics_uw_gsp and ref:fig:detail_kinematics_pph) and are mainly used for vibration isolation [[cite:&spanos95_soft_activ_vibrat_isolat;&rahman98_multiax;&thayer98_stewar;&mcinroy99_dynam;&preumont07_six_axis_singl_stage_activ]].
For applications requiring short stroke (typically smaller than $500\,\mu m$), piezoelectric actuators present an interesting alternative, as shown in [[cite:&agrawal04_algor_activ_vibrat_isolat_spacec;&furutani04_nanom_cuttin_machin_using_stewar;&yang19_dynam_model_decoup_contr_flexib]].
Examples of piezoelectric-actuated Stewart platforms are presented in Figures ref:fig:detail_kinematics_ulb_pz, ref:fig:detail_kinematics_uqp and ref:fig:detail_kinematics_yang19.
Although less frequently encountered, magnetostrictive actuators have been successfully implemented in [[cite:&zhang11_six_dof]] (Figure ref:fig:detail_kinematics_zhang11).

#+name: fig:detail_kinematics_stewart_examples_cubic
#+caption: Some examples of developped Stewart platform with Cubic geometry
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_jpl}California Institute of Technology - USA \cite{spanos95_soft_activ_vibrat_isolat}}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_jpl.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_uw_gsp}University of Wyoming - USA \cite{mcinroy99_dynam}}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_uw_gsp.jpg]]
#+end_subfigure

\bigskip
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_ulb_pz}ULB - Belgium \cite{abu02_stiff_soft_stewar_platf_activ}}
#+attr_latex: :options {0.53\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_ulb_pz.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_uqp}Naval Postgraduate School - USA \cite{agrawal04_algor_activ_vibrat_isolat_spacec}}
#+attr_latex: :options {0.43\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_uqp.jpg]]
#+end_subfigure
#+end_figure

The sensors integrated in these platforms are selected based on specific control requirements, as different sensors offer distinct advantages and limitations [[cite:&hauge04_sensor_contr_space_based_six]].
Force sensors are typically integrated within the struts in a collocated arrangement with actuators to enhance control robustness.
Stewart platforms incorporating force sensors are frequently utilized for vibration isolation [[cite:&spanos95_soft_activ_vibrat_isolat;&rahman98_multiax]] and active damping applications [[cite:&geng95_intel_contr_system_multip_degree;&abu02_stiff_soft_stewar_platf_activ]], as exemplified in Figure ref:fig:detail_kinematics_ulb_pz.

Inertial sensors (accelerometers and geophones) are commonly employed in vibration isolation applications [[cite:&chen03_payload_point_activ_vibrat_isolat;&chi15_desig_exper_study_vcm_based]].
These sensors are predominantly aligned with the struts [[cite:&hauge04_sensor_contr_space_based_six;&li01_simul_fault_vibrat_isolat_point;&thayer02_six_axis_vibrat_isolat_system;&zhang11_six_dof;&jiao18_dynam_model_exper_analy_stewar;&tang18_decen_vibrat_contr_voice_coil]], although they may also be fixed to the top platform [[cite:&wang16_inves_activ_vibrat_isolat_stewar]].

For high-precision positioning applications, various displacement sensors are implemented, including LVDTs [[cite:&thayer02_six_axis_vibrat_isolat_system;&kim00_robus_track_contr_desig_dof_paral_manip;&li01_simul_fault_vibrat_isolat_point;&thayer98_stewar]], capacitive sensors [[cite:&ting07_measur_calib_stewar_microm_system;&ting13_compos_contr_desig_stewar_nanos_platf]], eddy current sensors [[cite:&chen03_payload_point_activ_vibrat_isolat;&furutani04_nanom_cuttin_machin_using_stewar]], and strain gauges [[cite:&du14_piezo_actuat_high_precis_flexib]].
Notably, some designs incorporate external sensing methodologies rather than integrating sensors within the struts [[cite:&li01_simul_fault_vibrat_isolat_point;&chen03_payload_point_activ_vibrat_isolat;&ting13_compos_contr_desig_stewar_nanos_platf]].
A recent design [[cite:&naves20_desig]], although not strictly speaking a Stewart platform, has demonstrated the use of 3-phase rotary motors with rotary encoders for achieving long-stroke and highly repeatable positioning, as illustrated in Figure ref:fig:detail_kinematics_naves.

Two primary categories of Stewart platform geometry can be identified.
The first is cubic architecture (examples presented in Figure ref:fig:detail_kinematics_stewart_examples_cubic), wherein struts are positioned along six sides of a cube (and therefore oriented orthogonally to each other).
This architecture represents the most prevalent configuration for vibration isolation applications in the literature.
Its distinctive properties will be examined in Section ref:sec:detail_kinematics_cubic.
The second category comprises non-cubic architectures (Figure ref:fig:detail_kinematics_stewart_examples_non_cubic), where strut orientation and joint positioning can be optimized according to defined performance criteria.
The influence of strut orientation and joint positioning on Stewart platform properties is analyzed in Section ref:sec:detail_kinematics_geometry.

#+name: fig:detail_kinematics_stewart_examples_non_cubic
#+caption: Some examples of developped Stewart platform with non-cubic geometry
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_pph}Naval Postgraduate School - USA \cite{chen03_payload_point_activ_vibrat_isolat}}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :height 5cm
[[file:figs/detail_kinematics_pph.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_zhang11}Beihang University - China \cite{zhang11_six_dof}}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :height 5cm
[[file:figs/detail_kinematics_zhang11.jpg]]
#+end_subfigure

\bigskip
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_yang19}Nanjing University - China \cite{yang19_dynam_model_decoup_contr_flexib}}
#+attr_latex: :options {0.43\textwidth}
#+begin_subfigure
#+attr_latex: :height 5cm
[[file:figs/detail_kinematics_yang19.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_naves}University of Twente - Netherlands \cite{naves20_desig}}
#+attr_latex: :options {0.53\textwidth}
#+begin_subfigure
#+attr_latex: :height 5cm
[[file:figs/detail_kinematics_naves.jpg]]
#+end_subfigure
#+end_figure

* Effect of geometry on Stewart platform properties
:PROPERTIES:
:HEADER-ARGS:matlab+: :tangle matlab/detail_kinematics_1_geometry.m
:END:
<<sec:detail_kinematics_geometry>>
** Introduction                                                      :ignore:

# TODO - Section ref:sec:nhexa_stewart_platform (page pageref:sec:nhexa_stewart_platform),
As was demonstrated during the conceptual phase, the geometry of the Stewart platform impacts the stiffness and compliance characteristics, the mobility (or workspace), the force authority, and the dynamics of the manipulator.
It is therefore essential to understand how the geometry impacts these properties, and to develop methodologies for optimizing the geometry for specific applications.

A useful analytical tool for this study is the Jacobian matrix, which depends on $\bm{b}_i$ (joints' position with respect to the top platform) and $\hat{\bm{s}}_i$ (struts' orientation).
The choice of $\{A\}$ and $\{B\}$ frames, independently of the physical Stewart platform geometry, impacts the obtained kinematics and stiffness matrix, as these are defined for forces and motion evaluated at the chosen frame.

** Matlab Init                                              :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src

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

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

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

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

** Platform Mobility / Workspace
<<ssec:detail_kinematics_geometry_mobility>>
**** Introduction                                                  :ignore:

The mobility of the Stewart platform (or any manipulator) is defined as the range of motion that it can perform.
It corresponds to the set of possible poses (i.e., combined translation and rotation) of frame $\{B\}$ with respect to frame $\{A\}$.
This represents a six-dimensional property which is difficult to represent.
Depending on the applications, only the translation mobility (i.e., fixed orientation workspace) or the rotation mobility may be represented.
This approach is equivalent to projecting the six-dimensional value into a three-dimensional space, which is easier to represent.

Mobility of parallel manipulators is inherently difficult to study as the translational and orientation workspace are coupled [[cite:&merlet02_still]].
The analysis is significantly simplified when considering small motions, as the Jacobian matrix can be used to link the strut motion to the motion of frame $\{B\}$ with respect to $\{A\}$ through eqref:eq:detail_kinematics_jacobian, which is a linear equation.

\begin{equation}\label{eq:detail_kinematics_jacobian}
  \begin{bmatrix} \delta l_1 \\ \delta l_2 \\ \delta l_3 \\ \delta l_4 \\ \delta l_5 \\ \delta l_6 \end{bmatrix} = \underbrace{\begin{bmatrix}
    {{}^A\hat{\bm{s}}_1}^{\intercal} & ({}^A\bm{b}_1 \times {}^A\hat{\bm{s}}_1)^{\intercal} \\
    {{}^A\hat{\bm{s}}_2}^{\intercal} & ({}^A\bm{b}_2 \times {}^A\hat{\bm{s}}_2)^{\intercal} \\
    {{}^A\hat{\bm{s}}_3}^{\intercal} & ({}^A\bm{b}_3 \times {}^A\hat{\bm{s}}_3)^{\intercal} \\
    {{}^A\hat{\bm{s}}_4}^{\intercal} & ({}^A\bm{b}_4 \times {}^A\hat{\bm{s}}_4)^{\intercal} \\
    {{}^A\hat{\bm{s}}_5}^{\intercal} & ({}^A\bm{b}_5 \times {}^A\hat{\bm{s}}_5)^{\intercal} \\
    {{}^A\hat{\bm{s}}_6}^{\intercal} & ({}^A\bm{b}_6 \times {}^A\hat{\bm{s}}_6)^{\intercal}
  \end{bmatrix}}_{\bm{J}} \begin{bmatrix} \delta x \\ \delta y \\ \delta z \\ \delta \theta_x \\ \delta \theta_y \\ \delta \theta_z \end{bmatrix}
\end{equation}

Therefore, the mobility of the Stewart platform (defined as the set of achievable $[\delta x\ \delta y\ \delta z\ \delta \theta_x\ \delta \theta_y\ \delta \theta_z]$) depends on two key factors: the stroke of each strut and the geometry of the Stewart platform (embodied in the Jacobian matrix).
More specifically, the XYZ mobility only depends on the $\hat{\bm{s}}_i$ (orientation of struts), while the mobility in rotation also depends on $\bm{b}_i$ (position of top joints).

**** Mobility in translation

For simplicity, only translations are first considered (i.e., the Stewart platform is considered to have fixed orientation).
In the general case, the translational mobility can be represented by a 3D shape having 12 faces, where each actuator limits the stroke along its axis in positive and negative directions.
The faces are therefore perpendicular to the strut direction.
The obtained mobility for the Stewart platform geometry shown in Figure ref:fig:detail_kinematics_mobility_trans_arch is computed and represented in Figure ref:fig:detail_kinematics_mobility_trans_result.

#+begin_src matlab :exports none :results none
%% Example of one Stewart platform and associated translational mobility
L_max =  50e-6; % Maximum actuator stroke (+/-) [m]

stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', 100e-3, 'MO_B', -50e-3);
stewart = generateGeneralConfiguration(stewart, 'FH', 0, 'FR', 100e-3, 'MH', 0, 'MR', 100e-3, ...
                                      'FTh', [-5, 5, 120-5, 120+5, 240-5, 240+5]*(pi/180), ...
                                      'MTh', [-60+5, 60-5, 60+5, 180-5, 180+5, -60-5]*(pi/180));
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1);
stewart = computeJacobian(stewart);
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 110e-3, 'Mpr', 110e-3);

displayArchitecture(stewart, 'labels', false, 'frames', false, 'F_color', [0,0,0], 'M_color', [0,0,0], 'L_color', [0,0,0]);
#+end_src

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

#+begin_src matlab
thetas = linspace(0, pi, 100);
phis = linspace(0, 2*pi, 100);
rs = zeros(length(thetas), length(phis));

for i = 1:length(thetas)
    for j = 1:length(phis)
        Tx = sin(thetas(i))*cos(phis(j));
        Ty = sin(thetas(i))*sin(phis(j));
        Tz = cos(thetas(i));

        dL = stewart.kinematics.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction

        rs(i, j) = L_max/max(abs(dL));
    end
end

% Sphere with radius equal to actuator stroke
[Xs,Ys,Zs] = sphere;
Xs = 1e6*L_max*Xs;
Ys = 1e6*L_max*Ys;
Zs = 1e6*L_max*Zs;

[phi_grid, theta_grid] = meshgrid(phis, thetas);
X = 1e6 * rs .* sin(theta_grid) .* cos(phi_grid);
Y = 1e6 * rs .* sin(theta_grid) .* sin(phi_grid);
Z = 1e6 * rs .* cos(theta_grid);

figure('Renderer', 'painters');
hold on;
surf(X,  Y,  Z,  'FaceColor', 'none', 'LineWidth', 0.1, 'EdgeColor', [0, 0, 0]);
surf(Xs, Ys, Zs, 'FaceColor', colors(2,:), 'EdgeColor', 'none');
quiver3(0, 0, 0, 100, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.9);
quiver3(0, 0, 0, 0, 100, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.9);
quiver3(0, 0, 0, 0, 0, 100, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.9);
hold off;
axis equal;
grid off;
axis off;
view(55, 25);
#+end_src

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

#+name: fig:detail_kinematics_mobility_trans
#+caption: One Stewart platform geometry (\subref{fig:detail_kinematics_mobility_trans_arch}) and its associated translational mobility (\subref{fig:detail_kinematics_mobility_trans_result}). A sphere with radius equal to the strut stroke is contained in the translational mobility shape.
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_mobility_trans_arch}Stewart platform geometry}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/detail_kinematics_mobility_trans_arch.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_mobility_trans_result}Translational mobility}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/detail_kinematics_mobility_trans_result.png]]
#+end_subfigure
#+end_figure

With the previous interpretations of the 12 faces making the translational mobility 3D shape, it can be concluded that for a strut stroke of $\pm d$, a sphere with radius $d$ is contained in the 3D shape and touches it in directions defined by the strut axes, as illustrated in Figure ref:fig:detail_kinematics_mobility_trans_result.
This means that the mobile platform can be translated in any direction with a stroke equal to the strut stroke.

To better understand how the geometry of the Stewart platform impacts the translational mobility, two configurations are compared with struts oriented vertically (Figure ref:fig:detail_kinematics_stewart_mobility_vert_struts) and struts oriented horizontally (Figure ref:fig:detail_kinematics_stewart_mobility_hori_struts).
The vertically oriented struts configuration leads to greater stroke in the horizontal direction and reduced stroke in the vertical direction (Figure ref:fig:detail_kinematics_mobility_translation_strut_orientation).
Conversely, horizontal oriented struts configuration provides more stroke in the vertical direction.

It may seem counterintuitive that less stroke is available in the direction of the struts.
This phenomenon occurs because the struts form a lever mechanism that amplifies the motion.
The amplification factor increases when the struts have a high angle with the direction of motion and equals one (i.e. is minimal) when aligned with the direction of motion.

#+begin_src matlab :exports none :results none
%% Stewart platform with Vertically oriented struts
L_max =  50e-6; % Maximum actuator stroke (+/-) [m]

stewart_vert = initializeStewartPlatform();
stewart_vert = initializeFramesPositions(stewart_vert, 'H', 40e-3, 'MO_B', -20e-3);
stewart_vert = generateGeneralConfiguration(stewart_vert, 'FH', 0, 'FR', 100e-3, 'MH', 0, 'MR', 100e-3, ...
                                      'FTh', [-25, 25, 120-25, 120+25, 240-25, 240+25]*(pi/180), ...
                                      'MTh', [-60+25, 60-25, 60+25, 180-25, 180+25, -60-25]*(pi/180));
stewart_vert = computeJointsPose(stewart_vert);
stewart_vert = initializeStrutDynamics(stewart_vert, 'k', 1);
stewart_vert = computeJacobian(stewart_vert);
stewart_vert = initializeCylindricalPlatforms(stewart_vert, 'Fpr', 110e-3, 'Mpr', 110e-3);

displayArchitecture(stewart_vert, 'labels', false, 'frames', false, 'F_color', colors(1,:), 'M_color', colors(1,:), 'L_color', colors(1,:));
#+end_src

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

#+begin_src matlab :exports none :results none
%% Stewart platform with Horizontally oriented struts
stewart_hori = initializeStewartPlatform();
stewart_hori = initializeFramesPositions(stewart_hori, 'H', 40e-3, 'MO_B', -20e-3);
stewart_hori = generateGeneralConfiguration(stewart_hori, 'FH', 0, 'FR', 100e-3, 'MH', 0, 'MR', 100e-3, ...
                                      'FTh', [-5, 5, 120-5, 120+5, 240-5, 240+5]*(pi/180), ...
                                      'MTh', [-60+5, 60-5, 60+5, 180-5, 180+5, -60-5]*(pi/180));
stewart_hori = computeJointsPose(stewart_hori);
stewart_hori = initializeStrutDynamics(stewart_hori, 'k', 1);
stewart_hori = computeJacobian(stewart_hori);
stewart_hori = initializeCylindricalPlatforms(stewart_hori, 'Fpr', 110e-3, 'Mpr', 110e-3);

displayArchitecture(stewart_hori, 'labels', false, 'frames', false, 'F_color', colors(2,:), 'M_color', colors(2,:), 'L_color', colors(2,:));
#+end_src

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

#+begin_src matlab :exports none :results none
%% Translation mobility for two Stewart platform geometry
thetas = linspace(0, pi, 100);
phis = linspace(0, 2*pi, 100);
rs_vert = zeros(length(thetas), length(phis));
rs_hori = zeros(length(thetas), length(phis));

for i = 1:length(thetas)
    for j = 1:length(phis)
        Tx = sin(thetas(i))*cos(phis(j));
        Ty = sin(thetas(i))*sin(phis(j));
        Tz = cos(thetas(i));

        dL = stewart_vert.kinematics.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction
        rs_vert(i, j) = L_max/max(abs(dL));

        dL = stewart_hori.kinematics.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction
        rs_hori(i, j) = L_max/max(abs(dL));
    end
end

[phi_grid, theta_grid] = meshgrid(phis, thetas);

X_vert = 1e6 * rs_vert .* sin(theta_grid) .* cos(phi_grid);
Y_vert = 1e6 * rs_vert .* sin(theta_grid) .* sin(phi_grid);
Z_vert = 1e6 * rs_vert .* cos(theta_grid);

X_hori = 1e6 * rs_hori .* sin(theta_grid) .* cos(phi_grid);
Y_hori = 1e6 * rs_hori .* sin(theta_grid) .* sin(phi_grid);
Z_hori = 1e6 * rs_hori .* cos(theta_grid);

[Xs,Ys,Zs] = sphere;
Xs = 1e6*L_max*Xs;
Ys = 1e6*L_max*Ys;
Zs = 1e6*L_max*Zs;

figure;
hold on;
surf(X_vert, Y_vert, Z_vert, 'FaceColor', 'white', 'LineWidth', 0.1, 'EdgeColor', colors(1,:));
surf(X_hori, Y_hori+400, Z_hori, 'FaceColor', 'white', 'LineWidth', 0.1, 'EdgeColor', colors(2,:));
quiver3(0, 0, 0, 200, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 0, 0, 0, 200, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 0, 0, 0, 0, 200, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
text(200, 0, 0, '$D_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top'   );
text(0, 200, 0, '$D_y$', 'FontSize', 10, 'HorizontalAlignment', 'right',  'VerticalAlignment', 'bottom');
text(0, 0, 200, '$D_z$', 'FontSize', 10, 'HorizontalAlignment', 'left',   'VerticalAlignment', 'top'   );
quiver3(0, 400, 0, 200, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 400, 0, 0, 200, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 400, 0, 0, 0, 200, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
text(200, 400, 0,   '$D_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top'   );
text(0,   600, 0,   '$D_y$', 'FontSize', 10, 'HorizontalAlignment', 'right',  'VerticalAlignment', 'bottom');
text(0,   400, 200, '$D_z$', 'FontSize', 10, 'HorizontalAlignment', 'left',   'VerticalAlignment', 'top'   );
hold off;
axis equal;
grid off;
axis off;
view(105, 15);
#+end_src

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

#+name: fig:detail_kinematics_stewart_mobility_translation_examples
#+caption: Effect of strut orientation on the obtained mobility in translation. Two Stewart platform geometry are considered: struts oriented vertically (\subref{fig:detail_kinematics_stewart_mobility_vert_struts}) and struts oriented vertically (\subref{fig:detail_kinematics_stewart_mobility_hori_struts}). Obtained mobility for both geometry are shown in (\subref{fig:detail_kinematics_mobility_translation_strut_orientation}).
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_stewart_mobility_vert_struts}Vertical struts}
#+attr_latex: :options {0.25\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_stewart_mobility_vert_struts.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_stewart_mobility_hori_struts}Horizontal struts}
#+attr_latex: :options {0.25\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_stewart_mobility_hori_struts.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_mobility_translation_strut_orientation}Translational mobility}
#+attr_latex: :options {0.46\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_mobility_translation_strut_orientation.png]]
#+end_subfigure
#+end_figure

**** Mobility in rotation

As shown by equation eqref:eq:detail_kinematics_jacobian, the rotational mobility depends both on the orientation of the struts and on the location of the top joints.
Similarly to the translational case, to increase the rotational mobility in one direction, it is advantageous to have the struts more perpendicular to the rotational direction.

For instance, having the struts more vertical (Figure ref:fig:detail_kinematics_stewart_mobility_vert_struts) provides less rotational stroke along the vertical direction than having the struts oriented more horizontally (Figure ref:fig:detail_kinematics_stewart_mobility_hori_struts).

Two cases are considered with the same strut orientation but with different top joint positions: struts positioned close to each other (Figure ref:fig:detail_kinematics_stewart_mobility_close_struts) and struts positioned further apart (Figure ref:fig:detail_kinematics_stewart_mobility_space_struts).
The mobility for pure rotations is compared in Figure ref:fig:detail_kinematics_mobility_angle_strut_distance.
Having struts further apart decreases the "lever arm" and therefore reduces the rotational mobility.

#+begin_src matlab :exports none :results none
%% Stewart platform with struts close to each other
stewart_close = initializeStewartPlatform();
stewart_close = initializeFramesPositions(stewart_close, 'H', 100e-3, 'MO_B', 0);
stewart_close = generateGeneralConfiguration(stewart_close, 'FH', 0, 'FR', 50e-3, 'MH', 0, 'MR', 50e-3, ...
                                      'FTh', [-6, 6, 120-6, 120+6, 240-6, 240+6]*(pi/180), ...
                                      'MTh', [-60+6, 60-6, 60+6, 180-6, 180+6, -60-6]*(pi/180));
stewart_close = computeJointsPose(stewart_close);
stewart_close = initializeStrutDynamics(stewart_close, 'k', 1);
stewart_close = computeJacobian(stewart_close);
stewart_close = initializeCylindricalPlatforms(stewart_close, 'Fpr', 110e-3, 'Mpr', 110e-3);

displayArchitecture(stewart_close, 'labels', false, 'frames', false, 'F_color', colors(1,:), 'M_color', colors(1,:), 'L_color', colors(1,:));
#+end_src

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

#+begin_src matlab :exports none :results none
%% Stewart platform with struts further away from each other
stewart_space = initializeStewartPlatform();
stewart_space = initializeFramesPositions(stewart_space, 'H', 100e-3, 'MO_B', 0);
stewart_space = generateGeneralConfiguration(stewart_space, 'FH', 0, 'FR', 100e-3, 'MH', 0, 'MR', 100e-3, ...
                                      'FTh', [-6, 6, 120-6, 120+6, 240-6, 240+6]*(pi/180), ...
                                      'MTh', [-60+6, 60-6, 60+6, 180-6, 180+6, -60-6]*(pi/180));
stewart_space.platform_F.Fa = stewart_space.platform_M.Mb - (stewart_close.platform_M.Mb - stewart_close.platform_F.Fa);
stewart_space = computeJointsPose(stewart_space);
stewart_space = initializeStrutDynamics(stewart_space, 'k', 1);
stewart_space = computeJacobian(stewart_space);
stewart_space = initializeCylindricalPlatforms(stewart_space, 'Fpr', 110e-3, 'Mpr', 110e-3);

displayArchitecture(stewart_space, 'labels', false, 'frames', false, 'F_color', colors(2,:), 'M_color', colors(2,:), 'L_color', colors(2,:));
#+end_src

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

#+begin_src matlab :exports none :results none
%% Translation mobility for two Stewart platform geometry
thetas = linspace(0, pi, 50);
phis = linspace(0, 2*pi, 50);
rs_close = zeros(length(thetas), length(phis));
rs_space = zeros(length(thetas), length(phis));

for i = 1:length(thetas)
    for j = 1:length(phis)
        Rx = sin(thetas(i))*cos(phis(j));
        Ry = sin(thetas(i))*sin(phis(j));
        Rz = cos(thetas(i));

        dL = stewart_close.kinematics.J*[0; 0; 0; Rx; Ry; Rz;];
        rs_close(i, j) = L_max/max(abs(dL));

        dL = stewart_space.kinematics.J*[0; 0; 0; Rx; Ry; Rz;];
        rs_space(i, j) = L_max/max(abs(dL));
    end
end

[phi_grid, theta_grid] = meshgrid(phis, thetas);

X_close = 1e6 * rs_close .* sin(theta_grid) .* cos(phi_grid);
Y_close = 1e6 * rs_close .* sin(theta_grid) .* sin(phi_grid);
Z_close = 1e6 * rs_close .* cos(theta_grid);

X_space = 1e6 * rs_space .* sin(theta_grid) .* cos(phi_grid);
Y_space = 1e6 * rs_space .* sin(theta_grid) .* sin(phi_grid);
Z_space = 1e6 * rs_space .* cos(theta_grid);

figure;
hold on;
surf(X_close, Y_close, Z_close, 'FaceColor', 'white', 'LineWidth', 0.1, 'EdgeColor', colors(1,:));
surf(X_space, Y_space+6000, Z_space, 'FaceColor', 'white', 'LineWidth', 0.1, 'EdgeColor', colors(2,:));
quiver3(0, 0, 0, 4000, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 0, 0, 0, 4000, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 0, 0, 0, 0, 4000, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
text(4000, 0, 0, '$R_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top'   );
text(0, 4000, 0, '$R_y$', 'FontSize', 10, 'HorizontalAlignment', 'right',  'VerticalAlignment', 'bottom');
text(0, 0, 4000, '$R_z$', 'FontSize', 10, 'HorizontalAlignment', 'left',   'VerticalAlignment', 'top'   );
quiver3(0, 6000, 0, 4000, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 6000, 0, 0, 4000, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 6000, 0, 0, 0, 4000, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
text(4000, 6000, 0,   '$R_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top');
text(0,    10000, 0,   '$R_y$', 'FontSize', 10, 'HorizontalAlignment', 'right', 'VerticalAlignment', 'bottom');
text(0,    6000, 4000, '$R_z$', 'FontSize', 10, 'HorizontalAlignment', 'left', 'VerticalAlignment', 'top');
hold off;
axis equal;
grid off;
axis off;
view(105, 15);
#+end_src

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

#+name: fig:detail_kinematics_stewart_mobility_rotation_examples
#+caption: Effect of strut position on the obtained mobility in rotation. Two Stewart platform geometry are considered: struts close to each other (\subref{fig:detail_kinematics_stewart_mobility_close_struts}) and struts further appart (\subref{fig:detail_kinematics_stewart_mobility_space_struts}). Obtained mobility for both geometry are shown in (\subref{fig:detail_kinematics_mobility_angle_strut_distance}).
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_stewart_mobility_close_struts}Struts close together}
#+attr_latex: :options {0.25\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_stewart_mobility_close_struts.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_stewart_mobility_space_struts}Struts far apart}
#+attr_latex: :options {0.25\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_stewart_mobility_space_struts.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_mobility_angle_strut_distance}Rotational mobility}
#+attr_latex: :options {0.46\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_mobility_angle_strut_distance.png]]
#+end_subfigure
#+end_figure

**** Combined translations and rotations

It is possible to consider combined translations and rotations, although displaying such mobility becomes more complex.
For a fixed geometry and a desired mobility (combined translations and rotations), it is possible to estimate the required minimum actuator stroke.
This analysis is conducted in Section ref:sec:detail_kinematics_nano_hexapod to estimate the required actuator stroke for the nano-hexapod geometry.

** Stiffness
<<ssec:detail_kinematics_geometry_stiffness>>
**** Introduction                                                  :ignore:

The stiffness matrix defines how the top platform of the Stewart platform (i.e. frame $\{B\}$) deforms with respect to its fixed base (i.e. frame $\{A\}$) due to static forces/torques applied between frames $\{A\}$ and $\{B\}$.
It depends on the Jacobian matrix (i.e., the geometry) and the strut axial stiffness as shown in equation eqref:eq:detail_kinematics_stiffness_matrix.
The contribution of joints stiffness is not considered here, as the joints were optimized after the geometry was fixed.
However, theoretical frameworks for evaluating flexible joint contribution to the stiffness matrix have been established in the literature  [[cite:&mcinroy00_desig_contr_flexur_joint_hexap;&mcinroy02_model_desig_flexur_joint_stewar]].

\begin{equation}\label{eq:detail_kinematics_stiffness_matrix}
  \bm{K} = \bm{J}^{\intercal} \bm{\mathcal{K}} \bm{J}
\end{equation}

It is assumed that the stiffness of all struts is the same: $\bm{\mathcal{K}} = k \cdot \mathbf{I}_6$.
In that case, the obtained stiffness matrix linearly depends on the strut stiffness $k$, and is structured as shown in equation eqref:eq:detail_kinematics_stiffness_matrix_simplified.

\begin{equation}\label{eq:detail_kinematics_stiffness_matrix_simplified}
  \bm{K} = k \bm{J}^{\intercal} \bm{J} =
  k \left[
  \begin{array}{c|c}
      \Sigma_{i = 0}^{6} \hat{\bm{s}}_i \cdot \hat{\bm{s}}_i^{\intercal} & \Sigma_{i = 0}^{6} \bm{\hat{s}}_i \cdot ({}^A\bm{b}_i \times {}^A\hat{\bm{s}}_i)^{\intercal} \\
      \hline
      \Sigma_{i = 0}^{6} ({}^A\bm{b}_i \times {}^A\hat{\bm{s}}_i) \cdot \hat{\bm{s}}_i^{\intercal} & \Sigma_{i = 0}^{6} ({}^A\bm{b}_i \times {}^A\hat{\bm{s}}_i) \cdot ({}^A\bm{b}_i \times {}^A\hat{\bm{s}}_i)^{\intercal}\\
  \end{array}
  \right]
\end{equation}

**** Translation Stiffness

As shown by equation eqref:eq:detail_kinematics_stiffness_matrix_simplified, the translation stiffnesses (the $3 \times 3$ top left terms of the stiffness matrix) only depend on the orientation of the struts and not their location: $\hat{\bm{s}}_i \cdot \hat{\bm{s}}_i^{\intercal}$.
In the extreme case where all struts are vertical ($s_i = [0\ 0\ 1]$), a vertical stiffness of $6k$ is achieved, but with null stiffness in the horizontal directions.
If two struts are aligned with the X axis, two struts with the Y axis, and two struts with the Z axis, then $\hat{\bm{s}}_i \cdot \hat{\bm{s}}_i^{\intercal} = 2 \bm{I}_3$, resulting in well-distributed stiffness along all directions.
This configuration corresponds to the cubic architecture presented in Section ref:sec:detail_kinematics_cubic.

When the struts are oriented more vertically, as shown in Figure ref:fig:detail_kinematics_stewart_mobility_vert_struts, the vertical stiffness increases while the horizontal stiffness decreases.
Additionally, $R_x$ and $R_y$ stiffness increases while $R_z$ stiffness decreases.
The opposite conclusions apply if struts are oriented more horizontally, illustrated in Figure ref:fig:detail_kinematics_stewart_mobility_hori_struts.

**** Rotational Stiffness

The rotational stiffnesses depend both on the orientation of the struts and on the location of the top joints with respect to the considered center of rotation (i.e., the location of frame $\{A\}$).
With the same orientation but increased distances to the frame $\{A\}$ by a factor of 2, the rotational stiffness is increased by a factor of 4.
Therefore, the compact Stewart platform depicted in Figure ref:fig:detail_kinematics_stewart_mobility_close_struts has less rotational stiffness than the Stewart platform shown in Figure ref:fig:detail_kinematics_stewart_mobility_space_struts.

**** Diagonal Stiffness Matrix

Having a diagonal stiffness matrix $\bm{K}$ can be beneficial for control purposes as it would make the plant in the Cartesian frame decoupled at low frequency.
This property depends on both the geometry and the chosen $\{A\}$ frame.
For specific geometry and choice of $\{A\}$ frame, it is possible to achieve a diagonal $K$ matrix.
This is discussed in Section ref:ssec:detail_kinematics_cubic_static.

** Dynamical properties
<<ssec:detail_kinematics_geometry_dynamics>>

The dynamical equations (both in the Cartesian frame and in the frame of the struts) for the Stewart platform were derived during the conceptual phase with simplifying assumptions (massless struts and perfect joints).
The dynamics depends both on the geometry (Jacobian matrix) and on the payload being placed on top of the platform.
# Section ref:ssec:nhexa_stewart_platform_dynamics (page pageref:ssec:nhexa_stewart_platform_dynamics).

Under very specific conditions, the equations of motion in the Cartesian frame, given by equation eqref:eq:detail_kinematics_transfer_function_cart, can be decoupled.
These conditions are studied in Section ref:ssec:detail_kinematics_cubic_dynamic.

\begin{equation}\label{eq:detail_kinematics_transfer_function_cart}
    \frac{{\mathcal{X}}}{\bm{\mathcal{F}}}(s) = ( \bm{M} s^2 + \bm{J}^{\intercal} \bm{\mathcal{C}} \bm{J} s + \bm{J}^{\intercal} \bm{\mathcal{K}} \bm{J} )^{-1}
\end{equation}

In the frame of the struts, the equations of motion eqref:eq:detail_kinematics_transfer_function_struts are well decoupled at low frequency.
This is why most Stewart platforms are controlled in the frame of the struts: below the resonance frequency, the system is well decoupled and SISO control may be applied for each strut, independently of the payload being used.

\begin{equation}\label{eq:detail_kinematics_transfer_function_struts}
    \frac{\bm{\mathcal{L}}}{\bm{f}}(s) = ( \bm{J}^{-T} \bm{M} \bm{J}^{-1} s^2 + \bm{\mathcal{C}} + \bm{\mathcal{K}} )^{-1}
\end{equation}

Coupling between sensors (force sensors, relative position sensors or inertial sensors) in different struts may also be important for decentralized control.
In section ref:ssec:detail_kinematics_decentralized_control, it will be studied whether the Stewart platform geometry can be optimized to have lower coupling between the struts.

** Conclusion
:PROPERTIES:
:UNNUMBERED: t
:END:

The effects of two changes in the manipulator's geometry, namely the position and orientation of the struts, are summarized in Table ref:tab:detail_kinematics_geometry.
These results could have been easily deduced based on mechanical principles, but thanks to the kinematic analysis, they can be quantified.
These trade-offs provide important guidelines when choosing the Stewart platform geometry.

#+name: tab:detail_kinematics_geometry
#+attr_latex: :environment tabularx :width 0.8\linewidth :align Xcc
#+attr_latex: :center t :booktabs t :float t :font \small
#+caption: Effect of a change in geometry on the manipulator's stiffness and mobility
| *Struts*                      | *Vertically Oriented* | *Increased separation* |
|-------------------------------+-----------------------+------------------------|
| Vertical stiffness            | $\nearrow$            | $=$                    |
| Horizontal stiffness          | $\searrow$            | $=$                    |
| Vertical rotation stiffness   | $\searrow$            | $\nearrow$             |
| Horizontal rotation stiffness | $\nearrow$            | $\nearrow$             |
|-------------------------------+-----------------------+------------------------|
| Vertical mobility             | $\searrow$            | $=$                    |
| Horizontal mobility           | $\nearrow$            | $=$                    |
| Vertical rotation mobility    | $\nearrow$            | $\searrow$             |
| Horizontal rotation mobility  | $\searrow$            | $\searrow$             |

* The Cubic Architecture
:PROPERTIES:
:HEADER-ARGS:matlab+: :tangle matlab/detail_kinematics_2_cubic.m
:END:
<<sec:detail_kinematics_cubic>>
** Matlab Init                                              :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src

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

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

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

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

** Introduction                                                      :ignore:

The Cubic configuration for the Stewart platform was first proposed by Dr. Gough in a comment to the original paper by Dr. Stewart [[cite:&stewart65_platf_with_six_degrees_freed]].
This configuration is characterized by active struts arranged in a mutually orthogonal configuration connecting the corners of a cube, as shown in Figure ref:fig:detail_kinematics_cubic_architecture_example.

Typically, the struts have similar length to the cube's edges, as illustrated in Figure ref:fig:detail_kinematics_cubic_architecture_example.
Practical implementations of such configurations can be observed in Figures ref:fig:detail_kinematics_jpl, ref:fig:detail_kinematics_uw_gsp and ref:fig:detail_kinematics_uqp.
It is also possible to implement designs with strut lengths smaller than the cube's edges (Figure ref:fig:detail_kinematics_cubic_architecture_example_small), as exemplified in Figure ref:fig:detail_kinematics_ulb_pz.

#+begin_src matlab :exports none :results none
%% Example of a typical "cubic" architecture
MO_B = -50e-3;  % Position {B} with respect to {M} [m]

H = 100e-3;     % Height of the Stewart platform [m]

Hc = 100e-3;   % Size of the useful part of the cube [m]

FOc = H + MO_B; % Center of the cube with respect to {F}
                % here positionned at the frame {B}

stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1);
stewart = initializeJointDynamics(stewart);
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 150e-3, 'Mpr', 150e-3);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);

displayArchitecture(stewart, 'labels', false, 'frames', false);
plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', false);
view([40, 5]);
#+end_src

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

#+begin_src matlab :exports none :results none
%% Example of a typical "cubic" architecture
MO_B = -20e-3;  % Position {B} with respect to {M} [m]

H = 40e-3;     % Height of the Stewart platform [m]

Hc = 100e-3;   % Size of the useful part of the cube [m]

FOc = H + MO_B; % Center of the cube with respect to {F}
                % here positionned at the frame {B}

stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1);
stewart = computeJacobian(stewart);
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 150e-3, 'Mpr', 150e-3);

displayArchitecture(stewart, 'labels', false, 'frames', false);
plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', false);
view([40, 5]);
#+end_src

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

#+name: fig:detail_kinematics_cubic_architecture_examples
#+caption: Typical Stewart platform cubic architectures in which struts' length is similar to the cube edges's length (\subref{fig:detail_kinematics_cubic_architecture_example}) or is taking just a portion of the edge (\subref{fig:detail_kinematics_cubic_architecture_example_small}).
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_architecture_example}Classical Cubic architecture}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/detail_kinematics_cubic_architecture_example.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_architecture_example_small}Alternative configuration}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/detail_kinematics_cubic_architecture_example_small.png]]
#+end_subfigure
#+end_figure


Several advantageous properties attributed to the cubic configuration have contributed to its widespread adoption [[cite:&geng94_six_degree_of_freed_activ;&preumont07_six_axis_singl_stage_activ;&jafari03_orthog_gough_stewar_platf_microm]]: simplified kinematics relationships and dynamical analysis [[cite:&geng94_six_degree_of_freed_activ]]; uniform stiffness in all directions [[cite:&hanieh03_activ_stewar]]; uniform mobility [[cite:&preumont18_vibrat_contr_activ_struc_fourt_edition, chapt.8.5.2]]; and minimization of the cross coupling between actuators and sensors in different struts [[cite:&preumont07_six_axis_singl_stage_activ]].
This minimization is attributed to the fact that the struts are orthogonal to each other, and is said to facilitate collocated sensor-actuator control system design, i.e., the implementation of decentralized control [[cite:&geng94_six_degree_of_freed_activ;&thayer02_six_axis_vibrat_isolat_system]].

These properties are examined in this section to assess their relevance for the nano-hexapod.
The mobility and stiffness properties of the cubic configuration are analyzed in Section ref:ssec:detail_kinematics_cubic_static.
Dynamical decoupling is investigated in Section ref:ssec:detail_kinematics_cubic_dynamic, while decentralized control, crucial for the NASS, is examined in Section ref:ssec:detail_kinematics_decentralized_control.
Given that the cubic architecture imposes strict geometric constraints, alternative designs are proposed in Section ref:ssec:detail_kinematics_cubic_design.
The ultimate objective is to determine the suitability of the cubic architecture for the nano-hexapod.

** Static Properties
<<ssec:detail_kinematics_cubic_static>>
**** Stiffness matrix for the Cubic architecture

Consider the cubic architecture shown in Figure ref:fig:detail_kinematics_cubic_schematic_full.
The unit vectors corresponding to the edges of the cube are described by equation eqref:eq:detail_kinematics_cubic_s.

\begin{equation}\label{eq:detail_kinematics_cubic_s}
  \hat{\bm{s}}_1 = \begin{bmatrix}  \frac{\sqrt{2}}{\sqrt{3}} \\  0                  \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad
  \hat{\bm{s}}_2 = \begin{bmatrix}  \frac{-1}{\sqrt{6}}       \\ \frac{-1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad
  \hat{\bm{s}}_3 = \begin{bmatrix}  \frac{-1}{\sqrt{6}}       \\ \frac{ 1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad
  \hat{\bm{s}}_4 = \begin{bmatrix}  \frac{\sqrt{2}}{\sqrt{3}} \\  0                  \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad
  \hat{\bm{s}}_5 = \begin{bmatrix}  \frac{-1}{\sqrt{6}}       \\ \frac{-1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad
  \hat{\bm{s}}_6 = \begin{bmatrix}  \frac{-1}{\sqrt{6}}       \\ \frac{ 1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix}
\end{equation}

#+begin_src latex :file detail_kinematics_cubic_schematic_full.pdf :results file
  \begin{tikzpicture}
    \begin{scope}[rotate={45}, shift={(0, 0, -4)}]
      % We first define the coordinate of the points of the Cube
      \coordinate[] (bot) at (0,0,4);
      \coordinate[] (top) at (4,4,0);
      \coordinate[] (A1) at (0,0,0);
      \coordinate[] (A2) at (4,0,4);
      \coordinate[] (A3) at (0,4,4);
      \coordinate[] (B1) at (4,0,0);
      \coordinate[] (B2) at (4,4,4);
      \coordinate[] (B3) at (0,4,0);

      % Center of the Cube
      \coordinate[] (cubecenter) at ($0.5*(bot) + 0.5*(top)$);

      % We draw parts of the cube that corresponds to the Stewart platform
      \draw[] (A1)node[]{$\bullet$} -- (B1)node[]{$\bullet$} -- (A2)node[]{$\bullet$} -- (B2)node[]{$\bullet$} -- (A3)node[]{$\bullet$} -- (B3)node[]{$\bullet$} -- (A1);

      % ai and bi are computed
      \def\lfrom{0.0}
      \def\lto{1.0}

      \coordinate(a1) at ($(A1) - \lfrom*(A1) + \lfrom*(B1)$);
      \coordinate(b1) at ($(A1) - \lto*(A1)   + \lto*(B1)$);
      \coordinate(a2) at ($(A2) - \lfrom*(A2) + \lfrom*(B1)$);
      \coordinate(b2) at ($(A2) - \lto*(A2)   + \lto*(B1)$);
      \coordinate(a3) at ($(A2) - \lfrom*(A2) + \lfrom*(B2)$);
      \coordinate(b3) at ($(A2) - \lto*(A2)   + \lto*(B2)$);
      \coordinate(a4) at ($(A3) - \lfrom*(A3) + \lfrom*(B2)$);
      \coordinate(b4) at ($(A3) - \lto*(A3)   + \lto*(B2)$);
      \coordinate(a5) at ($(A3) - \lfrom*(A3) + \lfrom*(B3)$);
      \coordinate(b5) at ($(A3) - \lto*(A3)   + \lto*(B3)$);
      \coordinate(a6) at ($(A1) - \lfrom*(A1) + \lfrom*(B3)$);
      \coordinate(b6) at ($(A1) - \lto*(A1)   + \lto*(B3)$);

      % We draw the fixed and mobiles platforms
      \path[fill=colorblue, opacity=0.2] (a1) -- (a2) -- (a3) -- (a4) -- (a5) -- (a6) -- cycle;
      \path[fill=colorblue, opacity=0.2] (b1) -- (b2) -- (b3) -- (b4) -- (b5) -- (b6) -- cycle;
      \draw[color=colorblue, dashed] (a1) -- (a2) -- (a3) -- (a4) -- (a5) -- (a6) -- cycle;
      \draw[color=colorblue, dashed] (b1) -- (b2) -- (b3) -- (b4) -- (b5) -- (b6) -- cycle;

      % The legs of the hexapod are drawn
      \draw[color=colorblue] (a1)node{$\bullet$} -- (b1)node{$\bullet$};
      \draw[color=colorblue] (a2)node{$\bullet$} -- (b2)node{$\bullet$};
      \draw[color=colorblue] (a3)node{$\bullet$} -- (b3)node{$\bullet$};
      \draw[color=colorblue] (a4)node{$\bullet$} -- (b4)node{$\bullet$};
      \draw[color=colorblue] (a5)node{$\bullet$} -- (b5)node{$\bullet$};
      \draw[color=colorblue] (a6)node{$\bullet$} -- (b6)node{$\bullet$};

      % Unit vector
      \draw[color=colorred, ->] ($0.9*(a1)+0.1*(b1)$)node{$\bullet$} -- ($0.65*(a1)+0.35*(b1)$)node[right]{$\hat{\bm{s}}_3$};
      \draw[color=colorred, ->] ($0.9*(a2)+0.1*(b2)$)node{$\bullet$} -- ($0.65*(a2)+0.35*(b2)$)node[left]{$\hat{\bm{s}}_4$};
      \draw[color=colorred, ->] ($0.9*(a3)+0.1*(b3)$)node{$\bullet$} -- ($0.65*(a3)+0.35*(b3)$)node[below]{$\hat{\bm{s}}_5$};
      \draw[color=colorred, ->] ($0.9*(a4)+0.1*(b4)$)node{$\bullet$} -- ($0.65*(a4)+0.35*(b4)$)node[below]{$\hat{\bm{s}}_6$};
      \draw[color=colorred, ->] ($0.9*(a5)+0.1*(b5)$)node{$\bullet$} -- ($0.65*(a5)+0.35*(b5)$)node[left]{$\hat{\bm{s}}_1$};
      \draw[color=colorred, ->] ($0.9*(a6)+0.1*(b6)$)node{$\bullet$} -- ($0.65*(a6)+0.35*(b6)$)node[right]{$\hat{\bm{s}}_2$};

      % Labels
      \node[above=0.1 of B1] {$\tilde{\bm{b}}_3 = \tilde{\bm{b}}_4$};
      \node[above=0.1 of B2] {$\tilde{\bm{b}}_5 = \tilde{\bm{b}}_6$};
      \node[above=0.1 of B3] {$\tilde{\bm{b}}_1 = \tilde{\bm{b}}_2$};
    \end{scope}

    % Height of the Hexapod
    \coordinate[] (sizepos) at ($(a2)+(0.2, 0)$);
    \coordinate[] (origin) at (0,0,0);

    \draw[->, color=colorgreen] (cubecenter.center) node[above right]{$\{B\}$} -- ++(0,0,1);
    \draw[->, color=colorgreen] (cubecenter.center) -- ++(1,0,0);
    \draw[->, color=colorgreen] (cubecenter.center) -- ++(0,1,0);

    \node[] at (cubecenter.center){$\bullet$};
    \node[above left] at (cubecenter.center){$\{C\}$};

    % Useful part of the cube
    \draw[<->, dashed] ($(A2)+(0.5,0)$) -- node[midway, right]{$H_{C}$} ($(B1)+(0.5,0)$);
  \end{tikzpicture}
#+end_src

#+begin_src latex :file detail_kinematics_cubic_schematic.pdf :results file
  \begin{tikzpicture}
    \begin{scope}[rotate={45}, shift={(0, 0, -4)}]
      % We first define the coordinate of the points of the Cube
      \coordinate[] (bot) at (0,0,4);
      \coordinate[] (top) at (4,4,0);
      \coordinate[] (A1) at (0,0,0);
      \coordinate[] (A2) at (4,0,4);
      \coordinate[] (A3) at (0,4,4);
      \coordinate[] (B1) at (4,0,0);
      \coordinate[] (B2) at (4,4,4);
      \coordinate[] (B3) at (0,4,0);

      % Center of the Cube
      \coordinate[] (cubecenter) at ($0.5*(bot) + 0.5*(top)$);

      % We draw parts of the cube that corresponds to the Stewart platform
      \draw[] (A1)node[]{$\bullet$} -- (B1)node[]{$\bullet$} -- (A2)node[]{$\bullet$} -- (B2)node[]{$\bullet$} -- (A3)node[]{$\bullet$} -- (B3)node[]{$\bullet$} -- (A1);

      % ai and bi are computed
      \def\lfrom{0.2}
      \def\lto{0.8}

      \coordinate(a1) at ($(A1) - \lfrom*(A1) + \lfrom*(B1)$);
      \coordinate(b1) at ($(A1) - \lto*(A1)   + \lto*(B1)$);
      \coordinate(a2) at ($(A2) - \lfrom*(A2) + \lfrom*(B1)$);
      \coordinate(b2) at ($(A2) - \lto*(A2)   + \lto*(B1)$);
      \coordinate(a3) at ($(A2) - \lfrom*(A2) + \lfrom*(B2)$);
      \coordinate(b3) at ($(A2) - \lto*(A2)   + \lto*(B2)$);
      \coordinate(a4) at ($(A3) - \lfrom*(A3) + \lfrom*(B2)$);
      \coordinate(b4) at ($(A3) - \lto*(A3)   + \lto*(B2)$);
      \coordinate(a5) at ($(A3) - \lfrom*(A3) + \lfrom*(B3)$);
      \coordinate(b5) at ($(A3) - \lto*(A3)   + \lto*(B3)$);
      \coordinate(a6) at ($(A1) - \lfrom*(A1) + \lfrom*(B3)$);
      \coordinate(b6) at ($(A1) - \lto*(A1)   + \lto*(B3)$);

      % We draw the fixed and mobiles platforms
      \path[fill=colorblue, opacity=0.2] (a1) -- (a2) -- (a3) -- (a4) -- (a5) -- (a6) -- cycle;
      \path[fill=colorblue, opacity=0.2] (b1) -- (b2) -- (b3) -- (b4) -- (b5) -- (b6) -- cycle;
      \draw[color=colorblue, dashed] (a1) -- (a2) -- (a3) -- (a4) -- (a5) -- (a6) -- cycle;
      \draw[color=colorblue, dashed] (b1) -- (b2) -- (b3) -- (b4) -- (b5) -- (b6) -- cycle;

      % The legs of the hexapod are drawn
      \draw[color=colorblue] (a1)node{$\bullet$} -- (b1)node{$\bullet$}node[below right]{$\bm{b}_3$};
      \draw[color=colorblue] (a2)node{$\bullet$} -- (b2)node{$\bullet$}node[right]{$\bm{b}_4$};
      \draw[color=colorblue] (a3)node{$\bullet$} -- (b3)node{$\bullet$}node[above right]{$\bm{b}_5$};
      \draw[color=colorblue] (a4)node{$\bullet$} -- (b4)node{$\bullet$}node[above left]{$\bm{b}_6$};
      \draw[color=colorblue] (a5)node{$\bullet$} -- (b5)node{$\bullet$}node[left]{$\bm{b}_1$};
      \draw[color=colorblue] (a6)node{$\bullet$} -- (b6)node{$\bullet$}node[below left]{$\bm{b}_2$};
    \end{scope}

    % Height of the Hexapod
    \coordinate[] (sizepos) at ($(a2)+(0.2, 0)$);
    \coordinate[] (origin) at (0,0,0);

    \draw[->, color=colorgreen] ($(cubecenter.center)+(0,2.0,0)$) node[above right]{$\{B\}$} -- ++(0,0,1);
    \draw[->, color=colorgreen] ($(cubecenter.center)+(0,2.0,0)$) -- ++(1,0,0);
    \draw[->, color=colorgreen] ($(cubecenter.center)+(0,2.0,0)$) -- ++(0,1,0);

    \node[] at (cubecenter.center){$\bullet$};
    \node[right] at (cubecenter.center){$\{C\}$};

    \draw[<->, dashed] (cubecenter.center) -- node[midway, right]{$H$} ($(cubecenter.center)+(0,2.0,0)$);
  \end{tikzpicture}
#+end_src

#+name: fig:detail_kinematics_cubic_schematic_cases
#+caption: Cubic architecture. Struts are represented in blue. The cube's center by a black dot. The Struts can match the cube's edges (\subref{fig:detail_kinematics_cubic_schematic_full}) or just take a portion of the edge (\subref{fig:detail_kinematics_cubic_schematic})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_schematic_full}Full cube}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :scale 0.9
[[file:figs/detail_kinematics_cubic_schematic_full.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_schematic}Cube's portion}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :scale 0.9
[[file:figs/detail_kinematics_cubic_schematic.png]]
#+end_subfigure
#+end_figure

Coordinates of the cube's vertices relevant for the top joints, expressed with respect to the cube's center, are shown in equation eqref:eq:detail_kinematics_cubic_vertices.

\begin{equation}\label{eq:detail_kinematics_cubic_vertices}
  \tilde{\bm{b}}_1 = \tilde{\bm{b}}_2 = H_c \begin{bmatrix} \frac{1}{\sqrt{2}}  \\ \frac{-\sqrt{3}}{\sqrt{2}} \\ \frac{1}{2} \end{bmatrix}, \quad
  \tilde{\bm{b}}_3 = \tilde{\bm{b}}_4 = H_c \begin{bmatrix} \frac{1}{\sqrt{2}}  \\ \frac{ \sqrt{3}}{\sqrt{2}} \\ \frac{1}{2} \end{bmatrix}, \quad
  \tilde{\bm{b}}_5 = \tilde{\bm{b}}_6 = H_c \begin{bmatrix} \frac{-2}{\sqrt{2}} \\  0                         \\ \frac{1}{2} \end{bmatrix}
\end{equation}

In the case where top joints are positioned at the cube's vertices, a diagonal stiffness matrix is obtained as shown in equation eqref:eq:detail_kinematics_cubic_stiffness.
Translation stiffness is twice the stiffness of the struts, and rotational stiffness is proportional to the square of the cube's size $H_c$.

\begin{equation}\label{eq:detail_kinematics_cubic_stiffness}
  \bm{K}_{\{B\} = \{C\}} = k \begin{bmatrix}
    2 & 0 & 0 & 0 & 0 & 0 \\
    0 & 2 & 0 & 0 & 0 & 0 \\
    0 & 0 & 2 & 0 & 0 & 0 \\
    0 & 0 & 0 & \frac{3}{2} H_c^2 & 0 & 0 \\
    0 & 0 & 0 & 0 & \frac{3}{2} H_c^2 & 0 \\
    0 & 0 & 0 & 0 & 0 & 6 H_c^2 \\
  \end{bmatrix}
\end{equation}

However, typically, the top joints are not placed at the cube's vertices but at positions along the cube's edges (Figure ref:fig:detail_kinematics_cubic_schematic).
In that case, the location of the top joints can be expressed by equation eqref:eq:detail_kinematics_cubic_edges, yet the computed stiffness matrix remains identical to Equation eqref:eq:detail_kinematics_cubic_stiffness.

\begin{equation}\label{eq:detail_kinematics_cubic_edges}
  \bm{b}_i = \tilde{\bm{b}}_i + \alpha \hat{\bm{s}}_i
\end{equation}


The stiffness matrix is therefore diagonal when the considered $\{B\}$ frame is located at the center of the cube (shown by frame $\{C\}$).
This means that static forces (resp torques) applied at the cube's center will induce pure translations (resp rotations around the cube's center).
This specific location where the stiffness matrix is diagonal is referred to as the "Center of Stiffness" (analogous to the "Center of Mass" where the mass matrix is diagonal).

#+begin_src matlab
%% Analytical formula for Stiffness matrix of the Cubic Stewart platform
% Define symbolic variables
syms k Hc alpha H

assume(k > 0); % k is positive real
assume(Hc, 'real'); % Hc is real
assume(H, 'real'); % H is real
assume(alpha, 'real'); % alpha is real

% Define si matrix (edges of the cubes)
si = 1/sqrt(3)*[
  [ sqrt(2),    0,        1]; ...
  [-sqrt(2)/2, -sqrt(3/2), 1]; ...
  [-sqrt(2)/2,  sqrt(3/2), 1]; ...
  [ sqrt(2),    0,        1]; ...
  [-sqrt(2)/2, -sqrt(3/2), 1]; ...
  [-sqrt(2)/2,  sqrt(3/2), 1]  ...
];

% Define ci matrix (vertices of the cubes)
ci = Hc * [
  [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2),  sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2),  sqrt(3)/sqrt(2), 0.5]; ...
  [-sqrt(2),   0,               0.5]; ...
  [-sqrt(2),   0,               0.5]  ...
];

% Apply vertical shift to ci
ci = ci + H * [0, 0, 1];

% Calculate bi vectors (Stewart platform top joints)
bi = ci + alpha * si;

% Initialize stiffness matrix
K = sym(zeros(6,6));

% Calculate elements of the stiffness matrix
for i = 1:6
    % Extract vectors for each leg
    s_i = si(i,:)';
    b_i = bi(i,:)';

    % Calculate cross product vector
    cross_bs = cross(b_i, s_i);

    % Build matrix blocks
    K(1:3, 4:6) = K(1:3, 4:6) + s_i * cross_bs';
    K(4:6, 1:3) = K(4:6, 1:3) + cross_bs * s_i';
    K(1:3, 1:3) = K(1:3, 1:3) + s_i * s_i';
    K(4:6, 4:6) = K(4:6, 4:6) + cross_bs * cross_bs';
end

% Scale by stiffness coefficient
K = k * K;

% Simplify the expressions
K = simplify(K);

% Display the analytical stiffness matrix
disp('Analytical Stiffness Matrix:');
pretty(K);
#+end_src

**** Effect of having frame $\{B\}$ off-centered

When the reference frames $\{A\}$ and $\{B\}$ are shifted from the cube's center, off-diagonal elements emerge in the stiffness matrix.

Considering a vertical shift as shown in Figure ref:fig:detail_kinematics_cubic_schematic, the stiffness matrix transforms into that shown in Equation eqref:eq:detail_kinematics_cubic_stiffness_off_centered.
Off-diagonal elements increase proportionally with the height difference between the cube's center and the considered $\{B\}$ frame.

\begin{equation}\label{eq:detail_kinematics_cubic_stiffness_off_centered}
  \bm{K}_{\{B\} \neq \{C\}} = k \begin{bmatrix}
    2 & 0 & 0 & 0 & -2 H & 0 \\
    0 & 2 & 0 & 2 H & 0 & 0 \\
    0 & 0 & 2 & 0 & 0 & 0 \\
    0 & 2 H & 0 & \frac{3}{2} H_c^2 + 2 H^2 & 0 & 0 \\
    -2 H & 0 & 0 & 0 & \frac{3}{2} H_c^2 + 2 H^2 & 0 \\
    0 & 0 & 0 & 0 & 0 & 6 H_c^2 \\
  \end{bmatrix}
\end{equation}

This stiffness matrix structure is characteristic of Stewart platforms exhibiting symmetry, and is not an exclusive property of cubic architectures.
Therefore, the stiffness characteristics of the cubic architecture are only distinctive when considering a reference frame located at the cube's center.
This poses a practical limitation, as in most applications, the relevant frame (where motion is of interest and forces are applied) is located above the top platform.

It should be noted that for the stiffness matrix to be diagonal, the cube's center doesn't need to coincide with the geometric center of the Stewart platform.
This observation leads to the interesting alternative architectures presented in Section ref:ssec:detail_kinematics_cubic_design.

**** Uniform Mobility

The translational mobility of the Stewart platform with constant orientation was analyzed.
Considering limited actuator stroke (elongation of each strut), the maximum achievable positions in XYZ space were estimated.
The resulting mobility in X, Y, and Z directions for the cubic architecture is illustrated in Figure ref:fig:detail_kinematics_cubic_mobility_translations.

The translational workspace analysis reveals that for the cubic architecture, the achievable positions form a cube whose axes align with the struts, with the cube's edge length corresponding to the strut axial stroke.
These findings suggest that the mobility pattern is more subtle than sometimes described in the literature [[cite:&mcinroy00_desig_contr_flexur_joint_hexap]], exhibiting uniformity primarily along directions aligned with the cube's edges rather than uniform spherical distribution in all XYZ directions.
This configuration still offers more consistent mobility characteristics compared to alternative architectures illustrated in Figure ref:fig:detail_kinematics_mobility_trans.

The rotational mobility, illustrated in Figure ref:fig:detail_kinematics_cubic_mobility_rotations, exhibits greater achievable angular stroke in the $R_x$ and $R_y$ directions compared to the $R_z$ direction.
Furthermore, an inverse relationship exists between the cube's dimension and rotational mobility, with larger cube sizes corresponding to more limited angular displacement capabilities.

#+begin_src matlab
%% Cubic configuration
H = 100e-3;     % Height of the Stewart platform [m]
Hc = 100e-3;   % Size of the useful part of the cube [m]
FOc = 50e-3; % Center of the cube at the Stewart platform center
MO_B = -50e-3;  % Position {B} with respect to {M} [m]
MHb = 0;

stewart_cubic = initializeStewartPlatform();
stewart_cubic = initializeFramesPositions(stewart_cubic, 'H', H, 'MO_B', MO_B);
stewart_cubic = generateCubicConfiguration(stewart_cubic, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', MHb);
stewart_cubic = computeJointsPose(stewart_cubic);
stewart_cubic = initializeStrutDynamics(stewart_cubic, 'k', 1);
stewart_cubic = computeJacobian(stewart_cubic);
stewart_cubic = initializeCylindricalPlatforms(stewart_cubic, 'Fpr', 150e-3, 'Mpr', 150e-3);

% Let's now define the actuator stroke.
L_max =  50e-6; % [m]
#+end_src

#+begin_src matlab :exports none :results none
%% Mobility of a Stewart platform with Cubic architecture - Translations
thetas = linspace(0, pi, 100);
phis = linspace(0, 2*pi, 200);
rs = zeros(length(thetas), length(phis));

for i = 1:length(thetas)
    for j = 1:length(phis)
        Tx = sin(thetas(i))*cos(phis(j));
        Ty = sin(thetas(i))*sin(phis(j));
        Tz = cos(thetas(i));

        dL = stewart_cubic.kinematics.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction

        rs(i, j) = L_max/max(abs(dL));
        % rs(i, j) = max(abs([dL(dL<0)*L_min; dL(dL>=0)*L_max]));
    end
end

[phi_grid, theta_grid] = meshgrid(phis, thetas);
X = 1e6 * rs .* sin(theta_grid) .* cos(phi_grid);
Y = 1e6 * rs .* sin(theta_grid) .* sin(phi_grid);
Z = 1e6 * rs .* cos(theta_grid);

figure;
hold on;
surf(X, Y, Z, 'FaceColor', 'white', 'EdgeColor', colors(1,:));
quiver3(0, 0, 0, 150, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 0, 0, 0, 150, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 0, 0, 0, 0, 150, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
text(150, 0, 0, '$D_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top'   );
text(0, 150, 0, '$D_y$', 'FontSize', 10, 'HorizontalAlignment', 'right',  'VerticalAlignment', 'bottom');
text(0, 0, 150, '$D_z$', 'FontSize', 10, 'HorizontalAlignment', 'left',   'VerticalAlignment', 'top'   );
hold off;
axis equal;
grid off;
axis off;
view(105, 15);
#+end_src

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

#+begin_src matlab :exports none :results none
%% Mobility of a Stewart platform with Cubic architecture - Rotations
thetas = linspace(0, pi, 100);
phis = linspace(0, 2*pi, 200);
rs_cubic = zeros(length(thetas), length(phis));

for i = 1:length(thetas)
    for j = 1:length(phis)
        Rx = sin(thetas(i))*cos(phis(j));
        Ry = sin(thetas(i))*sin(phis(j));
        Rz = cos(thetas(i));

        dL = stewart_cubic.kinematics.J*[0; 0; 0; Rx; Ry; Rz;];
        rs_cubic(i, j) = L_max/max(abs(dL));
    end
end

[phi_grid, theta_grid] = meshgrid(phis, thetas);

X_cubic = 1e6 * rs_cubic .* sin(theta_grid) .* cos(phi_grid);
Y_cubic = 1e6 * rs_cubic .* sin(theta_grid) .* sin(phi_grid);
Z_cubic = 1e6 * rs_cubic .* cos(theta_grid);

figure;
hold on;
surf(X_cubic, Y_cubic, Z_cubic, 'FaceColor', 'white', 'LineWidth', 0.2, 'EdgeColor', colors(1,:));
quiver3(0, 0, 0, 1500, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 0, 0, 0, 1500, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
quiver3(0, 0, 0, 0, 0, 1500, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
text(1500, 0, 0, '$R_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top'   );
text(0, 1500, 0, '$R_y$', 'FontSize', 10, 'HorizontalAlignment', 'right',  'VerticalAlignment', 'bottom');
text(0, 0, 1500, '$R_z$', 'FontSize', 10, 'HorizontalAlignment', 'left',   'VerticalAlignment', 'top'   );
hold off;
axis equal;
grid off;
axis off;
view(105, 15);
#+end_src

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

#+name: fig:detail_kinematics_cubic_mobility
#+caption: Mobility of a Stewart platform with Cubic architecture. Both for translations (\subref{fig:detail_kinematics_cubic_mobility_translations}) and rotations (\subref{fig:detail_kinematics_cubic_mobility_rotations})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_mobility_translations}Mobility in translation}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/detail_kinematics_cubic_mobility_translations.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_mobility_rotations}Mobility in rotation}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/detail_kinematics_cubic_mobility_rotations.png]]
#+end_subfigure
#+end_figure

** Dynamical Decoupling
<<ssec:detail_kinematics_cubic_dynamic>>
**** Introduction                                                  :ignore:

This section examines the dynamics of the cubic architecture in the Cartesian frame which corresponds to the transfer function from forces and torques $\bm{\mathcal{F}}$ to translations and rotations $\bm{\mathcal{X}}$ of the top platform.
When relative motion sensors are integrated in each strut (measuring $\bm{\mathcal{L}}$), the pose $\bm{\mathcal{X}}$ is computed using the Jacobian matrix as shown in Figure ref:fig:detail_kinematics_centralized_control.

#+begin_src latex :file detail_kinematics_centralized_control.pdf
\begin{tikzpicture}
  \node[block] (Jt) at (0, 0) {$\bm{J}^{-T}$};
  \node[block, right= of Jt] (G) {$\bm{G}$};
  \node[block, right= of G] (J) {$\bm{J}^{-1}$};
  \node[block, left= of Jt] (Kx) {$\bm{K}_{\mathcal{X}}$};

  \draw[->] (Kx.east) -- node[midway, above]{$\bm{\mathcal{F}}$} (Jt.west);
  \draw[->] (Jt.east) -- (G.west) node[above left]{$\bm{\tau}$};
  \draw[->] (G.east) -- (J.west) node[above left]{$\bm{\mathcal{L}}$};
  \draw[->] (J.east) -- ++(1.0, 0);
  \draw[->] ($(J.east) + (0.5, 0)$)node[]{$\bullet$} node[above]{$\bm{\mathcal{X}}$} -- ++(0, -1) -| ($(Kx.west) + (-0.5, 0)$) -- (Kx.west);

  \begin{scope}[on background layer]
    \node[fit={(Jt.south west) (J.north east)}, fill=black!20!white, draw, dashed, inner sep=4pt] (Px) {};
    \node[anchor={south}] at (Px.north){\small{Cartesian Plant}};
  \end{scope}
\end{tikzpicture}
#+end_src

#+name: fig:detail_kinematics_centralized_control
#+caption: Typical control architecture in the cartesian frame
#+RESULTS:
[[file:figs/detail_kinematics_centralized_control.png]]

**** Low frequency and High frequency coupling

As derived during the conceptual design phase, the dynamics from $\bm{\mathcal{F}}$ to $\bm{\mathcal{X}}$ is described by Equation eqref:eq:detail_kinematics_transfer_function_cart.
At low frequency, the behavior of the platform depends on the stiffness matrix eqref:eq:detail_kinematics_transfer_function_cart_low_freq.

\begin{equation}\label{eq:detail_kinematics_transfer_function_cart_low_freq}
    \frac{{\mathcal{X}}}{\bm{\mathcal{F}}}(j \omega) \xrightarrow[\omega \to 0]{} \bm{K}^{-1}
\end{equation}

In Section ref:ssec:detail_kinematics_cubic_static, it was demonstrated that for the cubic configuration, the stiffness matrix is diagonal if frame $\{B\}$ is positioned at the cube's center.
In this case, the "Cartesian" plant is decoupled at low frequency.
At high frequency, the behavior is governed by the mass matrix (evaluated at frame $\{B\}$) eqref:eq:detail_kinematics_transfer_function_high_freq.

\begin{equation}\label{eq:detail_kinematics_transfer_function_high_freq}
    \frac{{\mathcal{X}}}{\bm{\mathcal{F}}}(j \omega) \xrightarrow[\omega \to \infty]{} - \omega^2 \bm{M}^{-1}
\end{equation}

To achieve a diagonal mass matrix, the center of mass of the mobile components must coincide with the $\{B\}$ frame, and the principal axes of inertia must align with the axes of the $\{B\}$ frame.

#+name: fig:detail_kinematics_cubic_payload
#+caption: Cubic stewart platform with top cylindrical payload
#+attr_latex: :width 0.6\linewidth
[[file:figs/detail_kinematics_cubic_payload.png]]

To verify these properties, a cubic Stewart platform with a cylindrical payload was analyzed (Figure ref:fig:detail_kinematics_cubic_payload).
Transfer functions from $\bm{\mathcal{F}}$ to $\bm{\mathcal{X}}$ were computed for two specific locations of the $\{B\}$ frames.
When the $\{B\}$ frame was positioned at the center of mass, coupling at low frequency was observed due to the non-diagonal stiffness matrix (Figure ref:fig:detail_kinematics_cubic_cart_coupling_com).
Conversely, when positioned at the center of stiffness, coupling occurred at high frequency due to the non-diagonal mass matrix (Figure ref:fig:detail_kinematics_cubic_cart_coupling_cok).

#+begin_src matlab
%% Input/Output definition of the Simscape model
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput');  io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/plant'],      1, 'openoutput'); io_i = io_i + 1; % External metrology [m,rad]

% Prepare simulation
controller = initializeController('type', 'open-loop');
sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'R', 100e-3);

%% Cubic Stewart platform with payload above the top platform - B frame at the CoM
H    = 200e-3; % height of the Stewart platform [m]
MO_B = 50e-3;  % Position {B} with respect to {M} [m]

stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeCylindricalPlatforms(stewart, ...
                                         'Mpm', 1e-6, ... % Massless platform
                                         'Fpm', 1e-6, ... % Massless platform
                                         'Mph', 20e-3, ... % Thin platform
                                         'Fph', 20e-3, ... % Thin platform
                                         'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
                                         'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
stewart = initializeCylindricalStruts(stewart, ...
    'Fsm', 1e-6, ... % Massless strut
    'Msm', 1e-6, ... % Massless strut
    'Fsh', stewart.geometry.l(1)/2, ...
    'Msh', stewart.geometry.l(1)/2 ...
);

% Run the linearization
G_CoM = linearize(mdl, io)*inv(stewart.kinematics.J).';
G_CoM.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_CoM.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};

%% Same geometry but B Frame at cube's center (CoK)
MO_B = -100e-3;  % Position {B} with respect to {M} [m]
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeCylindricalPlatforms(stewart, ...
                                         'Mpm', 1e-6, ... % Massless platform
                                         'Fpm', 1e-6, ... % Massless platform
                                         'Mph', 20e-3, ... % Thin platform
                                         'Fph', 20e-3, ... % Thin platform
                                         'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
                                         'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
stewart = initializeCylindricalStruts(stewart, ...
    'Fsm', 1e-6, ... % Massless strut
    'Msm', 1e-6, ... % Massless strut
    'Fsh', stewart.geometry.l(1)/2, ...
    'Msh', stewart.geometry.l(1)/2 ...
);

% Run the linearization
G_CoK = linearize(mdl, io)*inv(stewart.kinematics.J.');
G_CoK.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_CoK.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src

#+begin_src matlab :exports none :results none
%% Coupling in the cartesian frame for a Cubic Stewart platform - Frame {B} is at the center of mass of the payload
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
% for i = 1:5
%     for j = i+1:6
%         plot(freqs, abs(squeeze(freqresp(G_CoM(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
%              'HandleVisibility', 'off');
%     end
% end
plot(freqs, abs(squeeze(freqresp(G_CoM(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$D_x/F_x$');
plot(freqs, abs(squeeze(freqresp(G_CoM(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$D_y/F_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ...
     'DisplayName', '$D_z/F_z$');
plot(freqs, abs(squeeze(freqresp(G_CoM(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ...
     'DisplayName', '$R_x/M_x$');
plot(freqs, abs(squeeze(freqresp(G_CoM(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ...
     'DisplayName', '$R_y/M_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ...
     'DisplayName', '$R_z/M_z$');
plot(freqs, abs(squeeze(freqresp(G_CoM(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$R_x/F_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$R_y/F_x$');
plot(freqs, abs(squeeze(freqresp(G_CoM(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$D_x/M_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$D_y/M_x$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]);
ylim([1e-10, 2e-3])
#+end_src

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

#+begin_src matlab :exports none :results none
%% Coupling in the cartesian frame for a Cubic Stewart platform - Frame {B} is at the center of the cube
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
% for i = 1:5
%     for j = i+1:6
%         plot(freqs, abs(squeeze(freqresp(G_CoK(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
%              'HandleVisibility', 'off');
%     end
% end
plot(freqs, abs(squeeze(freqresp(G_CoK(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$D_x/F_x$');
plot(freqs, abs(squeeze(freqresp(G_CoK(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$D_y/F_y$');
plot(freqs, abs(squeeze(freqresp(G_CoK(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ...
     'DisplayName', '$D_z/F_z$');
plot(freqs, abs(squeeze(freqresp(G_CoK(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ...
     'DisplayName', '$R_x/M_x$');
plot(freqs, abs(squeeze(freqresp(G_CoK(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ...
     'DisplayName', '$R_y/M_y$');
plot(freqs, abs(squeeze(freqresp(G_CoK(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ...
     'DisplayName', '$R_z/M_z$');
plot(freqs, abs(squeeze(freqresp(G_CoK(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$R_x/F_y$');
plot(freqs, abs(squeeze(freqresp(G_CoK(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$R_y/F_x$');
plot(freqs, abs(squeeze(freqresp(G_CoK(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$D_x/M_y$');
plot(freqs, abs(squeeze(freqresp(G_CoK(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$D_y/M_x$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]);
ylim([1e-10, 2e-3])
#+end_src

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

#+name: fig:detail_kinematics_cubic_cart_coupling
#+caption: Transfer functions for a Cubic Stewart platform expressed in the Cartesian frame. Two locations of the $\{B\}$ frame are considered: at the center of mass of the moving body (\subref{fig:detail_kinematics_cubic_cart_coupling_com}) and at the cube's center (\subref{fig:detail_kinematics_cubic_cart_coupling_cok}).
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_cart_coupling_com}$\{B\}$ at the center of mass}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_cubic_cart_coupling_com.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_cart_coupling_cok}$\{B\}$ at the cube's center}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_cubic_cart_coupling_cok.png]]
#+end_subfigure
#+end_figure

**** Payload's CoM at the cube's center

An effective strategy for improving dynamical performances involves aligning the cube's center (center of stiffness) with the center of mass of the moving components [[cite:&li01_simul_fault_vibrat_isolat_point]].
This can be achieved by positioning the payload below the top platform, such that the center of mass of the moving body coincides with the cube's center (Figure ref:fig:detail_kinematics_cubic_centered_payload).
This approach was physically implemented in several studies [[cite:&mcinroy99_dynam;&jafari03_orthog_gough_stewar_platf_microm]], as shown in Figure ref:fig:detail_kinematics_uw_gsp.
The resulting dynamics are indeed well-decoupled (Figure ref:fig:detail_kinematics_cubic_cart_coupling_com_cok), taking advantage from diagonal stiffness and mass matrices.
The primary limitation of this approach is that, for many applications including the nano-hexapod, the payload must be positioned above the top platform.
If a design similar to Figure ref:fig:detail_kinematics_cubic_centered_payload were employed for the nano-hexapod, the X-ray beam would intersect with the struts during spindle rotation.

#+begin_src matlab
%% Cubic Stewart platform with payload above the top platform
H = 200e-3;
MO_B = -100e-3;  % Position {B} with respect to {M} [m]

stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeCylindricalPlatforms(stewart, ...
                                         'Mpm', 1e-6, ... % Massless platform
                                         'Fpm', 1e-6, ... % Massless platform
                                         'Mph', 20e-3, ... % Thin platform
                                         'Fph', 20e-3, ... % Thin platform
                                         'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
                                         'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
stewart = initializeCylindricalStruts(stewart, ...
    'Fsm', 1e-6, ... % Massless strut
    'Msm', 1e-6, ... % Massless strut
    'Fsh', stewart.geometry.l(1)/2, ...
    'Msh', stewart.geometry.l(1)/2 ...
);

% Sample at the Center of the cube
sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'H_offset', -H/2-50e-3);

% Run the linearization
G_CoM_CoK = linearize(mdl, io)*inv(stewart.kinematics.J.');
G_CoM_CoK.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_CoM_CoK.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src

#+begin_src matlab :exports none
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$D_x/F_x$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$D_y/F_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ...
     'DisplayName', '$D_z/F_z$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ...
     'DisplayName', '$R_x/M_x$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ...
     'DisplayName', '$R_y/M_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ...
     'DisplayName', '$R_z/M_z$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$R_x/F_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$R_y/F_x$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$D_x/M_y$');
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$D_y/M_x$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]);
ylim([1e-10, 2e-3])
#+end_src

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

#+name: fig:detail_kinematics_cubic_com_cok
#+caption: Cubic Stewart platform with payload at the cube's center (\subref{fig:detail_kinematics_cubic_centered_payload}). Obtained cartesian plant is fully decoupled (\subref{fig:detail_kinematics_cubic_cart_coupling_com_cok})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_centered_payload}Payload at the cube's center}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_cubic_centered_payload.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_cart_coupling_com_cok}Fully decoupled cartesian plant}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_cubic_cart_coupling_com_cok.png]]
#+end_subfigure
#+end_figure

**** Conclusion

The analysis of dynamical properties of the cubic architecture yields several important conclusions.
Static decoupling, characterized by a diagonal stiffness matrix, is achieved when reference frames $\{A\}$ and $\{B\}$ are positioned at the cube's center.
Note that this property can also be obtained with non-cubic architectures that exhibit symmetrical strut arrangements.
Dynamic decoupling requires both static decoupling and coincidence of the mobile platform's center of mass with reference frame $\{B\}$.
While this configuration offers powerful control advantages, it requires positioning the payload at the cube's center, which is highly restrictive and often impractical.

** Decentralized Control
<<ssec:detail_kinematics_decentralized_control>>
**** Introduction                                                  :ignore:

The orthogonal arrangement of struts in the cubic architecture suggests a potential minimization of inter-strut coupling, which could theoretically create favorable conditions for decentralized control.
Two sensor types integrated in the struts are considered: displacement sensors and force sensors.
The control architecture is illustrated in Figure ref:fig:detail_kinematics_decentralized_control, where $\bm{K}_{\mathcal{L}}$ represents a diagonal transfer function matrix.

#+begin_src latex :file detail_kinematics_decentralized_control.pdf
\begin{tikzpicture}
  \node[block] (G) at (0,0) {$\bm{G}$};

  \node[block, left= of G] (Kl) {$\bm{K}_{\mathcal{L}}$};

  \draw[->] (Kl.east) -- node[midway, above]{$\bm{\tau}$} (G.west);
  \draw[->] (G.east) -- ++(1.0, 0);
  \draw[->] ($(G.east) + (0.5, 0)$)node[]{$\bullet$} node[above]{$\bm{\mathcal{L}}$} -- ++(0, -1) -| ($(Kl.west) + (-0.5, 0)$) -- (Kl.west);

  \begin{scope}[on background layer]
    \node[fit={(G.south west) (G.north east)}, fill=black!20!white, draw, dashed, inner sep=4pt] (Pl) {};
    \node[anchor={south}] at (Pl.north){\small{Strut Plant}};
  \end{scope}
\end{tikzpicture}
#+end_src

#+name: fig:detail_kinematics_decentralized_control
#+caption: Decentralized control in the frame of the struts.
#+RESULTS:
[[file:figs/detail_kinematics_decentralized_control.png]]

The obtained plant dynamics in the frame of the struts are compared for two Stewart platforms.
The first employs a cubic architecture shown in Figure ref:fig:detail_kinematics_cubic_payload.
The second uses a non-cubic Stewart platform shown in Figure ref:fig:detail_kinematics_non_cubic_payload, featuring identical payload and strut dynamics but with struts oriented more vertically to differentiate it from the cubic architecture.

#+name: fig:detail_kinematics_non_cubic_payload
#+caption: Stewart platform with non-cubic architecture
#+attr_latex: :width 0.6\linewidth
[[file:figs/detail_kinematics_non_cubic_payload.png]]

#+begin_src matlab
%% Input/Output definition of the Simscape model
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput');  io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/plant'],      2, 'openoutput', [], 'dL'); io_i = io_i + 1; % Displacement sensors [m]
io(io_i) = linio([mdl, '/plant'],      2, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensor [N]

% Prepare simulation : Payload above the top platform
controller = initializeController('type', 'open-loop');
sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'R', 100e-3);

%% Cubic Stewart platform
H    = 200e-3; % height of the Stewart platform [m]
MO_B = 50e-3;  % Position {B} with respect to {M} [m]

stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeCylindricalPlatforms(stewart, ...
                                         'Mpm', 1e-6, ... % Massless platform
                                         'Fpm', 1e-6, ... % Massless platform
                                         'Mph', 20e-3, ... % Thin platform
                                         'Fph', 20e-3, ... % Thin platform
                                         'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
                                         'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
stewart = initializeCylindricalStruts(stewart, ...
    'Fsm', 1e-6, ... % Massless strut
    'Msm', 1e-6, ... % Massless strut
    'Fsh', stewart.geometry.l(1)/2, ...
    'Msh', stewart.geometry.l(1)/2 ...
);

% Run the linearization
G_cubic = linearize(mdl, io);
G_cubic.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_cubic.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
                      'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};

%% Non-Cubic Stewart platform
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
stewart = generateGeneralConfiguration(stewart, 'FH', 25e-3, 'FR', 250e-3, 'MH', 25e-3, 'MR', 250e-3, ...
                                      'FTh', [-22, 22, 120-22, 120+22, 240-22, 240+22]*(pi/180), ...
                                      'MTh', [-60+22, 60-22, 60+22, 180-22, 180+22, -60-22]*(pi/180));
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeCylindricalPlatforms(stewart, ...
                                         'Mpm', 1e-6, ... % Massless platform
                                         'Fpm', 1e-6, ... % Massless platform
                                         'Mph', 20e-3, ... % Thin platform
                                         'Fph', 20e-3, ... % Thin platform
                                         'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
                                         'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
stewart = initializeCylindricalStruts(stewart, ...
    'Fsm', 1e-6, ... % Massless strut
    'Msm', 1e-6, ... % Massless strut
    'Fsh', stewart.geometry.l(1)/2, ...
    'Msh', stewart.geometry.l(1)/2 ...
);

% Run the linearization
G_non_cubic = linearize(mdl, io);
G_non_cubic.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_non_cubic.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
                          'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
#+end_src

**** Relative Displacement Sensors

The transfer functions from actuator force in each strut to the relative motion of the struts are presented in Figure ref:fig:detail_kinematics_decentralized_dL.
As anticipated from the equations of motion from $\bm{f}$ to $\bm{\mathcal{L}}$ eqref:eq:detail_kinematics_transfer_function_struts, the $6 \times 6$ plant is decoupled at low frequency.
At high frequency, coupling is observed as the mass matrix projected in the strut frame is not diagonal.

No significant advantage is evident for the cubic architecture (Figure ref:fig:detail_kinematics_cubic_decentralized_dL) compared to the non-cubic architecture (Figure ref:fig:detail_kinematics_non_cubic_decentralized_dL).
The resonance frequencies differ between the two cases because the more vertical strut orientation in the non-cubic architecture alters the stiffness properties of the Stewart platform, consequently shifting the frequencies of various modes.

#+begin_src matlab :exports none :results none
%% Decentralized plant - Actuator force to Strut displacement - Cubic Architecture
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_non_cubic(sprintf('dL%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
             'HandleVisibility', 'off');
    end
end
plot(freqs, abs(squeeze(freqresp(G_non_cubic('dL1', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:)], 'linewidth', 2.5, ...
     'DisplayName', '$l_i/f_i$');
plot(freqs, abs(squeeze(freqresp(G_non_cubic('dL2', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
     'DisplayName', '$l_i/f_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]);
ylim([1e-10, 1e-4])
#+end_src

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

#+begin_src matlab :exports none :results none
%% Decentralized plant - Actuator force to Strut displacement - Cubic Architecture
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_cubic(sprintf('dL%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
             'HandleVisibility', 'off');
    end
end
plot(freqs, abs(squeeze(freqresp(G_cubic('dL1', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:)], 'linewidth', 2.5, ...
     'DisplayName', '$l_i/f_i$');
plot(freqs, abs(squeeze(freqresp(G_cubic('dL2', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
     'DisplayName', '$l_i/f_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]);
ylim([1e-10, 1e-4])
#+end_src

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

#+name: fig:detail_kinematics_decentralized_dL
#+caption: Bode plot of the transfer functions from actuator force to relative displacement sensor in each strut. Both for a non-cubic architecture (\subref{fig:detail_kinematics_non_cubic_decentralized_dL}) and for a cubic architecture (\subref{fig:detail_kinematics_cubic_decentralized_dL})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_non_cubic_decentralized_dL}Non cubic architecture}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_non_cubic_decentralized_dL.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_decentralized_dL}Cubic architecture}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_cubic_decentralized_dL.png]]
#+end_subfigure
#+end_figure

**** Force Sensors

Similarly, the transfer functions from actuator force to force sensors in each strut were analyzed for both cubic and non-cubic Stewart platforms.
The results are presented in Figure ref:fig:detail_kinematics_decentralized_fn.
The system demonstrates good decoupling at high frequency in both cases, with no clear advantage for the cubic architecture.

#+begin_src matlab :exports none :results none
%% Decentralized plant - Actuator force to strut force sensor - Cubic Architecture
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_non_cubic(sprintf('fn%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
             'HandleVisibility', 'off');
    end
end
plot(freqs, abs(squeeze(freqresp(G_non_cubic('fn1', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:)], 'linewidth', 2.5, ...
     'DisplayName', '$f_{m,i}/f_i$');
plot(freqs, abs(squeeze(freqresp(G_non_cubic('fn2', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
     'DisplayName', '$f_{m,i}/f_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [N/N]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]); ylim([1e-4, 1e2]);
#+end_src

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

#+begin_src matlab :exports none :results none
%% Decentralized plant - Actuator force to strut force sensor - Cubic Architecture
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_cubic(sprintf('fn%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
             'HandleVisibility', 'off');
    end
end
plot(freqs, abs(squeeze(freqresp(G_cubic('fn1', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:)], 'linewidth', 2.5, ...
     'DisplayName', '$f_{m,i}/f_i$');
plot(freqs, abs(squeeze(freqresp(G_cubic('fn2', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
     'DisplayName', '$f_{m,i}/f_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [N/N]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]); ylim([1e-4, 1e2]);
#+end_src

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

#+name: fig:detail_kinematics_decentralized_fn
#+caption: Bode plot of the transfer functions from actuator force to force sensor in each strut. Both for a non-cubic architecture (\subref{fig:detail_kinematics_non_cubic_decentralized_fn}) and for a cubic architecture (\subref{fig:detail_kinematics_cubic_decentralized_fn})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_non_cubic_decentralized_fn}Non cubic architecture}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_non_cubic_decentralized_fn.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_decentralized_fn}Cubic architecture}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/detail_kinematics_cubic_decentralized_fn.png]]
#+end_subfigure
#+end_figure

**** Conclusion

The presented results do not demonstrate the pronounced decoupling advantages often associated with cubic architectures in the literature.
Both the cubic and non-cubic configurations exhibited similar coupling characteristics, suggesting that the benefits of orthogonal strut arrangement for decentralized control is less obvious than often reported in the literature.

** Cubic architecture with Cube's center above the top platform
<<ssec:detail_kinematics_cubic_design>>
**** Introduction                                                  :ignore:

As demonstrated in Section ref:ssec:detail_kinematics_cubic_dynamic, the cubic architecture can exhibit advantageous dynamical properties when the center of mass of the moving body coincides with the cube's center, resulting in diagonal mass and stiffness matrices.
As shown in Section ref:ssec:detail_kinematics_cubic_static, the stiffness matrix is diagonal when the considered $\{B\}$ frame is located at the cube's center.
However, the $\{B\}$ frame is typically positioned above the top platform where forces are applied and displacements are measured.

This section proposes modifications to the cubic architecture to enable positioning the payload above the top platform while still leveraging the advantageous dynamical properties of the cubic configuration.

Three key parameters define the geometry of the cubic Stewart platform: $H$, the height of the Stewart platform (distance from fixed base to mobile platform); $H_c$, the height of the cube, as shown in Figure ref:fig:detail_kinematics_cubic_schematic_full; and $H_{CoM}$, the height of the center of mass relative to the mobile platform (coincident with the cube's center).

Depending on the cube's size $H_c$ in relation to $H$ and $H_{CoM}$, different designs emerge.
In the following examples, $H = 100\,mm$ and $H_{CoM} = 20\,mm$.

#+begin_src matlab
%% Cubic configurations with center of the cube above the top platform
H    = 100e-3; % height of the Stewart platform [m]
MO_B = 20e-3;  % Position {B} with respect to {M} [m]
FOc  = H + MO_B; % Center of the cube with respect to {F}
#+end_src

**** Small cube

When the cube size $H_c$ is smaller than twice the height of the CoM $H_{CoM}$ eqref:eq:detail_kinematics_cube_small, the resulting design is shown in Figure ref:fig:detail_kinematics_cubic_above_small.

\begin{equation}\label{eq:detail_kinematics_cube_small}
  H_c < 2 H_{CoM}
\end{equation}

# TODO - Add link to Figure ref:fig:nhexa_stewart_piezo_furutani (page pageref:fig:nhexa_stewart_piezo_furutani)
This configuration is similar to that described in [[cite:&furutani04_nanom_cuttin_machin_using_stewar]], although they do not explicitly identify it as a cubic configuration.
Adjacent struts are parallel to each other, differing from the typical architecture where parallel struts are positioned opposite to each other.

This approach yields a compact architecture, but the small cube size may result in insufficient rotational stiffness.

#+begin_src matlab
%% Small cube
Hc   = 2*MO_B;    % Size of the useful part of the cube [m]

stewart_small = initializeStewartPlatform();
stewart_small = initializeFramesPositions(stewart_small, 'H', H, 'MO_B', MO_B);
stewart_small = generateCubicConfiguration(stewart_small, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3);
stewart_small = computeJointsPose(stewart_small);
stewart_small = initializeStrutDynamics(stewart_small, 'k', 1);
stewart_small = computeJacobian(stewart_small);
stewart_small = initializeCylindricalPlatforms(stewart_small, 'Fpr', 1.1*max(vecnorm(stewart_small.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_small.platform_M.Mb)));
#+end_src

#+begin_src matlab :exports none :results none
%% ISO View
displayArchitecture(stewart_small, 'labels', false, 'frames', false);
plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');
#+end_src

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

#+begin_src matlab :exports none :results none
%% Side view
displayArchitecture(stewart_small, 'labels', false, 'frames', false);
plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');
view([90,0])
#+end_src

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

#+begin_src matlab :exports none :results none
%% Top view
displayArchitecture(stewart_small, 'labels', false, 'frames', false);
plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');
view([0,90])
#+end_src

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

#+name: fig:detail_kinematics_cubic_above_small
#+caption: Cubic architecture with cube's center above the top platform. A cube height of 40mm is used.
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_small_iso}Isometric view}
#+attr_latex: :options {0.36\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/detail_kinematics_cubic_above_small_iso.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_small_side}Side view}
#+attr_latex: :options {0.30\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/detail_kinematics_cubic_above_small_side.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_small_top}Top view}
#+attr_latex: :options {0.30\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/detail_kinematics_cubic_above_small_top.png]]
#+end_subfigure
#+end_figure

**** Medium sized cube

Increasing the cube's size such that eqref:eq:detail_kinematics_cube_medium is verified produces an architecture with intersecting struts (Figure ref:fig:detail_kinematics_cubic_above_medium).

\begin{equation}\label{eq:detail_kinematics_cube_medium}
  2 H_{CoM} < H_c < 2 (H_{CoM} + H)
\end{equation}

This configuration resembles the design proposed in [[cite:&yang19_dynam_model_decoup_contr_flexib]] (Figure ref:fig:detail_kinematics_yang19), although their design is not strictly cubic.

#+begin_src matlab :exports none :results none
%% Example of a cubic architecture with cube's center above the top platform - Medium cube size
Hc   = H + 2*MO_B;    % Size of the useful part of the cube [m]

stewart_medium = initializeStewartPlatform();
stewart_medium = initializeFramesPositions(stewart_medium, 'H', H, 'MO_B', MO_B);
stewart_medium = generateCubicConfiguration(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3);
stewart_medium = computeJointsPose(stewart_medium);
stewart_medium = initializeStrutDynamics(stewart_medium, 'k', 1);
stewart_medium = computeJacobian(stewart_medium);
stewart_medium = initializeCylindricalPlatforms(stewart_medium, 'Fpr', 1.1*max(vecnorm(stewart_medium.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_medium.platform_M.Mb)));
#+end_src

#+begin_src matlab :exports none :results none
%% ISO View
displayArchitecture(stewart_medium, 'labels', false, 'frames', false);
plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');
#+end_src

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

#+begin_src matlab :exports none :results none
%% Side view
displayArchitecture(stewart_medium, 'labels', false, 'frames', false);
plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');
view([90,0])
#+end_src

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

#+begin_src matlab :exports none :results none
%% Top view
displayArchitecture(stewart_medium, 'labels', false, 'frames', false);
plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');
view([0,90])
#+end_src

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

#+name: fig:detail_kinematics_cubic_above_medium
#+caption: Cubic architecture with cube's center above the top platform. A cube height of 140mm is used.
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_medium_iso}Isometric view}
#+attr_latex: :options {0.36\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/detail_kinematics_cubic_above_medium_iso.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_medium_side}Side view}
#+attr_latex: :options {0.30\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/detail_kinematics_cubic_above_medium_side.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_medium_top}Top view}
#+attr_latex: :options {0.30\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/detail_kinematics_cubic_above_medium_top.png]]
#+end_subfigure
#+end_figure

**** Large cube

When the cube's height exceeds twice the sum of the platform height and CoM height eqref:eq:detail_kinematics_cube_large, the architecture shown in Figure ref:fig:detail_kinematics_cubic_above_large is obtained.

\begin{equation}\label{eq:detail_kinematics_cube_large}
  2 (H_{CoM} + H) < H_c
\end{equation}

#+begin_src matlab :exports none :results none
%% Example of a cubic architecture with cube's center above the top platform - Large cube size
Hc   = 2*(H + MO_B);    % Size of the useful part of the cube [m]

stewart_large = initializeStewartPlatform();
stewart_large = initializeFramesPositions(stewart_large, 'H', H, 'MO_B', MO_B);
stewart_large = generateCubicConfiguration(stewart_large, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3);
stewart_large = computeJointsPose(stewart_large);
stewart_large = initializeStrutDynamics(stewart_large, 'k', 1);
stewart_large = computeJacobian(stewart_large);
stewart_large = initializeCylindricalPlatforms(stewart_large, 'Fpr', 1.1*max(vecnorm(stewart_large.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_large.platform_M.Mb)));
#+end_src

#+begin_src matlab :exports none :results none
%% ISO View
displayArchitecture(stewart_large, 'labels', false, 'frames', false);
plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');
#+end_src

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

#+begin_src matlab :exports none :results none
%% Side view
displayArchitecture(stewart_large, 'labels', false, 'frames', false);
plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');
view([90,0])
#+end_src

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

#+begin_src matlab :exports none :results none
%% Top view
displayArchitecture(stewart_large, 'labels', false, 'frames', false);
plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
scatter3(0, 0, FOc, 200, 'kh');
view([0,90])
#+end_src

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

#+name: fig:detail_kinematics_cubic_above_large
#+caption: Cubic architecture with cube's center above the top platform. A cube height of 240mm is used.
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_large_iso}Isometric view}
#+attr_latex: :options {0.36\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/detail_kinematics_cubic_above_large_iso.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_large_side}Side view}
#+attr_latex: :options {0.30\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/detail_kinematics_cubic_above_large_side.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_large_top}Top view}
#+attr_latex: :options {0.30\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/detail_kinematics_cubic_above_large_top.png]]
#+end_subfigure
#+end_figure

**** Platform size

#+begin_src matlab
%% Get the analytical formula for the location of the top and bottom joints
% Define symbolic variables
syms k Hc Hcom alpha H

assume(k > 0); % k is positive real
assume(Hcom > 0); % k is positive real
assume(Hc > 0); % Hc is real
assume(H > 0); % H is real
assume(alpha, 'real'); % alpha is real

% Define si matrix (edges of the cubes)
si = 1/sqrt(3)*[
  [ sqrt(2),    0,        1]; ...
  [-sqrt(2)/2, -sqrt(3/2), 1]; ...
  [-sqrt(2)/2,  sqrt(3/2), 1]; ...
  [ sqrt(2),    0,        1]; ...
  [-sqrt(2)/2, -sqrt(3/2), 1]; ...
  [-sqrt(2)/2,  sqrt(3/2), 1]  ...
];

% Define ci matrix (vertices of the cubes)
ci = Hc * [
  [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2),  sqrt(3)/sqrt(2), 0.5]; ...
  [1/sqrt(2),  sqrt(3)/sqrt(2), 0.5]; ...
  [-sqrt(2),   0,               0.5]; ...
  [-sqrt(2),   0,               0.5]  ...
];

% Apply vertical shift to ci
ci = ci + (H + Hcom) * [0, 0, 1];

% Calculate bi vectors (Stewart platform top joints)
bi = ci + alpha * si;


% Extract the z-component value from the first row of ci
% (all rows have the same z-component)
ci_z = ci(1, 3);

% The z-component of si is 1 for all rows
si_z = si(1, 3);

alpha_for_0 = solve(ci_z + alpha * si_z == 0, alpha);
alpha_for_H = solve(ci_z + alpha * si_z == H, alpha);

% Verify the results
% Substitute alpha values and check the resulting bi_z values
bi_z_0 = ci + alpha_for_0 * si;
disp('Radius for fixed base:');
simplify(sqrt(bi_z_0(1,1).^2 + bi_z_0(1,2).^2))

bi_z_H = ci + alpha_for_H * si;
disp('Radius for mobile platform:');
simplify(sqrt(bi_z_H(1,1).^2 + bi_z_H(1,2).^2))
#+end_src

For the proposed configuration, the top joints $\bm{b}_i$ (resp. the bottom joints $\bm{a}_i$) and are positioned on a circle with radius $R_{b_i}$ (resp. $R_{a_i}$) described by Equation eqref:eq:detail_kinematics_cube_joints.

\begin{subequations}\label{eq:detail_kinematics_cube_joints}
\begin{align}
  R_{b_i} &= \sqrt{\frac{3}{2} H_c^2 + 2 H_{CoM}^2} \label{eq:detail_kinematics_cube_top_joints} \\
  R_{a_i} &= \sqrt{\frac{3}{2} H_c^2 + 2 (H_{CoM} + H)^2} \label{eq:detail_kinematics_cube_bot_joints}
\end{align}
\end{subequations}

Since the rotational stiffness for the cubic architecture scales with the square of the cube's height eqref:eq:detail_kinematics_cubic_stiffness, the cube's size can be determined based on rotational stiffness requirements.
Subsequently, using Equation eqref:eq:detail_kinematics_cube_joints, the dimensions of the top and bottom platforms can be calculated.

** Conclusion
:PROPERTIES:
:UNNUMBERED: t
:END:

The analysis of the cubic architecture for Stewart platforms yielded several important findings.
While the cubic configuration provides uniform stiffness in the XYZ directions, it stiffness property becomes particularly advantageous when forces and torques are applied at the cube's center.
Under these conditions, the stiffness matrix becomes diagonal, resulting in a decoupled Cartesian plant at low frequencies.

Regarding mobility, the translational capabilities of the cubic configuration exhibit uniformity along the directions of the orthogonal struts, rather than complete uniformity in the Cartesian space.
This understanding refines the characterization of cubic architecture mobility commonly presented in literature.

The analysis of decentralized control in the frame of the struts revealed more nuanced results than expected.
While cubic architectures are frequently associated with reduced coupling between actuators and sensors, this study showed that these benefits may be more subtle or context-dependent than commonly described.
Under the conditions analyzed, the coupling characteristics of cubic and non-cubic configurations, in the frame of the struts, appeared similar.

Fully decoupled dynamics in the Cartesian frame can be achieved when the center of mass of the moving body coincides with the cube's center.
However, this arrangement presents practical challenges, as the cube's center is traditionally located between the top and bottom platforms, making payload placement problematic for many applications.

To address this limitation, modified cubic architectures have been proposed with the cube's center positioned above the top platform.
Three distinct configurations have been identified, each with different geometric arrangements but sharing the common characteristic that the cube's center is positioned above the top platform.
This structural modification enables the alignment of the moving body's center of mass with the center of stiffness, resulting in beneficial decoupling properties in the Cartesian frame.

* Nano Hexapod
:PROPERTIES:
:HEADER-ARGS:matlab+: :tangle matlab/detail_kinematics_3_nano_hexapod.m
:END:
<<sec:detail_kinematics_nano_hexapod>>
** Introduction                                                      :ignore:

Based on previous analysis, this section aims to determine the nano-hexapod optimal geometry.
For the NASS, the chosen reference frames $\{A\}$ and $\{B\}$ coincide with the sample's point of interest, which is positioned $150\,mm$ above the top platform.
This is the location where precise control of the sample's position is required, as it is where the x-ray beam is focused.

** Matlab Init                                              :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src

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

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

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

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

** Requirements
<<ssec:detail_kinematics_nano_hexapod_requirements>>

The design of the nano-hexapod must satisfy several constraints.
The device should fit within a cylinder with radius of $120\,mm$ and height of $95\,mm$.
Based on the measured errors of all stages of the micro-stations, and incorporating safety margins, the required mobility should enable combined translations in any direction of $\pm 50\,\mu m$.
At any position, the system should be capable of performing $R_x$ and $R_y$ rotations of $\pm 50\,\mu \text{rad}$.
Regarding stiffness, the resonance frequencies should be well above the maximum rotational velocity of $2\pi\,\text{rad/s}$ to minimize gyroscopic effects, while remaining below the problematic modes of the micro-station to ensure decoupling from its complex dynamics.
In terms of dynamics, the design should facilitate implementation of Integral Force Feedback (IFF) in a decentralized manner, and provide good decoupling for the high authority controller in the frame of the struts.

** Obtained Geometry
<<ssec:detail_kinematics_nano_hexapod_geometry>>

Based on the previous analysis of Stewart platform configurations, while the geometry can be optimized to achieve the desired trade-off between stiffness and mobility in different directions, the wide range of potential payloads, with masses ranging from 1kg to 50kg, makes it impossible to develop a single geometry that provides optimal dynamical properties for all possible configurations.

For the nano-hexapod design, the struts were oriented more vertically compared to a cubic architecture due to several considerations.
First, the performance requirements in the vertical direction are more stringent than in the horizontal direction.
This vertical strut orientation decreases the amplification factor in the vertical direction, providing greater resolution and reducing the effects of actuator noise.
Second, the micro-station's vertical modes exhibit higher frequencies than its lateral modes.
Therefore, higher resonance frequencies of the nano-hexapod in the vertical direction compared to the horizontal direction enhance the decoupling properties between the micro-station and the nano-hexapod.

Regarding dynamical properties, particularly for control in the frame of the struts, no specific optimization was implemented since the analysis revealed that strut orientation has minimal impact on the resulting coupling characteristics.

Consequently, the geometry was selected according to practical constraints.
The height between the two plates is maximized and set at $95\,mm$.
Both platforms utilize the maximum available size, with joints offset by $15\,mm$ from the plate surfaces and positioned along circles with radii of $120\,mm$ for the fixed joints and $110\,mm$ for the mobile joints.
The positioning angles, as shown in Figure ref:fig:detail_kinematics_nano_hexapod_top, are $[255,\ 285,\ 15,\ 45,\ 135,\ 165]$ degrees for the top joints and $[220,\ 320,\ 340,\ 80,\ 100,\ 200]$ degrees for the bottom joints.

#+begin_src matlab
%% Obtained Nano Hexapod Design
nano_hexapod = initializeStewartPlatform();
nano_hexapod = initializeFramesPositions(nano_hexapod, ...
                                    'H', 95e-3, ...
                                    'MO_B', 150e-3);

nano_hexapod = generateGeneralConfiguration(nano_hexapod, ...
                                       'FH', 15e-3, ...
                                       'FR', 120e-3, ...
                                       'FTh', [220, 320, 340, 80, 100, 200]*(pi/180), ...
                                       'MH', 15e-3, ...
                                       'MR', 110e-3, ...
                                       'MTh', [255, 285, 15, 45, 135, 165]*(pi/180));
nano_hexapod = computeJointsPose(nano_hexapod);
nano_hexapod = initializeStrutDynamics(nano_hexapod, 'k', 1);
nano_hexapod = computeJacobian(nano_hexapod);
nano_hexapod = initializeCylindricalPlatforms(nano_hexapod, 'Fpr', 125e-3, 'Mpr', 115e-3);
#+end_src

#+begin_src matlab :exports none :results none
%% Obtained architecture for the Nano Hexapod
displayArchitecture(nano_hexapod, 'labels', true, 'frames', true);
% Bottom circle
h = 15e-3;
r = 120e-3;
theta = linspace(0, 2*pi, 100);
x = r * cos(theta);
y = r * sin(theta);
z = h * ones(size(theta)); % All points at same height
plot3(x, y, z, '--', 'color', [colors(1,:)], 'LineWidth', 0.5);
for i = 1:6
    plot3([0, nano_hexapod.platform_F.Fa(1,i)], [0, nano_hexapod.platform_F.Fa(2,i)], [h,h], '--', 'color', [colors(1,:)], 'LineWidth', 0.5);
end
% Top circle
h = 95e-3 - 15e-3;
r = 110e-3;
theta = linspace(0, 2*pi, 100);
x = r * cos(theta);
y = r * sin(theta);
z = h * ones(size(theta)); % All points at same height
plot3(x, y, z, '--', 'color', [colors(2,:)], 'LineWidth', 0.5);
for i = 1:6
    plot3([0, nano_hexapod.platform_M.Mb(1,i)], [0, nano_hexapod.platform_M.Mb(2,i)], [h,h], '--', 'color', [colors(2,:)], 'LineWidth', 0.5);
end
#+end_src

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

#+begin_src matlab :exports none :results none :tangle no
%% Obtained architecture for the Nano Hexapod
displayArchitecture(nano_hexapod, 'labels', false, 'frames', true);
% Bottom circle
h = 15e-3;
r = 120e-3;
theta = linspace(0, 2*pi, 100);
x = r * cos(theta);
y = r * sin(theta);
z = h * ones(size(theta)); % All points at same height
plot3(x, y, z, '--', 'color', [colors(1,:)], 'LineWidth', 0.5);
for i = 1:6
    plot3([0, nano_hexapod.platform_F.Fa(1,i)], [0, nano_hexapod.platform_F.Fa(2,i)], [h,h], '--', 'color', [colors(1,:)], 'LineWidth', 0.5);
end
% Top circle
h = 95e-3 - 15e-3;
r = 110e-3;
theta = linspace(0, 2*pi, 100);
x = r * cos(theta);
y = r * sin(theta);
z = h * ones(size(theta)); % All points at same height
plot3(x, y, z, '--', 'color', [colors(2,:)], 'LineWidth', 0.5);
for i = 1:6
    plot3([0, nano_hexapod.platform_M.Mb(1,i)], [0, nano_hexapod.platform_M.Mb(2,i)], [h,h], '--', 'color', [colors(2,:)], 'LineWidth', 0.5);
end
view([0,90])
#+end_src

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

#+name: fig:detail_kinematics_nano_hexapod
#+caption: Obtained architecture for the Nano Hexapod
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_nano_hexapod_iso}Isometric view}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/detail_kinematics_nano_hexapod_iso.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_nano_hexapod_top}Top view}
#+attr_latex: :options {0.48\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/detail_kinematics_nano_hexapod_top.png]]
#+end_subfigure
#+end_figure

The resulting geometry is illustrated in Figure ref:fig:detail_kinematics_nano_hexapod.
While minor refinements may occur during detailed mechanical design to address manufacturing and assembly considerations, the fundamental geometry will remain consistent with this configuration.
This geometry serves as the foundation for estimating required actuator stroke (Section ref:ssec:detail_kinematics_nano_hexapod_actuator_stroke), determining flexible joint stroke requirements (Section ref:ssec:detail_kinematics_nano_hexapod_joint_stroke), performing noise budgeting for instrumentation selection, and developing control strategies.
# TODO - Add link to sections

Implementing a cubic architecture as proposed in Section ref:ssec:detail_kinematics_cubic_design was considered.
However, positioning the cube's center $150\,mm$ above the top platform would have resulted in platform dimensions exceeding the maximum available size.
Additionally, to benefit from the cubic configuration's dynamical properties, each payload would require careful calibration of inertia before placement on the nano-hexapod, ensuring that its center of mass coincides with the cube's center.
Given the impracticality of consistently aligning the center of mass with the cube's center, the cubic architecture was deemed unsuitable for the nano-hexapod application.

** Required Actuator stroke
<<ssec:detail_kinematics_nano_hexapod_actuator_stroke>>

With the geometry established, the actuator stroke necessary to achieve the desired mobility can be determined.

The required mobility parameters include combined translations in the XYZ directions of $\pm 50\,\mu m$ (essentially a cubic workspace).
Additionally, at any point within this workspace, combined $R_x$ and $R_y$ rotations of $\pm 50\,\mu \text{rad}$, with $R_z$ maintained at 0, should be possible.

Calculations based on the selected geometry indicate that an actuator stroke of $\pm 94\,\mu m$ is required to achieve the desired mobility.
This specification will be used during the actuator selection process.
# TODO - Add link to section

Figure ref:fig:detail_kinematics_nano_hexapod_mobility illustrates both the desired mobility (represented as a cube) and the calculated mobility envelope of the nano-hexapod with an actuator stroke of $\pm 94\,\mu m$.
The diagram confirms that the required workspace fits within the system's capabilities.

#+begin_src matlab
%% Estimate required actuator stroke for the wanted mobility
max_translation = 50e-6; % Wanted translation mobility [m]
max_rotation = 50e-6; % Wanted rotation mobility [rad]

Dxs = linspace(-max_translation, max_translation, 3);
Dys = linspace(-max_translation, max_translation, 3);
Dzs = linspace(-max_translation, max_translation, 3);
Rxs = linspace(-max_rotation, max_rotation, 3);
Rys = linspace(-max_rotation, max_rotation, 3);

L_min = 0; % Required actuator negative stroke [m]
L_max = 0; % Required actuator negative stroke [m]

for Dx = Dxs
    for Dy = Dys
        for Dz = Dzs
            for Rx = Rxs
                for Ry = Rys
                    ARB = [ cos(Ry) 0        sin(Ry);
                            0       1        0;
                           -sin(Ry) 0        cos(Ry)] * ...
                          [ 1       0        0;
                            0       cos(Rx) -sin(Rx);
                            0       sin(Rx)  cos(Rx)];

                    [~, Ls] = inverseKinematics(nano_hexapod, 'AP', [Dx;Dy;Dz], 'ARB', ARB);

                    if min(Ls) < L_min
                        L_min = min(Ls);
                    end
                    if max(Ls) > L_max
                        L_max = max(Ls);
                    end
                end
            end
        end
    end
end

sprintf('Actuator stroke should be from %.0f um to %.0f um', 1e6*L_min, 1e6*L_max)
#+end_src

#+begin_src matlab
%% Compute mobility in translation with combined angular motion
% Direction of motion (spherical coordinates)
thetas = linspace(0, pi, 100);
phis = linspace(0, 2*pi, 100);

% Considered Rotations
Rxs = linspace(-max_rotation, max_rotation, 3);
Rys = linspace(-max_rotation, max_rotation, 3);

% Maximum distance that can be reached in the direction of motion
% Considering combined angular motion and limited actuator stroke
rs = zeros(length(thetas), length(phis));
worst_rx_ry = zeros(length(thetas), length(phis), 2);

for i = 1:length(thetas)
    for j = 1:length(phis)
        % Compute unitary motion in the considered direction
        Tx = sin(thetas(i))*cos(phis(j));
        Ty = sin(thetas(i))*sin(phis(j));
        Tz = cos(thetas(i));
        % Start without considering rotations
        dL_lin = nano_hexapod.kinematics.J*[Tx; Ty; Tz; 0; 0; 0];
        % Strut motion for maximum displacement in the considered direction
        dL_lin_max = L_max*dL_lin/max(abs(dL_lin));

        % Find rotation that gives worst case stroke
        dL_worst = max(abs(dL_lin_max)); % This should be equal to L_max
        dL_rot_max = zeros(6,1);
        % Perform (small) rotations, and find the (worst) case requiring maximum strut motion
        for Rx = Rxs
            for Ry = Rys
                dL_rot = nano_hexapod.kinematics.J*[0; 0; 0; Rx; Ry; 0];
                if max(abs(dL_lin_max + dL_rot)) > dL_worst
                    dL_worst = max(abs(dL_lin_max + dL_rot));
                    dL_rot_max = dL_rot;
                    worst_rx_ry(i,j,:) = [Rx, Ry];
                end
            end
        end

        stroke_ratio = min(abs([( L_max - dL_rot_max) ./ dL_lin_max; (-L_max - dL_rot_max) ./ dL_lin_max]));
        dL_real = dL_lin_max*stroke_ratio + dL_rot_max;

        % % Obtained maximum displacement in the considered direction with angular motion
        rs(i, j) = stroke_ratio*L_max/max(abs(dL_lin));
    end
end
#+end_src

#+begin_src matlab :exports none :results none
%% Wanted translation mobility of the Nano-Hexapod and computed Mobility
[phi_grid, theta_grid] = meshgrid(phis, thetas);
X = 1e6 * rs .* sin(theta_grid) .* cos(phi_grid);
Y = 1e6 * rs .* sin(theta_grid) .* sin(phi_grid);
Z = 1e6 * rs .* cos(theta_grid);

vertices = 1e6*max_translation*[
    -1 -1 -1;  % vertex 1
     1 -1 -1;  % vertex 2
     1  1 -1;  % vertex 3
    -1  1 -1;  % vertex 4
    -1 -1  1;  % vertex 5
     1 -1  1;  % vertex 6
     1  1  1;  % vertex 7
    -1  1  1   % vertex 8
];

% Define the faces using the vertex indices
faces = [
    1 2 3 4;  % bottom face
    5 6 7 8;  % top face
    1 2 6 5;  % front face
    2 3 7 6;  % right face
    3 4 8 7;  % back face
    4 1 5 8   % left face
];

figure;
hold on;
s = surf(X, Y, Z, 'FaceColor', 'none');
s.EdgeColor = colors(2, :);
patch('Vertices', vertices, 'Faces', faces, ...
      'FaceColor', [0.7 0.7 0.7], ...
      'EdgeColor', 'black', ...
      'FaceAlpha', 1);
hold off;
axis equal;
grid on;
xlabel('X Translation [$\mu$m]'); ylabel('Y Translation [$\mu$m]'); zlabel('Z Translation [$\mu$m]');
xlim([-150 150]); ylim([-150, 150]); zlim([-120, 120]);
view(-160, 20)
#+end_src

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

#+name: fig:detail_kinematics_nano_hexapod_mobility
#+caption: Specified translation mobility of the Nano-Hexapod (grey cube) and computed Mobility (red volume).
#+RESULTS:
[[file:figs/detail_kinematics_nano_hexapod_mobility.png]]

** Required Joint angular stroke
<<ssec:detail_kinematics_nano_hexapod_joint_stroke>>

With the nano-hexapod geometry and mobility requirements established, the flexible joint angular stroke necessary to avoid limiting the achievable workspace can be determined.

This analysis focuses solely on bending stroke, as the torsional stroke of the flexible joints is expected to be minimal given the absence of vertical rotation requirements.
The required angular stroke for both fixed and mobile joints is estimated to be equal to $1\,\text{mrad}$.
This specification will guide the design of the flexible joints.
# TODO - Add link to section

#+begin_src matlab
%% Estimate required actuator stroke for the wanted mobility
max_translation = 50e-6; % Wanted translation mobility [m]
max_rotation = 50e-6; % Wanted rotation mobility [rad]

Dxs = linspace(-max_translation, max_translation, 3);
Dys = linspace(-max_translation, max_translation, 3);
Dzs = linspace(-max_translation, max_translation, 3);
Rxs = linspace(-max_rotation, max_rotation, 3);
Rys = linspace(-max_rotation, max_rotation, 3);

max_angles_F = zeros(1,6); % Maximum bending angle - Fixed joints [rad]
max_angles_M = zeros(1,6); % Maximum bending angle - Mobile joints [rad]

% Compute initial strut orientation
nano_hexapod = computeJointsPose(nano_hexapod, 'AP', zeros(3,1), 'ARB', eye(3));
As = nano_hexapod.geometry.As; % Fixed joints
Bs = nano_hexapod.geometry.Bs; % Mobile joints

for Dx = Dxs
    for Dy = Dys
        for Dz = Dzs
            for Rx = Rxs
                for Ry = Rys
                    ARB = [ cos(Ry) 0        sin(Ry);
                            0       1        0;
                           -sin(Ry) 0        cos(Ry)] * ...
                          [ 1       0        0;
                            0       cos(Rx) -sin(Rx);
                            0       sin(Rx)  cos(Rx)];

                    nano_hexapod = computeJointsPose(nano_hexapod, 'AP', [Dx;Dy;Dz], 'ARB', ARB);

                    angles_M = acos(dot(Bs, nano_hexapod.geometry.Bs));
                    max_angles_M(angles_M > max_angles_M) = angles_M(angles_M > max_angles_M);

                    angles_F = acos(dot(Bs, nano_hexapod.geometry.As));
                    max_angles_F(angles_F > max_angles_F) = angles_F(angles_F > max_angles_F);
                end
            end
        end
    end
end

sprintf('Fixed joint stroke should be %.1f mrad', 1e3*max(max_angles_F))
sprintf('Mobile joint stroke should be %.1f mrad', 1e3*max(max_angles_M))
#+end_src

* Conclusion
<<sec:detail_kinematics_conclusion>>

This chapter has explored the optimization of the nano-hexapod geometry for the Nano Active Stabilization System (NASS).

First, a review of existing Stewart platforms revealed two main geometric categories: cubic architectures, characterized by mutually orthogonal struts arranged along the edges of a cube, and non-cubic architectures with varied strut orientations.
While cubic architectures are prevalent in the literature and attributed with beneficial properties such as simplified kinematics, uniform stiffness, and reduced cross-coupling, the performed analysis revealed that some of these advantages should be more nuanced or context-dependent than commonly described.

The analytical relationships between Stewart platform geometry and its mechanical properties were established, enabling a better understanding of the trade-offs between competing requirements such as mobility and stiffness along different axes.
These insights were useful during the nano-hexapod geometry optimization.

For the cubic configuration, complete dynamical decoupling in the Cartesian frame can be achieved when the center of mass of the moving body coincides with the cube's center, but this arrangement is often impractical for real-world applications.
Modified cubic architectures with the cube's center positioned above the top platform were proposed as a potential solution, but proved unsuitable for the nano-hexapod due to size constraints and the impracticality of ensuring that different payloads' centers of mass would consistently align with the cube's center.

For the nano-hexapod design, a key challenge was addressing the wide range of potential payloads (1 to 50kg), which made it impossible to optimize the geometry for consistent dynamic performance across all usage scenarios.
This led to a practical design approach where struts were oriented more vertically than in cubic configurations to address several application-specific needs: achieving higher resolution in the vertical direction by reducing amplification factors and better matching the micro-station's modal characteristics with higher vertical resonance frequencies.

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

* Matlab Functions                                                  :noexport:
** =generateCubicConfiguration=: Generate a Cubic Configuration
#+begin_src matlab :tangle matlab/src/generateCubicConfiguration.m :comments none :mkdirp yes :eval no
function [stewart] = generateCubicConfiguration(stewart, args)
% generateCubicConfiguration - Generate a Cubic Configuration
%
% Syntax: [stewart] = generateCubicConfiguration(stewart, args)
%
% Inputs:
%    - stewart - A structure with the following fields
%        - geometry.H [1x1] - Total height of the platform [m]
%    - args - Can have the following fields:
%        - Hc  [1x1] - Height of the "useful" part of the cube [m]
%        - FOc [1x1] - Height of the center of the cube with respect to {F} [m]
%        - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m]
%        - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m]
%
% Outputs:

%    - stewart - updated Stewart structure with the added fields:
%        - platform_F.Fa  [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
%        - platform_M.Mb  [3x6] - Its i'th column is the position vector of joint bi with respect to {M}

arguments
    stewart
    args.Hc  (1,1) double {mustBeNumeric, mustBePositive} = 60e-3
    args.FOc (1,1) double {mustBeNumeric} = 50e-3
    args.FHa (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
    args.MHb (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
end

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

% We define the useful points of the cube with respect to the Cube's center.
% ${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is located at the center of the cube and aligned with {F} and {M}.

sx = [ 2; -1; -1];
sy = [ 0;  1; -1];
sz = [ 1;  1;  1];

R = [sx, sy, sz]./vecnorm([sx, sy, sz]);

L = args.Hc*sqrt(3);

Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*args.Hc];

CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg
CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg

% We can compute the vector of each leg ${}^{C}\hat{\bm{s}}_{i}$ (unit vector from ${}^{C}C_{f}$ to ${}^{C}C_{m}$).

CSi = (CCm - CCf)./vecnorm(CCm - CCf);

% We now which to compute the position of the joints $a_{i}$ and $b_{i}$.
Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
Mb = CCf + [0; 0; args.FOc-H] + ((H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;

stewart.platform_F.Fa = Fa;
stewart.platform_M.Mb = Mb;
#+end_src

** =plotCube=: Plot the Cube
#+begin_src matlab :tangle matlab/src/plotCube.m :comments none :mkdirp yes :eval no
function [] = plotCube(stewart, args)

arguments
    stewart
    args.Hc  (1,1) double {mustBeNumeric, mustBePositive} = 60e-3
    args.FOc (1,1) double {mustBeNumeric} = 50e-3
    args.color (4,1) double {mustBeNumeric} = [0,0,0,0.5]
    args.linewidth (1,1) double {mustBeNumeric, mustBePositive} = 2.5
    args.link_to_struts  logical {mustBeNumericOrLogical} = false
end

sx = [ 2; -1; -1];
sy = [ 0;  1; -1];
sz = [ 1;  1;  1];

R = [sx, sy, sz]./vecnorm([sx, sy, sz]);

L = args.Hc*sqrt(3);

p_xyz = R'*[[0;0;0],[L;0;0],[L;L;0],[0;L;0],[0;0;L],[L;0;L],[L;L;L],[0;L;L]] - [0;0;1.5*args.Hc];

% Position center of the cube
p_xyz = p_xyz + args.FOc*[0;0;1]*ones(1,8);

edges_order = [1 2 3 4 1];
plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth);
edges_order = [5 6 7 8 5];
plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth);
edges_order = [1 5];
plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth);
edges_order = [2 6];
plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth);
edges_order = [3 7];
plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth);
edges_order = [4 8];
plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth);

if args.link_to_struts
    Fb = stewart.platform_M.Mb + stewart.geometry.FO_M;
    plot3([Fb(1,1), p_xyz(1,5)],...
          [Fb(2,1), p_xyz(2,5)],...
          [Fb(3,1), p_xyz(3,5)], '--', 'color', args.color, 'linewidth', args.linewidth);
    plot3([Fb(1,2), p_xyz(1,2)],...
          [Fb(2,2), p_xyz(2,2)],...
          [Fb(3,2), p_xyz(3,2)], '--', 'color', args.color, 'linewidth', args.linewidth);
    plot3([Fb(1,3), p_xyz(1,2)],...
          [Fb(2,3), p_xyz(2,2)],...
          [Fb(3,3), p_xyz(3,2)], '--', 'color', args.color, 'linewidth', args.linewidth);
    plot3([Fb(1,4), p_xyz(1,4)],...
          [Fb(2,4), p_xyz(2,4)],...
          [Fb(3,4), p_xyz(3,4)], '--', 'color', args.color, 'linewidth', args.linewidth);
    plot3([Fb(1,5), p_xyz(1,4)],...
          [Fb(2,5), p_xyz(2,4)],...
          [Fb(3,5), p_xyz(3,4)], '--', 'color', args.color, 'linewidth', args.linewidth);
    plot3([Fb(1,6), p_xyz(1,5)],...
          [Fb(2,6), p_xyz(2,5)],...
          [Fb(3,6), p_xyz(3,5)], '--', 'color', args.color, 'linewidth', args.linewidth);
end
#+end_src

** =plotCylindricalPayload=: Plot a cylindrical Payload
#+begin_src matlab :tangle matlab/src/plotCylindricalPayload.m :comments none :mkdirp yes :eval no
function [] = plotCylindricalPayload(stewart, args)

arguments
    stewart
    args.H (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
    args.R (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
    args.H_offset (1,1) double {mustBeNumeric} = 0
    args.color (3,1) double {mustBeNumeric} = [0.5,0.5,0.5]
end

[X,Y,Z] = cylinder(args.R);
Z = args.H*Z + args.H_offset;
surf(X, Y, Z, 'facecolor', args.color, 'edgecolor', 'none')
fill3(X(1,:), Y(1,:), Z(1,:), 'k', 'facecolor', args.color)
fill3(X(2,:), Y(2,:), Z(2,:), 'k', 'facecolor', args.color)
#+end_src

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

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

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

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

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

K = J'*Ki*J;

C = inv(K);

stewart.kinematics.J = J;
stewart.kinematics.K = K;
stewart.kinematics.C = C;
#+end_src

** =inverseKinematics=: Compute Inverse Kinematics

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

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

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

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

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

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

dLi = Li-l;
#+end_src

** =forwardKinematicsApprox=: Compute the Approximate Forward Kinematics

#+begin_src matlab :tangle matlab/src/forwardKinematicsApprox.m :comments none :mkdirp yes :eval no
function [P, R] = forwardKinematicsApprox(stewart, args)
% forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using
%                           the Jacobian Matrix
%
% Syntax: [P, R] = forwardKinematicsApprox(stewart, args)
%
% Inputs:
%    - stewart - A structure with the following fields
%        - kinematics.J  [6x6] - The Jacobian Matrix
%    - args - Can have the following fields:
%        - dL [6x1] - Displacement of each strut [m]
%
% Outputs:
%    - P  [3x1] - The estimated position of {B} with respect to {A}
%    - R  [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}

arguments
    stewart
    args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
end

assert(isfield(stewart.kinematics, 'J'),   'stewart.kinematics should have attribute J')
J = stewart.kinematics.J;

X = J\args.dL;

P = X(1:3);

theta = norm(X(4:6));
s = X(4:6)/theta;

R = [s(1)^2*(1-cos(theta)) + cos(theta) ,        s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta);
     s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta),         s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
     s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];
#+end_src

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

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

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

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

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

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

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

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

stewart.geometry.H      = H;
stewart.geometry.FO_M   = FO_M;
stewart.platform_M.MO_B = MO_B;
stewart.platform_F.FO_A = FO_A;
#+end_src

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

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

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

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

stewart.platform_F.Fa = Fa;
stewart.platform_M.Mb = Mb;
#+end_src

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

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

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

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

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

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

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

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

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

Ab = args.ARB *Bb - repmat(-args.AP, [1, 6]);
Ba = args.ARB'*Aa - repmat( args.AP, [1, 6]);

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

l = vecnorm(Ab - Aa)';

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

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

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

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

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

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

stewart.platform_F.FRa = FRa;
stewart.platform_M.MRb = MRb;
#+end_src

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

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

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

stewart.actuators.Leq = dLi;
#+end_src

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

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

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

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

stewart.platform_F.type = 1;

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

stewart.platform_M.type = 1;

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

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

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

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

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

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

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

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

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

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

    stewart.actuators.F_gain = args.F_gain;

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

end
#+end_src


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

arguments
    stewart
    args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
    args.ARB (3,3) double {mustBeNumeric} = eye(3)
    args.F_color = [0 0.4470 0.7410]
    args.M_color = [0.8500 0.3250 0.0980]
    args.L_color = [0 0 0]
    args.frames    logical {mustBeNumericOrLogical} = true
    args.legs      logical {mustBeNumericOrLogical} = true
    args.joints    logical {mustBeNumericOrLogical} = true
    args.labels    logical {mustBeNumericOrLogical} = true
    args.platforms logical {mustBeNumericOrLogical} = true
    args.views     char    {mustBeMember(args.views,{'all', 'xy', 'xz', 'yz', 'default'})} = 'default'
end

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

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

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

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

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

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

hold on;

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

FTm = FTa*ATb*BTm;

% Let's define a parameter that define the length of the unit vectors used to display the frames.
d_unit_vector = H/4;

% Let's define a parameter used to position the labels with respect to the center of the element.
d_label = H/20;
% Let's first plot the frame $\{F\}$.
Ff = [0, 0, 0];
if args.frames
    quiver3(Ff(1)*ones(1,3), Ff(2)*ones(1,3), Ff(3)*ones(1,3), ...
            [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

% Plot the legs connecting the joints of the fixed base to the joints of the mobile platform.
if args.legs
    for i = 1:6
        plot3([Fa(1,i), Fb(1,i)], ...
              [Fa(2,i), Fb(2,i)], ...
              [Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color);

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

switch args.views
  case 'default'
    view([1 -0.6 0.4]);
  case 'xy'
    view([0 0 1]);
  case 'xz'
    view([0 -1 0]);
  case 'yz'
    view([1 0 0]);
end
axis equal;
axis off;

if strcmp(args.views, 'all')
    hAx = findobj('type', 'axes');

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

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

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

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

    close(f);
end
#+end_src

** =describeStewartPlatform=: Display some text describing the current defined Stewart Platform
#+begin_src matlab :tangle matlab/src/describeStewartPlatform.m :comments none :mkdirp yes :eval no
function [] = describeStewartPlatform(stewart)
% describeStewartPlatform - Display some text describing the current defined Stewart Platform
%
% Syntax: [] = describeStewartPlatform(args)
%
% Inputs:
%    - stewart
%
% Outputs:

arguments
    stewart
end

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

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

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

fprintf('ACTUATORS:\n')
if stewart.actuators.type == 1
    fprintf('- The actuators are classical.\n')
    fprintf('- The Stiffness and Damping of each actuators is:\n')
    fprintf('\t k = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.k, stewart.actuators.c)
elseif stewart.actuators.type == 2
    fprintf('- The actuators are mechanicaly amplified.\n')
end
fprintf('\n')

fprintf('JOINTS:\n')

switch stewart.joints_F.type
  case 1
    fprintf('- The joints on the fixed based are universal joints\n')
  case 2
    fprintf('- The joints on the fixed based are spherical joints\n')
  case 3
    fprintf('- The joints on the fixed based are perfect universal joints\n')
  case 4
    fprintf('- The joints on the fixed based are perfect spherical joints\n')
end

switch stewart.joints_M.type
  case 1
    fprintf('- The joints on the mobile based are universal joints\n')
  case 2
    fprintf('- The joints on the mobile based are spherical joints\n')
  case 3
    fprintf('- The joints on the mobile based are perfect universal joints\n')
  case 4
    fprintf('- The joints on the mobile based are perfect spherical joints\n')
end

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

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

fprintf('KINEMATICS:\n')

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

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

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

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

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

    stewart.struts_M.type = 1;

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

    stewart.struts_F.type = 1;

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

end
#+end_src

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

end
#+end_src

** =initializeSample=: Sample

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

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

    sample = struct();

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

        sample.H_offset = args.H_offset;

        sample.H = args.H;
        sample.R = args.R;
        sample.m = args.m;
    end
end
#+end_src

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

    arguments
        args.type char {mustBeMember(args.type,{'open-loop', 'iff'})} = 'open-loop'
    end

    controller = struct();

    switch args.type
      case 'open-loop'
        controller.type = 1;
        controller.name = 'Open-Loop';
      case 'iff'
        controller.type = 2;
        controller.name = 'Decentralized Integral Force Feedback';
    end
end
#+end_src

* Helping Functions                                                 :noexport:
** Initialize Path
#+NAME: m-init-path
#+BEGIN_SRC matlab
%% Path for functions, data and scripts
addpath('./matlab/mat/'); % Path for data
addpath('./matlab/src/'); % Path for functions
addpath('./matlab/subsystems/'); % Path for Subsystems Simulink files
addpath('./matlab/');     % Path for scripts
#+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('./subsystems/'); % Path for Subsystems Simulink files
#+END_SRC

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

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