#+TITLE: Subsystems used for the Simscape Models
:DRAWER:
#+STARTUP: overview

#+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="./css/htmlize.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/zenburn.css"/>
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/bootstrap.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/readtheorg.js"></script>

#+HTML_MATHJAX: align: center tagside: right font: TeX

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

#+PROPERTY: header-args:shell  :eval no-export

#+PROPERTY: header-args:latex  :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/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 raw replace :buffer no
#+PROPERTY: header-args:latex+ :eval no-export
#+PROPERTY: header-args:latex+ :exports both
#+PROPERTY: header-args:latex+ :mkdirp yes
#+PROPERTY: header-args:latex+ :output-dir figs
:END:

* Introduction                                                        :ignore:
The full Simscape Model is represented in Figure [[fig:simscape_picture]].

#+name: fig:simscape_picture
#+caption: Screenshot of the Multi-Body Model representation
[[file:figs/images/simscape_picture.png]]

This model is divided into multiple subsystems that are independent.
These subsystems are saved in separate files and imported in the main file using a block balled "subsystem reference".

Each stage is configured (geometry, mass properties, dynamic properties ...) using one function.

These functions are defined below.

* Simscape Configuration
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeSimscapeConfiguration.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeSimscapeConfiguration>>

** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  function [] = initializeSimscapeConfiguration(args)
#+end_src

** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  arguments
    args.gravity logical {mustBeNumericOrLogical} = true
  end
#+end_src

** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  conf_simscape = struct();
#+end_src

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

** Save the Structure
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  save('./mat/conf_simscape.mat', 'conf_simscape');
#+end_src

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

** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  function [] = initializeLoggingConfiguration(args)
#+end_src

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

** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  conf_log = struct();
#+end_src

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

** Sampling Time
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  conf_log.Ts = args.Ts;
#+end_src

** Save the Structure
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  save('./mat/conf_log.mat', 'conf_log');
#+end_src

* Ground
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeGround.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeGround>>

** Simscape Model
:PROPERTIES:
:UNNUMBERED: t
:END:

The model of the Ground is composed of:
- A *Cartesian* joint that is used to simulation the ground motion
- A solid that represents the ground on which the granite is located

#+name: fig:simscape_model_ground
#+attr_org: :width 800
#+caption: Simscape model for the Ground
[[file:figs/images/simscape_model_ground.png]]

#+name: fig:simscape_picture_ground
#+attr_org: :width 800
#+caption: Simscape picture for the Ground
[[file:figs/images/simscape_picture_ground.png]]

** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  function [ground] = initializeGround(args)
#+end_src

** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  arguments
    args.type char {mustBeMember(args.type,{'none', 'rigid'})} = 'rigid'
    args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) % Rotation point for the ground motion [m]
  end
#+end_src

** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
First, we initialize the =granite= structure.
#+begin_src matlab
  ground = struct();
#+end_src

** Add Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  switch args.type
    case 'none'
      ground.type = 0;
    case 'rigid'
      ground.type = 1;
  end
#+end_src

** Ground Solid properties
:PROPERTIES:
:UNNUMBERED: t
:END:
We set the shape and density of the ground solid element.
#+begin_src matlab
  ground.shape   = [2, 2, 0.5]; % [m]
  ground.density = 2800;        % [kg/m3]
#+end_src

** Rotation Point
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
  ground.rot_point = args.rot_point;
#+end_src

** Save the Structure
:PROPERTIES:
:UNNUMBERED: t
:END:
The =ground= structure is saved.
#+begin_src matlab
  save('./mat/stages.mat', 'ground', '-append');
#+end_src

* Granite
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeGranite.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeGranite>>

** Simscape Model
:PROPERTIES:
:UNNUMBERED: t
:END:

The Simscape model of the granite is composed of:
- A cartesian joint such that the granite can vibrations along x, y and z axis
- A solid

The output =sample_pos= corresponds to the impact point of the X-ray.

#+name: fig:simscape_model_granite
#+attr_org: :width 800
#+caption: Simscape model for the Granite
[[file:figs/images/simscape_model_granite.png]]

#+name: fig:simscape_picture_granite
#+attr_org: :width 800
#+caption: Simscape picture for the Granite
[[file:figs/images/simscape_picture_granite.png]]

** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  function [granite] = initializeGranite(args)
#+end_src

** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  arguments
    args.type          char    {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis', 'init'})} = 'flexible'
    args.Foffset       logical {mustBeNumericOrLogical} = false
    args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
    args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m]
    args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m]
    args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
  end
#+end_src


** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
First, we initialize the =granite= structure.
#+begin_src matlab
  granite = struct();
#+end_src

** Add Granite Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  switch args.type
    case 'none'
      granite.type = 0;
    case 'rigid'
      granite.type = 1;
    case 'flexible'
      granite.type = 2;
    case 'modal-analysis'
      granite.type = 3;
    case 'init'
      granite.type = 4;
  end
#+end_src

** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:

Properties of the Material and link to the geometry of the granite.
#+begin_src matlab
  granite.density = args.density; % [kg/m3]
  granite.STEP    = './STEPS/granite/granite.STEP';
#+end_src

Z-offset for the initial position of the sample with respect to the granite top surface.
#+begin_src matlab
  granite.sample_pos = 0.8; % [m]
#+end_src

** Stiffness and Damping properties
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
  granite.K = [4e9; 3e8; 8e8]; % [N/m]
  granite.C = [4.0e5; 1.1e5; 9.0e5]; % [N/(m/s)]
#+end_src

** Equilibrium position of the each joint.
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
    load('mat/Foffset.mat', 'Fgm');
    granite.Deq = -Fgm'./granite.K;
  else
    granite.Deq = zeros(6,1);
  end
#+end_src

** Save the Structure
:PROPERTIES:
:UNNUMBERED: t
:END:
The =granite= structure is saved.
#+begin_src matlab
  save('./mat/stages.mat', 'granite', '-append');
#+end_src

* Translation Stage
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeTy.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeTy>>

** Simscape Model
:PROPERTIES:
:UNNUMBERED: t
:END:

The Simscape model of the Translation stage consist of:
- One rigid body for the fixed part of the translation stage
- One rigid body for the moving part of the translation stage
- Four 6-DOF Joints that only have some rigidity in the X and Z directions.
  The rigidity in rotation comes from the fact that we use multiple joints that are located at different points
- One 6-DOF joint that represent the Actuator.
  It is used to impose the motion in the Y direction
- One 6-DOF joint to inject force disturbance in the X and Z directions

#+name: fig:simscape_model_ty
#+ATTR_ORG: :width 800
#+caption: Simscape model for the Translation Stage
[[file:figs/images/simscape_model_ty.png]]

#+name: fig:simscape_picture_ty
#+attr_org: :width 800
#+caption: Simscape picture for the Translation Stage
[[file:figs/images/simscape_picture_ty.png]]

** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  function [ty] = initializeTy(args)
#+end_src

** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  arguments
    args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
    args.Foffset logical {mustBeNumericOrLogical} = false
  end
#+end_src

** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
First, we initialize the =ty= structure.
#+begin_src matlab
  ty = struct();
#+end_src

** Add Translation Stage Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  switch args.type
    case 'none'
      ty.type = 0;
    case 'rigid'
      ty.type = 1;
    case 'flexible'
      ty.type = 2;
    case 'modal-analysis'
      ty.type = 3;
    case 'init'
      ty.type = 4;
  end
#+end_src

** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:
Define the density of the materials as well as the geometry (STEP files).
#+begin_src matlab
  % Ty Granite frame
  ty.granite_frame.density = 7800; % [kg/m3] => 43kg
  ty.granite_frame.STEP    = './STEPS/Ty/Ty_Granite_Frame.STEP';

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

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

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

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

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

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

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

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

** Stiffness and Damping properties
:PROPERTIES:
:UNNUMBERED: t
:END:

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

** Equilibrium position of the each joint.
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
    load('mat/Foffset.mat', 'Ftym');
    ty.Deq = -Ftym'./ty.K;
  else
    ty.Deq = zeros(6,1);
  end
#+end_src

** Save the Structure
:PROPERTIES:
:UNNUMBERED: t
:END:
The =ty= structure is saved.
#+begin_src matlab
  save('./mat/stages.mat', 'ty', '-append');
#+end_src

* Tilt Stage
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeRy.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeRy>>

** Simscape Model
:PROPERTIES:
:UNNUMBERED: t
:END:

The Simscape model of the Tilt stage is composed of:
- Two solid bodies for the two part of the stage
- *Four* 6-DOF joints to model the flexibility of the stage.
  These joints are virtually located along the rotation axis and are connecting the two solid bodies.
  These joints have some translation stiffness in the u-v-w directions aligned with the joint.
  The stiffness in rotation between the two solids is due to the fact that the 4 joints are connecting the two solids are different locations
- A Bushing Joint used for the Actuator.
  The Ry motion is imposed by the input.

#+name: fig:simscape_model_ry
#+attr_org: :width 800
#+caption: Simscape model for the Tilt Stage
[[file:figs/images/simscape_model_ry.png]]

#+name: fig:simscape_picture_ry
#+attr_org: :width 800
#+caption: Simscape picture for the Tilt Stage
[[file:figs/images/simscape_picture_ry.png]]

** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  function [ry] = initializeRy(args)
#+end_src

** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  arguments
    args.type          char    {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
    args.Foffset       logical {mustBeNumericOrLogical} = false
    args.Ry_init (1,1) double  {mustBeNumeric} = 0
  end
#+end_src

** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
First, we initialize the =ry= structure.
#+begin_src matlab
  ry = struct();
#+end_src


** Add Tilt Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  switch args.type
    case 'none'
      ry.type = 0;
    case 'rigid'
      ry.type = 1;
    case 'flexible'
      ry.type = 2;
    case 'modal-analysis'
      ry.type = 3;
    case 'init'
      ry.type = 4;
  end
#+end_src

** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:
Properties of the Material and link to the geometry of the Tilt stage.
#+begin_src matlab
  % Ry - Guide for the tilt stage
  ry.guide.density = 7800; % [kg/m3]
  ry.guide.STEP    = './STEPS/ry/Tilt_Guide.STEP';

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

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

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

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

#+begin_src matlab
  ry.Ry_init = args.Ry_init; % [rad]
#+end_src

** Stiffness and Damping properties
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
  ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
  ry.C = [1e5;   1e5; 1e5;   3e4;   1e3; 3e4];
#+end_src

** Equilibrium position of the each joint.
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
    load('mat/Foffset.mat', 'Fym');
    ry.Deq = -Fym'./ry.K;
  else
    ry.Deq = zeros(6,1);
  end
#+end_src

** Save the Structure
:PROPERTIES:
:UNNUMBERED: t
:END:
The =ry= structure is saved.
#+begin_src matlab
  save('./mat/stages.mat', 'ry', '-append');
#+end_src

* Spindle
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeRz.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeRz>>

** Simscape Model
:PROPERTIES:
:UNNUMBERED: t
:END:

The Simscape model of the Spindle is composed of:
- Two rigid bodies: the stator and the rotor
- A Bushing Joint that is used both as the actuator (the Rz motion is imposed by the input) and as the force perturbation in the Z direction.
- The Bushing joint has some flexibility in the X-Y-Z directions as well as in Rx and Ry rotations

#+name: fig:simscape_model_rz
#+attr_org: :width 800
#+caption: Simscape model for the Spindle
[[file:figs/images/simscape_model_rz.png]]

#+name: fig:simscape_picture_rz
#+attr_org: :width 800
#+caption: Simscape picture for the Spindle
[[file:figs/images/simscape_picture_rz.png]]

** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  function [rz] = initializeRz(args)
#+end_src

** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  arguments
    args.type    char    {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
    args.Foffset logical {mustBeNumericOrLogical} = false
  end
#+end_src

** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
First, we initialize the =rz= structure.
#+begin_src matlab
  rz = struct();
#+end_src

** Add Spindle Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  switch args.type
    case 'none'
      rz.type = 0;
    case 'rigid'
      rz.type = 1;
    case 'flexible'
      rz.type = 2;
    case 'modal-analysis'
      rz.type = 3;
    case 'init'
      rz.type = 4;
  end
#+end_src

** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:

Properties of the Material and link to the geometry of the spindle.
#+begin_src matlab
  % Spindle - Slip Ring
  rz.slipring.density = 7800; % [kg/m3]
  rz.slipring.STEP    = './STEPS/rz/Spindle_Slip_Ring.STEP';

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

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

** Stiffness and Damping properties
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
  rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
  rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];
#+end_src

** Equilibrium position of the each joint.
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
    load('mat/Foffset.mat', 'Fzm');
    rz.Deq = -Fzm'./rz.K;
  else
    rz.Deq = zeros(6,1);
  end
#+end_src

** Save the Structure
:PROPERTIES:
:UNNUMBERED: t
:END:
The =rz= structure is saved.
#+begin_src matlab
  save('./mat/stages.mat', 'rz', '-append');
#+end_src

* Micro Hexapod
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeMicroHexapod.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeMicroHexapod>>

** Simscape Model
:PROPERTIES:
:UNNUMBERED: t
:END:

#+name: fig:simscape_model_micro_hexapod
#+attr_org: :width 800
#+caption: Simscape model for the Micro-Hexapod
[[file:figs/images/simscape_model_micro_hexapod.png]]

#+name: fig:simscape_picture_micro_hexapod
#+attr_org: :width 800
#+caption: Simscape picture for the Micro-Hexapod
[[file:figs/images/simscape_picture_micro_hexapod.png]]

** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  function [micro_hexapod] = initializeMicroHexapod(args)
#+end_src

** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  arguments
      args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
      % initializeFramesPositions
      args.H    (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
      args.MO_B (1,1) double {mustBeNumeric} = 270e-3
      % generateGeneralConfiguration
      args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
      args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-3
      args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
      args.MH  (1,1) double {mustBeNumeric, mustBePositive} = 45e-3
      args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 118e-3
      args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
      % initializeStrutDynamics
      args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7*ones(6,1)
      args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3*ones(6,1)
      % initializeCylindricalPlatforms
      args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10
      args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
      args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3
      args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10
      args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
      args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
      % initializeCylindricalStruts
      args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1
      args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
      args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
      args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1
      args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
      args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
      % inverseKinematics
      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
      args.ARB (3,3) double {mustBeNumeric} = eye(3)
      % Force that stiffness of each joint should apply at t=0
      args.Foffset      logical {mustBeNumericOrLogical} = false
  end
#+end_src

** Function content
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  micro_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
  micro_hexapod = generateGeneralConfiguration(micro_hexapod, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh);
  micro_hexapod = computeJointsPose(micro_hexapod);
  micro_hexapod = initializeStrutDynamics(micro_hexapod, 'Ki', args.Ki, 'Ci', args.Ci);
  micro_hexapod = initializeCylindricalPlatforms(micro_hexapod, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
  micro_hexapod = initializeCylindricalStruts(micro_hexapod, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
  micro_hexapod = computeJacobian(micro_hexapod);
  [Li, dLi] = inverseKinematics(micro_hexapod, 'AP', args.AP, 'ARB', args.ARB);
  micro_hexapod.Li = Li;
  micro_hexapod.dLi = dLi;
#+end_src

Equilibrium position of the each joint.
#+begin_src matlab
  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
    load('mat/Foffset.mat', 'Fhm');
    micro_hexapod.dLeq = -Fhm'./args.Ki;
  else
    micro_hexapod.dLeq = zeros(6,1);
  end
#+end_src

** Add Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  switch args.type
    case 'none'
      micro_hexapod.type = 0;
    case 'rigid'
      micro_hexapod.type = 1;
    case 'flexible'
      micro_hexapod.type = 2;
    case 'modal-analysis'
      micro_hexapod.type = 3;
    case 'init'
      micro_hexapod.type = 4;
  end
#+end_src

** Save the Structure
:PROPERTIES:
:UNNUMBERED: t
:END:
The =micro_hexapod= structure is saved.
#+begin_src matlab
  save('./mat/stages.mat', 'micro_hexapod', '-append');
#+end_src

* Center of gravity compensation
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeAxisc.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeAxisc>>

** Simscape Model
:PROPERTIES:
:UNNUMBERED: t
:END:

The Simscape model of the Center of gravity compensator is composed of:
- One main solid that is connected to two other solids (the masses to position of center of mass) through two revolute joints
- The angle of both revolute joints is set by the input

#+name: fig:simscape_model_axisc
#+attr_org: :width 800
#+caption: Simscape model for the Center of Mass compensation system
[[file:figs/images/simscape_model_axisc.png]]

#+name: fig:simscape_picture_axisc
#+attr_org: :width 800
#+caption: Simscape picture for the Center of Mass compensation system
[[file:figs/images/simscape_picture_axisc.png]]

** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  function [axisc] = initializeAxisc(args)
#+end_src

** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  arguments
    args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
  end
#+end_src

** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
First, we initialize the =axisc= structure.
#+begin_src matlab
  axisc = struct();
#+end_src

** Add Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  switch args.type
    case 'none'
      axisc.type = 0;
    case 'rigid'
      axisc.type = 1;
    case 'flexible'
      axisc.type = 2;
  end
#+end_src

** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:

Properties of the Material and link to the geometry files.
#+begin_src matlab
  % Structure
  axisc.structure.density = 3400; % [kg/m3]
  axisc.structure.STEP    = './STEPS/axisc/axisc_structure.STEP';

  % Wheel
  axisc.wheel.density     = 2700; % [kg/m3]
  axisc.wheel.STEP        = './STEPS/axisc/axisc_wheel.STEP';

  % Mass
  axisc.mass.density      = 7800; % [kg/m3]
  axisc.mass.STEP         = './STEPS/axisc/axisc_mass.STEP';

  % Gear
  axisc.gear.density      = 7800; % [kg/m3]
  axisc.gear.STEP         = './STEPS/axisc/axisc_gear.STEP';
#+end_src

** Save the Structure
:PROPERTIES:
:UNNUMBERED: t
:END:
The =axisc= structure is saved.
#+begin_src matlab
  save('./mat/stages.mat', 'axisc', '-append');
#+end_src

* Mirror
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeMirror.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeMirror>>

** Simscape Model
:PROPERTIES:
:UNNUMBERED: t
:END:

The Simscape Model of the mirror is just a solid body.
The output =mirror_center= corresponds to the center of the Sphere and is the point of measurement for the metrology

#+name: fig:simscape_model_mirror
#+attr_org: :width 800
#+caption: Simscape model for the Mirror
[[file:figs/images/simscape_model_mirror.png]]

#+name: fig:simscape_picture_mirror
#+attr_org: :width 800
#+caption: Simscape picture for the Mirror
[[file:figs/images/simscape_picture_mirror.png]]

** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  function [] = initializeMirror(args)
#+end_src

** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  arguments
    args.type        char   {mustBeMember(args.type,{'none', 'rigid'})} = 'rigid'
    args.shape       char   {mustBeMember(args.shape,{'spherical', 'conical'})} = 'spherical'
    args.angle (1,1) double {mustBeNumeric, mustBePositive} = 45 % [deg]
  end
#+end_src

** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
First, we initialize the =mirror= structure.
#+begin_src matlab
  mirror = struct();
#+end_src

** Add Mirror Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  switch args.type
    case 'none'
      mirror.type = 0;
    case 'rigid'
      mirror.type = 1;
  end
#+end_src

** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:

We define the geometrical values.
#+begin_src matlab
  mirror.h = 0.05; % Height of the mirror [m]

  mirror.thickness = 0.025; % Thickness of the plate supporting the sample [m]

  mirror.hole_rad = 0.120; % radius of the hole in the mirror [m]

  mirror.support_rad = 0.1; % radius of the support plate [m]

  % point of interest offset in z (above the top surfave) [m]
  switch args.type
    case 'none'
      mirror.jacobian = 0.20;
    case 'rigid'
      mirror.jacobian = 0.20 - mirror.h;
  end

  mirror.rad = 0.180; % radius of the mirror (at the bottom surface) [m]
#+end_src

#+begin_src matlab
  mirror.density = 2400; % Density of the material [kg/m3]
#+end_src

#+begin_src matlab
  mirror.cone_length = mirror.rad*tand(args.angle)+mirror.h+mirror.jacobian; % Distance from Apex point of the cone to jacobian point
#+end_src

Now we define the Shape of the mirror.
We first start with the internal part.
#+begin_src matlab
  mirror.shape = [...
      0 mirror.h-mirror.thickness
      mirror.hole_rad mirror.h-mirror.thickness; ...
      mirror.hole_rad 0; ...
      mirror.rad 0 ...
  ];
#+end_src

Then, we define the reflective used part of the mirror.
#+begin_src matlab
  if strcmp(args.shape, 'spherical')
      mirror.sphere_radius = sqrt((mirror.jacobian+mirror.h)^2+mirror.rad^2); % Radius of the sphere [mm]

      for z = linspace(0, mirror.h, 101)
          mirror.shape = [mirror.shape; sqrt(mirror.sphere_radius^2-(z-mirror.jacobian-mirror.h)^2) z];
      end
  elseif strcmp(args.shape, 'conical')
      mirror.shape = [mirror.shape; mirror.rad+mirror.h/tand(args.angle) mirror.h];
  else
      error('Shape should be either conical or spherical');
  end
#+end_src

Finally, we close the shape.
#+begin_src matlab
  mirror.shape = [mirror.shape; 0 mirror.h];
#+end_src

** Save the Structure
:PROPERTIES:
:UNNUMBERED: t
:END:
The =mirror= structure is saved.
#+begin_src matlab
  save('./mat/stages.mat', 'mirror', '-append');
#+end_src

* Nano Hexapod
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeNanoHexapod.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeNanoHexapod>>

** Simscape Model
:PROPERTIES:
:UNNUMBERED: t
:END:
#+name: fig:simscape_model_nano_hexapod
#+attr_org: :width 800
#+caption: Simscape model for the Nano Hexapod
[[file:figs/images/simscape_model_nano_hexapod.png]]

#+name: fig:simscape_picture_nano_hexapod
#+attr_org: :width 800
#+caption: Simscape picture for the Nano Hexapod
[[file:figs/images/simscape_picture_nano_hexapod.png]]

** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  function [nano_hexapod] = initializeNanoHexapod(args)
#+end_src

** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  arguments
      args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
      % initializeFramesPositions
      args.H    (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
      args.MO_B (1,1) double {mustBeNumeric} = 175e-3
      % generateGeneralConfiguration
      args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
      args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
      args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
      args.MH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
      args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
      args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
      % initializeStrutDynamics
      args.actuator  char   {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo'
      % initializeCylindricalPlatforms
      args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
      args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
      args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-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
      % initializeCylindricalStruts
      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
      % inverseKinematics
      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
      args.ARB (3,3) double {mustBeNumeric} = eye(3)
      % Equilibrium position of each leg
      args.dLeq (6,1) double {mustBeNumeric} = zeros(6,1)
  end
#+end_src

** Function content
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  nano_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
  nano_hexapod = generateGeneralConfiguration(nano_hexapod, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh);
  nano_hexapod = computeJointsPose(nano_hexapod);
  if strcmp(args.actuator, 'piezo')
      nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', 1e7*ones(6,1), 'Ci', 1e2*ones(6,1));
  elseif strcmp(args.actuator, 'lorentz')
      nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', 1e4*ones(6,1), 'Ci', 1e2*ones(6,1));
  else
      error('args.actuator should be piezo or lorentz');
  end
  nano_hexapod = initializeCylindricalPlatforms(nano_hexapod, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
  nano_hexapod = initializeCylindricalStruts(nano_hexapod, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
  nano_hexapod = computeJacobian(nano_hexapod);
  [Li, dLi] = inverseKinematics(nano_hexapod, 'AP', args.AP, 'ARB', args.ARB);
  nano_hexapod.Li = Li;
  nano_hexapod.dLi = dLi;
#+end_src

#+begin_src matlab
  nano_hexapod.dLeq = args.dLeq;
#+end_src

** Add Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  switch args.type
    case 'none'
      nano_hexapod.type = 0;
    case 'rigid'
      nano_hexapod.type = 1;
    case 'flexible'
      nano_hexapod.type = 2;
  end
#+end_src

** Save the Structure
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  save('./mat/stages.mat', 'nano_hexapod', '-append');
#+end_src

* Sample
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeSample.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeSample>>

** Simscape Model
:PROPERTIES:
:UNNUMBERED: t
:END:

The Simscape model of the sample environment is composed of:
- A rigid transform that can be used to translate the sample (position offset)
- A cartesian joint to add some flexibility to the sample environment mount
- A solid that represent the sample
- An input is added to apply some external forces and torques at the center of the sample environment.
  This could be the case for cable forces for instance.

#+name: fig:simscape_model_sample
#+attr_org: :width 800
#+caption: Simscape model for the Sample
[[file:figs/images/simscape_model_sample.png]]

#+name: fig:simscape_picture_sample
#+attr_org: :width 800
#+caption: Simscape picture for the Sample
[[file:figs/images/simscape_picture_sample.png]]

** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  function [sample] = initializeSample(args)
#+end_src

** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  arguments
    args.type         char    {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'init'})} = 'flexible'
    args.radius (1,1) double  {mustBeNumeric, mustBePositive} = 0.1 % [m]
    args.height (1,1) double  {mustBeNumeric, mustBePositive} = 0.3 % [m]
    args.mass   (1,1) double  {mustBeNumeric, mustBePositive} = 50 % [kg]
    args.freq   (1,1) double  {mustBeNumeric, mustBePositive} = 100 % [Hz]
    args.offset (1,1) double  {mustBeNumeric} = 0 % [m]
    args.Foffset      logical {mustBeNumericOrLogical} = false
  end
#+end_src

** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
First, we initialize the =sample= structure.
#+begin_src matlab
  sample = struct();
#+end_src

** Add Sample Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  switch args.type
    case 'none'
      sample.type = 0;
    case 'rigid'
      sample.type = 1;
    case 'flexible'
      sample.type = 2;
    case 'init'
      sample.type = 3;
  end
#+end_src

** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:

We define the geometrical parameters of the sample as well as its mass and position.
#+begin_src matlab
  sample.radius = args.radius; % [m]
  sample.height = args.height; % [m]
  sample.mass   = args.mass; % [kg]
  sample.offset = args.offset; % [m]
#+end_src

** Stiffness and Damping properties
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
  sample.K = ones(3,1) * sample.mass * (2*pi * args.freq)^2; % [N/m]
  sample.C = 0.1 * sqrt(sample.K*sample.mass); % [N/(m/s)]
#+end_src

** Equilibrium position of the each joint.
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
    load('mat/Foffset.mat', 'Fsm');
    sample.Deq = -Fsm'./sample.K;
  else
    sample.Deq = zeros(3,1);
  end
#+end_src

** Save the Structure
:PROPERTIES:
:UNNUMBERED: t
:END:
The =sample= structure is saved.
#+begin_src matlab
  save('./mat/stages.mat', 'sample', '-append');
#+end_src

* Initialize Controller
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeController.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeController>>

** Function Declaration and Documentation
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  function [] = initializeController(args)
#+end_src

** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  arguments
    args.type char {mustBeMember(args.type,{'open-loop', 'iff', 'dvf', 'hac-dvf', 'ref-track-L', 'ref-track-iff-L', 'cascade-hac-lac', 'hac-iff'})} = 'open-loop'
  end
#+end_src

** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
First, we initialize the =controller= structure.
#+begin_src matlab
  controller = struct();
#+end_src

** Controller Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  switch args.type
    case 'open-loop'
      controller.type = 1;
    case 'dvf'
      controller.type = 2;
    case 'iff'
      controller.type = 3;
    case 'hac-dvf'
      controller.type = 4;
    case 'ref-track-L'
      controller.type = 5;
    case 'ref-track-iff-L'
      controller.type = 6;
    case 'cascade-hac-lac'
      controller.type = 7;
    case 'hac-iff'
      controller.type = 8;
  end
#+end_src

** Save the Structure
:PROPERTIES:
:UNNUMBERED: t
:END:
The =controller= structure is saved.
#+begin_src matlab
  save('./mat/controller.mat', 'controller');
#+end_src

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

** Function Declaration and Documentation
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  function [ref] = initializeReferences(args)
#+end_src

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


** Initialize Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  %% Set Sampling Time
  Ts = args.Ts;
  Tmax = args.Tmax;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

** Axis Compensation
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  %% Axis Compensation - Rm
  t = [0, Ts];

  Rm = [args.Rm_pos, args.Rm_pos];
  Rm = struct('time', t, 'signals', struct('values', Rm));
#+end_src

** Nano Hexapod
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  %% Nano-Hexapod
  t = [0, Ts];
  Dn = zeros(length(t), 6);

  switch args.Dn_type
    case 'constant'
      Dn = [args.Dn_pos, args.Dn_pos];

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

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

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

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

      [~, Dnl] = inverseKinematics(nano_hexapod, 'AP', AP, 'ARB', ARB);
      Dnl = [Dnl, Dnl];
    otherwise
      warning('Dn_type is not set correctly');
  end

  Dn = struct('time', t, 'signals', struct('values', Dn));
  Dnl = struct('time', t, 'signals', struct('values', Dnl));
#+end_src

** Save
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
      %% Save
      save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'Ts');
  end
#+end_src

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

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

#+end_src

** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  arguments
      % Global parameter to enable or disable the disturbances
      args.enable logical {mustBeNumericOrLogical} = true
      % Ground Motion - X direction
      args.Dwx logical {mustBeNumericOrLogical} = true
      % Ground Motion - Y direction
      args.Dwy logical {mustBeNumericOrLogical} = true
      % Ground Motion - Z direction
      args.Dwz logical {mustBeNumericOrLogical} = true
      % Translation Stage - X direction
      args.Fty_x logical {mustBeNumericOrLogical} = true
      % Translation Stage - Z direction
      args.Fty_z logical {mustBeNumericOrLogical} = true
      % Spindle - Z direction
      args.Frz_z logical {mustBeNumericOrLogical} = true
  end
#+end_src


** Load Data
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  load('./mat/dist_psd.mat', 'dist_f');
#+end_src

We remove the first frequency point that usually is very large.
#+begin_src matlab :exports none
  dist_f.f = dist_f.f(2:end);
  dist_f.psd_gm = dist_f.psd_gm(2:end);
  dist_f.psd_ty = dist_f.psd_ty(2:end);
  dist_f.psd_rz = dist_f.psd_rz(2:end);
#+end_src

** Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
We define some parameters that will be used in the algorithm.
#+begin_src matlab
  Fs = 2*dist_f.f(end);      % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
  N  = 2*length(dist_f.f);   % Number of Samples match the one of the wanted PSD
  T0 = N/Fs;                 % Signal Duration [s]
  df = 1/T0;                 % Frequency resolution of the DFT [Hz]
                             % Also equal to (dist_f.f(2)-dist_f.f(1))
  t = linspace(0, T0, N+1)'; % Time Vector [s]
  Ts = 1/Fs;                 % Sampling Time [s]
#+end_src

** Ground Motion
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  phi = dist_f.psd_gm;
  C = zeros(N/2,1);
  for i = 1:N/2
    C(i) = sqrt(phi(i)*df);
  end
#+end_src

#+begin_src matlab
  if args.Dwx && args.enable
    rng(111);
    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
    Cx = [Cx; flipud(conj(Cx(2:end)))];;
    Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
  else
    Dwx = zeros(length(t), 1);
  end
#+end_src

#+begin_src matlab
  if args.Dwy && args.enable
    rng(112);
    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
    Cx = [Cx; flipud(conj(Cx(2:end)))];;
    Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
  else
    Dwy = zeros(length(t), 1);
  end
#+end_src

#+begin_src matlab
  if args.Dwy && args.enable
    rng(113);
    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
    Cx = [Cx; flipud(conj(Cx(2:end)))];;
    Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
  else
    Dwz = zeros(length(t), 1);
  end
#+end_src

** Translation Stage - X direction
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  if args.Fty_x && args.enable
    phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
    C = zeros(N/2,1);
    for i = 1:N/2
      C(i) = sqrt(phi(i)*df);
    end
    rng(121);
    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
    Cx = [Cx; flipud(conj(Cx(2:end)))];;
    u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
    Fty_x = u;
  else
    Fty_x = zeros(length(t), 1);
  end
#+end_src

** Translation Stage - Z direction
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  if args.Fty_z && args.enable
    phi = dist_f.psd_ty;
    C = zeros(N/2,1);
    for i = 1:N/2
      C(i) = sqrt(phi(i)*df);
    end
    rng(122);
    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
    Cx = [Cx; flipud(conj(Cx(2:end)))];;
    u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
    Fty_z = u;
  else
    Fty_z = zeros(length(t), 1);
  end
#+end_src

** Spindle - Z direction
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  if args.Frz_z && args.enable
    phi = dist_f.psd_rz;
    C = zeros(N/2,1);
    for i = 1:N/2
      C(i) = sqrt(phi(i)*df);
    end
    rng(131);
    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
    Cx = [Cx; flipud(conj(Cx(2:end)))];;
    u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
    Frz_z = u;
  else
    Frz_z = zeros(length(t), 1);
  end
#+end_src

** Direct Forces
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  u = zeros(length(t), 6);
  Fd = u;
#+end_src

** Set initial value to zero
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  Dwx    = Dwx   - Dwx(1);
  Dwy    = Dwy   - Dwy(1);
  Dwz    = Dwz   - Dwz(1);
  Fty_x  = Fty_x - Fty_x(1);
  Fty_z  = Fty_z - Fty_z(1);
  Frz_z  = Frz_z - Frz_z(1);
#+end_src

** Save
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  save('./mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't');
#+end_src

* Initialize Position Errors
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializePosError.m
:header-args:matlab+: :comments none :mkdirp yes
:header-args:matlab+: :eval no :results none
:END:
<<sec:initializePosError>>

** Function Declaration and Documentation
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  function [] = initializePosError(args)
  % initializePosError - Initialize the position errors
  %
  % Syntax: [] = initializePosError(args)
  %
  % Inputs:
  %    - args -

#+end_src

** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  arguments
      args.error    logical {mustBeNumericOrLogical} = false
      args.Dy (1,1) double  {mustBeNumeric} = 0 % [m]
      args.Ry (1,1) double  {mustBeNumeric} = 0 % [m]
      args.Rz (1,1) double  {mustBeNumeric} = 0 % [m]
  end
#+end_src

** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
First, we initialize the =pos_error= structure.
#+begin_src matlab
  pos_error = struct();
#+end_src

** Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  if args.error
    pos_error.type = 1;
  else
    pos_error.type = 0;
  end
#+end_src

** Position Errors
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  pos_error.Dy = args.Dy;
  pos_error.Ry = args.Ry;
  pos_error.Rz = args.Rz;
#+end_src

** Save
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  save('./mat/pos_error.mat', 'pos_error');
#+end_src

* Z-Axis Geophone
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeZAxisGeophone.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeZAxisGeophone>>

#+begin_src matlab
  function [geophone] = initializeZAxisGeophone(args)
      arguments
          args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
          args.freq (1,1) double {mustBeNumeric, mustBePositive} = 1    % [Hz]
      end

      %%
      geophone.m = args.mass;

      %% The Stiffness is set to have the damping resonance frequency
      geophone.k = geophone.m * (2*pi*args.freq)^2;

      %% We set the damping value to have critical damping
      geophone.c = 2*sqrt(geophone.m * geophone.k);

      %% Save
      save('./mat/geophone_z_axis.mat', 'geophone');
  end
#+end_src

* Z-Axis Accelerometer
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeZAxisAccelerometer.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeZAxisAccelerometer>>

#+begin_src matlab
  function [accelerometer] = initializeZAxisAccelerometer(args)
      arguments
          args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
          args.freq (1,1) double {mustBeNumeric, mustBePositive} = 5e3  % [Hz]
      end

      %%
      accelerometer.m = args.mass;

      %% The Stiffness is set to have the damping resonance frequency
      accelerometer.k = accelerometer.m * (2*pi*args.freq)^2;

      %% We set the damping value to have critical damping
      accelerometer.c = 2*sqrt(accelerometer.m * accelerometer.k);

      %% Gain correction of the accelerometer to have a unity gain until the resonance
      accelerometer.gain = -accelerometer.k/accelerometer.m;

      %% Save
      save('./mat/accelerometer_z_axis.mat', 'accelerometer');
  end
#+end_src
* Old functions                                                     :noexport:
** Micro Hexapod
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeMicroHexapodOld.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeMicroHexapod>>

#+begin_src matlab
  function [micro_hexapod] = initializeMicroHexapod(args)
      arguments
          args.rigid logical {mustBeNumericOrLogical} = false
          args.AP    (3,1) double {mustBeNumeric} = zeros(3,1)
          args.ARB   (3,3) double {mustBeNumeric} = eye(3)
      end

      %% Stewart Object
      micro_hexapod = struct();
      micro_hexapod.h        = 350; % Total height of the platform [mm]
      micro_hexapod.jacobian = 270; % Distance from the top of the mobile platform to the Jacobian point [mm]

      %% Bottom Plate - Mechanical Design
      BP = struct();

      BP.rad.int   = 110;   % Internal Radius [mm]
      BP.rad.ext   = 207.5; % External Radius [mm]
      BP.thickness = 26;    % Thickness [mm]
      BP.leg.rad   = 175.5; % Radius where the legs articulations are positionned [mm]
      BP.leg.ang   = 9.5;   % Angle Offset [deg]
      BP.density   = 8000;  % Density of the material [kg/m^3]
      BP.color     = [0.6 0.6 0.6]; % Color [rgb]
      BP.shape     = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness];

      %% Top Plate - Mechanical Design
      TP = struct();

      TP.rad.int   = 82;   % Internal Radius [mm]
      TP.rad.ext   = 150;  % Internal Radius [mm]
      TP.thickness = 26;   % Thickness [mm]
      TP.leg.rad   = 118;  % Radius where the legs articulations are positionned [mm]
      TP.leg.ang   = 12.1; % Angle Offset [deg]
      TP.density   = 8000; % Density of the material [kg/m^3]
      TP.color     = [0.6 0.6 0.6]; % Color [rgb]
      TP.shape     = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness];

      %% Struts
      Leg = struct();

      Leg.stroke     = 10e-3; % Maximum Stroke of each leg [m]
      if args.rigid
        Leg.k.ax     = 1e12; % Stiffness of each leg [N/m]
      else
        Leg.k.ax     = 2e7; % Stiffness of each leg [N/m]
      end
      Leg.ksi.ax     = 0.1;   % Modal damping ksi = 1/2*c/sqrt(km) []
      Leg.rad.bottom = 25;    % Radius of the cylinder of the bottom part [mm]
      Leg.rad.top    = 17;    % Radius of the cylinder of the top part [mm]
      Leg.density    = 8000;  % Density of the material [kg/m^3]
      Leg.color.bottom  = [0.5 0.5 0.5]; % Color [rgb]
      Leg.color.top     = [0.5 0.5 0.5]; % Color [rgb]

      Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm]
      Leg.sphere.top    = Leg.rad.top; % Size of the sphere at the end of the leg [mm]
      Leg.m             = TP.density*((pi*(TP.rad.ext/1000)^2)*(TP.thickness/1000)-(pi*(TP.rad.int/1000^2))*(TP.thickness/1000))/6; % TODO [kg]
      Leg = updateDamping(Leg);


      %% Sphere
      SP = struct();

      SP.height.bottom  = 27; % [mm]
      SP.height.top     = 27; % [mm]
      SP.density.bottom = 8000; % [kg/m^3]
      SP.density.top    = 8000; % [kg/m^3]
      SP.color.bottom   = [0.6 0.6 0.6]; % [rgb]
      SP.color.top      = [0.6 0.6 0.6]; % [rgb]
      SP.k.ax           = 0; % [N*m/deg]
      SP.ksi.ax         = 10;

      SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm]
      SP.thickness.top    = SP.height.top-Leg.sphere.top; % [mm]
      SP.rad.bottom       = Leg.sphere.bottom; % [mm]
      SP.rad.top          = Leg.sphere.top; % [mm]
      SP.m                = SP.density.bottom*2*pi*((SP.rad.bottom*1e-3)^2)*(SP.height.bottom*1e-3); % TODO [kg]

      SP = updateDamping(SP);

      %%
      Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom];
      Leg.support.top    = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top];

      %%
      micro_hexapod.BP = BP;
      micro_hexapod.TP = TP;
      micro_hexapod.Leg = Leg;
      micro_hexapod.SP = SP;

      %%
      micro_hexapod = initializeParameters(micro_hexapod);

      %% Setup equilibrium position of each leg
      micro_hexapod.L0 = inverseKinematicsHexapod(micro_hexapod, args.AP, args.ARB);

      %% Save
      save('./mat/stages.mat', 'micro_hexapod', '-append');

      %%
      function [element] = updateDamping(element)
        field = fieldnames(element.k);
        for i = 1:length(field)
          element.c.(field{i}) = 2*element.ksi.(field{i})*sqrt(element.k.(field{i})*element.m);
        end
      end

      %%
      function [stewart] = initializeParameters(stewart)
          %% Connection points on base and top plate w.r.t. World frame at the center of the base plate
          stewart.pos_base = zeros(6, 3);
          stewart.pos_top = zeros(6, 3);

          alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
          alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top

          height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001; % TODO

          radius_b = stewart.BP.leg.rad*0.001; % rayon emplacement support base
          radius_t = stewart.TP.leg.rad*0.001; % top radius in meters

          for i = 1:3
            % base points
            angle_m_b = (2*pi/3)* (i-1) - alpha_b;
            angle_p_b = (2*pi/3)* (i-1) + alpha_b;
            stewart.pos_base(2*i-1,:) =  [radius_b*cos(angle_m_b), radius_b*sin(angle_m_b), 0.0];
            stewart.pos_base(2*i,:) = [radius_b*cos(angle_p_b), radius_b*sin(angle_p_b), 0.0];

            % top points
            % Top points are 60 degrees offset
            angle_m_t = (2*pi/3)* (i-1) - alpha_t + 2*pi/6;
            angle_p_t = (2*pi/3)* (i-1) + alpha_t + 2*pi/6;
            stewart.pos_top(2*i-1,:) = [radius_t*cos(angle_m_t), radius_t*sin(angle_m_t), height];
            stewart.pos_top(2*i,:) = [radius_t*cos(angle_p_t), radius_t*sin(angle_p_t), height];
          end

          % permute pos_top points so that legs are end points of base and top points
          stewart.pos_top = [stewart.pos_top(6,:); stewart.pos_top(1:5,:)]; %6th point on top connects to 1st on bottom
          stewart.pos_top_tranform = stewart.pos_top - height*[zeros(6, 2),ones(6, 1)];

          %% leg vectors
          legs = stewart.pos_top - stewart.pos_base;
          leg_length = zeros(6, 1);
          leg_vectors = zeros(6, 3);
          for i = 1:6
            leg_length(i) = norm(legs(i,:));
            leg_vectors(i,:)  = legs(i,:) / leg_length(i);
          end

          stewart.Leg.lenght = 1000*leg_length(1)/1.5;
          stewart.Leg.shape.bot = [0 0; ...
                                  stewart.Leg.rad.bottom 0; ...
                                  stewart.Leg.rad.bottom stewart.Leg.lenght; ...
                                  stewart.Leg.rad.top stewart.Leg.lenght; ...
                                  stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
                                  0 0.2*stewart.Leg.lenght];

          %% Calculate revolute and cylindrical axes
          rev1 = zeros(6, 3);
          rev2 = zeros(6, 3);
          cyl1 = zeros(6, 3);
          for i = 1:6
            rev1(i,:) = cross(leg_vectors(i,:), [0 0 1]);
            rev1(i,:) = rev1(i,:) / norm(rev1(i,:));

            rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:));
            rev2(i,:) = rev2(i,:) / norm(rev2(i,:));

            cyl1(i,:) = leg_vectors(i,:);
          end


          %% Coordinate systems
          stewart.lower_leg = struct('rotation', eye(3));
          stewart.upper_leg = struct('rotation', eye(3));

          for i = 1:6
            stewart.lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
            stewart.upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
          end

          %% Position Matrix
          stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.thickness+stewart.Leg.sphere.top+stewart.SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];

          %% Compute Jacobian Matrix
          aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
          stewart.J  = getJacobianMatrix(leg_vectors', aa');
      end

      %%
      function J  = getJacobianMatrix(RM, M_pos_base)
          % RM: [3x6] unit vector of each leg in the fixed frame
          % M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
          J = zeros(6);
          J(:, 1:3) = RM';
          J(:, 4:6) = cross(M_pos_base, RM)';
      end
  end
#+end_src
** Cedrat Actuator
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeCedratPiezo.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeCedratPiezo>>

#+begin_src matlab
  function [cedrat] = initializeCedratPiezo()
    %% Stewart Object
    cedrat = struct();
    cedrat.k = 10e7; % Linear Stiffness of each "blade" [N/m]
    cedrat.ka = 10e7; % Linear Stiffness of the stack [N/m]

    cedrat.c = 0.1*sqrt(1*cedrat.k); % [N/(m/s)]
    cedrat.ca = 0.1*sqrt(1*cedrat.ka); % [N/(m/s)]

    cedrat.L = 80; % Total Width of the Actuator[mm]
    cedrat.H = 45; % Total Height of the Actuator [mm]
    cedrat.L2 = sqrt((cedrat.L/2)^2 + (cedrat.H/2)^2); % Length of the elipsoidal sections [mm]
    cedrat.alpha = 180/pi*atan2(cedrat.L/2, cedrat.H/2); % [deg]

    %% Save
    save('./mat/stages.mat', 'cedrat', '-append');
  end
#+end_src