diff --git a/active_damping/matlab/sim_nass_active_damping.slx b/active_damping/matlab/sim_nass_active_damping.slx
index 957b081..d7483ac 100644
Binary files a/active_damping/matlab/sim_nass_active_damping.slx and b/active_damping/matlab/sim_nass_active_damping.slx differ
diff --git a/mat/conf_simulink.mat b/mat/conf_simulink.mat
new file mode 100644
index 0000000..6d01d43
Binary files /dev/null and b/mat/conf_simulink.mat differ
diff --git a/simscape_subsystems/axisc.slx b/simscape_subsystems/axisc.slx
deleted file mode 100644
index 5e962dc..0000000
Binary files a/simscape_subsystems/axisc.slx and /dev/null differ
diff --git a/simscape_subsystems/axisc_weld.slx b/simscape_subsystems/axisc_weld.slx
deleted file mode 100644
index 8a89b26..0000000
Binary files a/simscape_subsystems/axisc_weld.slx and /dev/null differ
diff --git a/simscape_subsystems/figs b/simscape_subsystems/figs
index d440220..8ea5186 120000
--- a/simscape_subsystems/figs
+++ b/simscape_subsystems/figs
@@ -1 +1 @@
-../figs/
\ No newline at end of file
+../figs
\ No newline at end of file
diff --git a/simscape_subsystems/geophone_z_axis.slx b/simscape_subsystems/geophone_z_axis.slx
deleted file mode 100644
index c4742c8..0000000
Binary files a/simscape_subsystems/geophone_z_axis.slx and /dev/null differ
diff --git a/simscape_subsystems/granite.slx b/simscape_subsystems/granite.slx
deleted file mode 100644
index 1bb5a07..0000000
Binary files a/simscape_subsystems/granite.slx and /dev/null differ
diff --git a/simscape_subsystems/granite_3dof.slx b/simscape_subsystems/granite_3dof.slx
deleted file mode 100644
index 82305d2..0000000
Binary files a/simscape_subsystems/granite_3dof.slx and /dev/null differ
diff --git a/simscape_subsystems/granite_rigid.slx b/simscape_subsystems/granite_rigid.slx
deleted file mode 100644
index af5d5ae..0000000
Binary files a/simscape_subsystems/granite_rigid.slx and /dev/null differ
diff --git a/simscape_subsystems/ground.slx b/simscape_subsystems/ground.slx
deleted file mode 100644
index 748be9c..0000000
Binary files a/simscape_subsystems/ground.slx and /dev/null differ
diff --git a/simscape_subsystems/index.org b/simscape_subsystems/index.org
index d98b951..d8911e7 100644
--- a/simscape_subsystems/index.org
+++ b/simscape_subsystems/index.org
@@ -42,7 +42,6 @@
 :END:
 
 * Introduction                                                        :ignore:
-
 The full Simscape Model is represented in Figure [[fig:simscape_picture]].
 
 #+name: fig:simscape_picture
@@ -56,6 +55,59 @@ Each stage is configured (geometry, mass properties, dynamic properties ...) usi
 
 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
+
 * Ground
 :PROPERTIES:
 :header-args:matlab+: :tangle ../src/initializeGround.m
@@ -87,21 +139,55 @@ The model of the Ground is composed of:
 :UNNUMBERED: t
 :END:
 #+begin_src matlab
-  function [ground] = initializeGround()
+  function [ground] = initializeGround(args)
 #+end_src
 
-** Function content
+** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+      args.type char {mustBeMember(args.type,{'none', 'solid'})} = 'solid'
+  end
+#+end_src
+
+** Structure initialization
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
 First, we initialize the =granite= structure.
 #+begin_src matlab
   ground = struct();
 #+end_src
 
-We set the shape and density of the ground solid element.
+** Add Type
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
 #+begin_src matlab
-  ground.shape = [2, 2, 0.5]; % [m]
-  ground.density = 2800; % [kg/m3]
+  switch args.type
+    case 'none'
+      ground.type = 0;
+    case 'solid'
+      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
+
+** Save the Structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
 The =ground= structure is saved.
 #+begin_src matlab
   save('./mat/stages.mat', 'ground', '-append');
@@ -148,15 +234,17 @@ The output =sample_pos= corresponds to the impact point of the X-ray.
 :UNNUMBERED: t
 :END:
 #+begin_src matlab
-   arguments
-       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
+  arguments
+    args.type          char   {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = 'flexible'
+    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
 
-** Function content
+
+** Structure initialization
 :PROPERTIES:
 :UNNUMBERED: t
 :END:
@@ -165,6 +253,25 @@ First, we initialize the =granite= structure.
   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;
+  end
+#+end_src
+
+** Function content
+: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]
@@ -197,6 +304,10 @@ Z-offset for the initial position of the sample with respect to the granite top
   granite.sample_pos = 0.8; % [m]
 #+end_src
 
+** Save the Structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
 The =granite= structure is saved.
 #+begin_src matlab
   save('./mat/stages.mat', 'granite', '-append');
@@ -247,18 +358,19 @@ The Simscape model of the Translation stage consist of:
 :END:
 #+begin_src matlab
   arguments
-      args.x11 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.z11 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.x21 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.z21 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.x12 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.z12 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.x22 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.z22 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
+    args.x11 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.z11 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.x21 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.z21 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.x12 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.z12 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.x22 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.z22 (1,1) double {mustBeNumeric} = 0 % [m]
   end
 #+end_src
 
-** Function content
+** Structure initialization
 :PROPERTIES:
 :UNNUMBERED: t
 :END:
@@ -267,6 +379,27 @@ First, we initialize the =ty= structure.
   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;
+  end
+#+end_src
+
+
+** Function content
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
 Define the density of the materials as well as the geometry (STEP files).
 #+begin_src matlab
   % Ty Granite frame
@@ -330,6 +463,10 @@ Equilibrium position of the joints.
   ty.z0_22 = args.z22;
 #+end_src
 
+** Save the Structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
 The =ty= structure is saved.
 #+begin_src matlab
   save('./mat/stages.mat', 'ty', '-append');
@@ -380,22 +517,23 @@ The Simscape model of the Tilt stage is composed of:
 :END:
 #+begin_src matlab
   arguments
-      args.x11 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.y11 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.z11 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.x12 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.y12 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.z12 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.x21 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.y21 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.z21 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.x22 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.y22 (1,1) double {mustBeNumeric} = 0 % [m]
-      args.z22 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
+    args.x11 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.y11 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.z11 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.x12 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.y12 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.z12 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.x21 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.y21 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.z21 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.x22 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.y22 (1,1) double {mustBeNumeric} = 0 % [m]
+    args.z22 (1,1) double {mustBeNumeric} = 0 % [m]
   end
 #+end_src
 
-** Function content
+** Structure initialization
 :PROPERTIES:
 :UNNUMBERED: t
 :END:
@@ -404,6 +542,28 @@ First, we initialize the =ry= structure.
   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;
+  end
+#+end_src
+
+
+
+** Function content
+: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
@@ -460,6 +620,10 @@ Z-Offset so that the center of rotation matches the sample center;
   ry.z_offset = 0.58178; % [m]
 #+end_src
 
+** Save the Structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
 The =ty= structure is saved.
 #+begin_src matlab
   save('./mat/stages.mat', 'ry', '-append');
@@ -506,15 +670,16 @@ The Simscape model of the Spindle is composed of:
 :END:
 #+begin_src matlab
   arguments
-      args.x0  (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
-      args.y0  (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
-      args.z0  (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
-      args.rx0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
-      args.ry0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
+    args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
+    args.x0  (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
+    args.y0  (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
+    args.z0  (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
+    args.rx0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
+    args.ry0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
   end
 #+end_src
 
-** Function content
+** Structure initialization
 :PROPERTIES:
 :UNNUMBERED: t
 :END:
@@ -523,6 +688,26 @@ First, we initialize the =rz= structure.
   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;
+  end
+#+end_src
+
+** Function content
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
 Properties of the Material and link to the geometry of the spindle.
 #+begin_src matlab
   % Spindle - Slip Ring
@@ -563,6 +748,10 @@ Equilibrium position of the joints.
   rz.ry0 = args.ry0;
 #+end_src
 
+** Save the Structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
 The =rz= structure is saved.
 #+begin_src matlab
   save('./mat/stages.mat', 'rz', '-append');
@@ -661,6 +850,10 @@ Equilibrium position of the each joint.
   micro_hexapod.dLeq = args.dLeq;
 #+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');
@@ -697,16 +890,20 @@ The Simscape model of the Center of gravity compensator is composed of:
 :UNNUMBERED: t
 :END:
 #+begin_src matlab
-  function [axisc] = initializeAxisc()
+  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
 
-
-** Function content
+** Structure initialization
 :PROPERTIES:
 :UNNUMBERED: t
 :END:
@@ -715,6 +912,25 @@ First, we initialize the =axisc= structure.
   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
+
+** Function content
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
 Properties of the Material and link to the geometry files.
 #+begin_src matlab
   % Structure
@@ -734,6 +950,10 @@ Properties of the Material and link to the geometry files.
   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');
@@ -778,12 +998,13 @@ The output =mirror_center= corresponds to the center of the Sphere and is the po
 :END:
 #+begin_src matlab
   arguments
-      args.shape       char   {mustBeMember(args.shape,{'spherical', 'conical'})} = 'spherical'
-      args.angle (1,1) double {mustBeNumeric, mustBePositive} = 45 % [deg]
+    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
 
-** Function content
+** Structure initialization
 :PROPERTIES:
 :UNNUMBERED: t
 :END:
@@ -792,13 +1013,41 @@ First, we initialize the =mirror= structure.
   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
+
+** Function content
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
 We define the geometrical values.
 #+begin_src matlab
   mirror.h = 50; % Height of the mirror [mm]
+
   mirror.thickness = 25; % Thickness of the plate supporting the sample [mm]
+
   mirror.hole_rad = 120; % radius of the hole in the mirror [mm]
+
   mirror.support_rad = 100; % radius of the support plate [mm]
-  mirror.jacobian = 150; % point of interest offset in z (above the top surfave) [mm]
+
+  % point of interest offset in z (above the top surfave) [mm]
+  switch args.type
+    case 'none'
+      mirror.jacobian = 200;
+    case 'rigid'
+      mirror.jacobian = 200 - mirror.h;
+  end
+
   mirror.rad = 180; % radius of the mirror (at the bottom surface) [mm]
 #+end_src
 
@@ -841,6 +1090,10 @@ Finally, we close the shape.
   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');
@@ -942,6 +1195,10 @@ The =mirror= structure is saved.
   nano_hexapod.dLeq = args.dLeq;
 #+end_src
 
+** Save the Structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
 #+begin_src matlab
   save('./mat/stages.mat', 'nano_hexapod', '-append');
 #+end_src
@@ -989,18 +1246,19 @@ The Simscape model of the sample environment is composed of:
 :END:
 #+begin_src matlab
   arguments
-      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.x0     (1,1) double {mustBeNumeric} = 0 % [m]
-      args.y0     (1,1) double {mustBeNumeric} = 0 % [m]
-      args.z0     (1,1) double {mustBeNumeric} = 0 % [m]
+    args.type         char   {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = '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.x0     (1,1) double {mustBeNumeric} = 0 % [m]
+    args.y0     (1,1) double {mustBeNumeric} = 0 % [m]
+    args.z0     (1,1) double {mustBeNumeric} = 0 % [m]
   end
 #+end_src
 
-** Function content
+** Structure initialization
 :PROPERTIES:
 :UNNUMBERED: t
 :END:
@@ -1009,6 +1267,25 @@ First, we initialize the =sample= structure.
   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;
+  end
+#+end_src
+
+** Function content
+: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]
@@ -1038,11 +1315,82 @@ Equilibrium position of the Cartesian joint corresponding to the sample fixation
   sample.z0 = args.z0; % [m]
 #+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'})} = 'open-loop'
+    args.K (6,6)                                                                   = ss(zeros(6, 6))
+  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;
+  end
+#+end_src
+
+** Control Law
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  controller.K = args.K;
+#+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
diff --git a/simscape_subsystems/metrology_6dof_homog_transform.slx b/simscape_subsystems/metrology_6dof_homog_transform.slx
deleted file mode 100644
index 1d92b12..0000000
Binary files a/simscape_subsystems/metrology_6dof_homog_transform.slx and /dev/null differ
diff --git a/simscape_subsystems/metrology_6dof_rotation_matrix.slx b/simscape_subsystems/metrology_6dof_rotation_matrix.slx
deleted file mode 100644
index b10bc8d..0000000
Binary files a/simscape_subsystems/metrology_6dof_rotation_matrix.slx and /dev/null differ
diff --git a/simscape_subsystems/micro_hexapod_F.slx b/simscape_subsystems/micro_hexapod_F.slx
deleted file mode 100644
index 156c727..0000000
Binary files a/simscape_subsystems/micro_hexapod_F.slx and /dev/null differ
diff --git a/simscape_subsystems/micro_hexapod_new.slx b/simscape_subsystems/micro_hexapod_new.slx
index e3aa3bc..17c4e36 100644
Binary files a/simscape_subsystems/micro_hexapod_new.slx and b/simscape_subsystems/micro_hexapod_new.slx differ
diff --git a/simscape_subsystems/micro_hexapod_rigid.slx b/simscape_subsystems/micro_hexapod_rigid.slx
deleted file mode 100644
index d36d11a..0000000
Binary files a/simscape_subsystems/micro_hexapod_rigid.slx and /dev/null differ
diff --git a/simscape_subsystems/micro_hexapod_rigid_legs.slx b/simscape_subsystems/micro_hexapod_rigid_legs.slx
deleted file mode 100644
index be44293..0000000
Binary files a/simscape_subsystems/micro_hexapod_rigid_legs.slx and /dev/null differ
diff --git a/simscape_subsystems/micro_hexapod_rigid_simple.slx b/simscape_subsystems/micro_hexapod_rigid_simple.slx
deleted file mode 100644
index c0e1823..0000000
Binary files a/simscape_subsystems/micro_hexapod_rigid_simple.slx and /dev/null differ
diff --git a/simscape_subsystems/nano_hexapod_D.slx b/simscape_subsystems/nano_hexapod_D.slx
deleted file mode 100644
index 45d54cf..0000000
Binary files a/simscape_subsystems/nano_hexapod_D.slx and /dev/null differ
diff --git a/simscape_subsystems/nano_hexapod_F.slx b/simscape_subsystems/nano_hexapod_F.slx
index 1559e53..3072538 100644
Binary files a/simscape_subsystems/nano_hexapod_F.slx and b/simscape_subsystems/nano_hexapod_F.slx differ
diff --git a/simscape_subsystems/nano_hexapod_cedrat_1dof.slx b/simscape_subsystems/nano_hexapod_cedrat_1dof.slx
deleted file mode 100644
index bc5ef07..0000000
Binary files a/simscape_subsystems/nano_hexapod_cedrat_1dof.slx and /dev/null differ
diff --git a/simscape_subsystems/nano_hexapod_leg_rigid.slx b/simscape_subsystems/nano_hexapod_leg_rigid.slx
deleted file mode 100644
index 68b4435..0000000
Binary files a/simscape_subsystems/nano_hexapod_leg_rigid.slx and /dev/null differ
diff --git a/simscape_subsystems/nano_hexapod_rigid.slx b/simscape_subsystems/nano_hexapod_rigid.slx
deleted file mode 100644
index a4c890c..0000000
Binary files a/simscape_subsystems/nano_hexapod_rigid.slx and /dev/null differ
diff --git a/simscape_subsystems/nano_hexapod_rigid_simple.slx b/simscape_subsystems/nano_hexapod_rigid_simple.slx
deleted file mode 100644
index f71ff77..0000000
Binary files a/simscape_subsystems/nano_hexapod_rigid_simple.slx and /dev/null differ
diff --git a/simscape_subsystems/piezo_actuator_cedrat.slx b/simscape_subsystems/piezo_actuator_cedrat.slx
deleted file mode 100644
index 0c7513a..0000000
Binary files a/simscape_subsystems/piezo_actuator_cedrat.slx and /dev/null differ
diff --git a/simscape_subsystems/piezo_actuator_cedrat_simple.slx b/simscape_subsystems/piezo_actuator_cedrat_simple.slx
deleted file mode 100644
index 17a8b86..0000000
Binary files a/simscape_subsystems/piezo_actuator_cedrat_simple.slx and /dev/null differ
diff --git a/simscape_subsystems/reference_mirror.slx b/simscape_subsystems/reference_mirror.slx
deleted file mode 100644
index 8d960b5..0000000
Binary files a/simscape_subsystems/reference_mirror.slx and /dev/null differ
diff --git a/simscape_subsystems/sample_environment.slx b/simscape_subsystems/sample_environment.slx
deleted file mode 100644
index c7dbd3c..0000000
Binary files a/simscape_subsystems/sample_environment.slx and /dev/null differ
diff --git a/simscape_subsystems/sample_environment_rigid.slx b/simscape_subsystems/sample_environment_rigid.slx
deleted file mode 100644
index 03b006b..0000000
Binary files a/simscape_subsystems/sample_environment_rigid.slx and /dev/null differ
diff --git a/simscape_subsystems/spindle_D.slx b/simscape_subsystems/spindle_D.slx
deleted file mode 100644
index bd231ba..0000000
Binary files a/simscape_subsystems/spindle_D.slx and /dev/null differ
diff --git a/simscape_subsystems/spindle_rigid.slx b/simscape_subsystems/spindle_rigid.slx
deleted file mode 100644
index 83f6058..0000000
Binary files a/simscape_subsystems/spindle_rigid.slx and /dev/null differ
diff --git a/simscape_subsystems/tilt_stage_D.slx b/simscape_subsystems/tilt_stage_D.slx
deleted file mode 100644
index 2d5b4df..0000000
Binary files a/simscape_subsystems/tilt_stage_D.slx and /dev/null differ
diff --git a/simscape_subsystems/tilt_stage_rigid.slx b/simscape_subsystems/tilt_stage_rigid.slx
deleted file mode 100644
index e14ef67..0000000
Binary files a/simscape_subsystems/tilt_stage_rigid.slx and /dev/null differ
diff --git a/simscape_subsystems/translation_stage_D.slx b/simscape_subsystems/translation_stage_D.slx
deleted file mode 100644
index 0dcb3c4..0000000
Binary files a/simscape_subsystems/translation_stage_D.slx and /dev/null differ
diff --git a/simscape_subsystems/translation_stage_F.slx b/simscape_subsystems/translation_stage_F.slx
deleted file mode 100644
index 977dcab..0000000
Binary files a/simscape_subsystems/translation_stage_F.slx and /dev/null differ
diff --git a/simscape_subsystems/translation_stage_modal_analysis.slx b/simscape_subsystems/translation_stage_modal_analysis.slx
deleted file mode 100644
index 3b921f1..0000000
Binary files a/simscape_subsystems/translation_stage_modal_analysis.slx and /dev/null differ
diff --git a/simscape_subsystems/translation_stage_rigid.slx b/simscape_subsystems/translation_stage_rigid.slx
deleted file mode 100644
index 518a4c1..0000000
Binary files a/simscape_subsystems/translation_stage_rigid.slx and /dev/null differ
diff --git a/src/initializeAxisc.m b/src/initializeAxisc.m
index ac21aec..9358a59 100644
--- a/src/initializeAxisc.m
+++ b/src/initializeAxisc.m
@@ -1,7 +1,20 @@
-function [axisc] = initializeAxisc()
+function [axisc] = initializeAxisc(args)
+
+arguments
+  args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
+end
 
 axisc = struct();
 
+switch args.type
+  case 'none'
+    axisc.type = 0;
+  case 'rigid'
+    axisc.type = 1;
+  case 'flexible'
+    axisc.type = 2;
+end
+
 % Structure
 axisc.structure.density = 3400; % [kg/m3]
 axisc.structure.STEP    = './STEPS/axisc/axisc_structure.STEP';
diff --git a/src/initializeController.m b/src/initializeController.m
new file mode 100644
index 0000000..b5f39d2
--- /dev/null
+++ b/src/initializeController.m
@@ -0,0 +1,21 @@
+function [] = initializeController(args)
+
+arguments
+  args.type         char   {mustBeMember(args.type,{'open-loop', 'iff', 'dvf'})} = 'open-loop'
+  args.K (6,6)                                                                   = ss(zeros(6, 6))
+end
+
+controller = struct();
+
+switch args.type
+  case 'open-loop'
+    controller.type = 1;
+  case 'dvf'
+    controller.type = 2;
+  case 'iff'
+    controller.type = 3;
+end
+
+controller.K = args.K;
+
+save('./mat/controller.mat', 'controller');
diff --git a/src/initializeGranite.m b/src/initializeGranite.m
index 7529112..88807ae 100644
--- a/src/initializeGranite.m
+++ b/src/initializeGranite.m
@@ -1,14 +1,24 @@
 function [granite] = initializeGranite(args)
 
 arguments
-    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]
+  args.type          char   {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = 'flexible'
+  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
 
 granite = struct();
 
+switch args.type
+  case 'none'
+    granite.type = 0;
+  case 'rigid'
+    granite.type = 1;
+  case 'flexible'
+    granite.type = 2;
+end
+
 granite.density = args.density; % [kg/m3]
 granite.STEP    = './STEPS/granite/granite.STEP';
 
diff --git a/src/initializeGround.m b/src/initializeGround.m
index a5716c9..457ca07 100644
--- a/src/initializeGround.m
+++ b/src/initializeGround.m
@@ -1,8 +1,19 @@
-function [ground] = initializeGround()
+function [ground] = initializeGround(args)
+
+arguments
+    args.type char {mustBeMember(args.type,{'none', 'solid'})} = 'solid'
+end
 
 ground = struct();
 
-ground.shape = [2, 2, 0.5]; % [m]
-ground.density = 2800; % [kg/m3]
+switch args.type
+  case 'none'
+    ground.type = 0;
+  case 'solid'
+    ground.type = 1;
+end
+
+ground.shape   = [2, 2, 0.5]; % [m]
+ground.density = 2800;        % [kg/m3]
 
 save('./mat/stages.mat', 'ground', '-append');
diff --git a/src/initializeMicroHexapod.m b/src/initializeMicroHexapod.m
index e464a44..cb2a71c 100644
--- a/src/initializeMicroHexapod.m
+++ b/src/initializeMicroHexapod.m
@@ -1,4 +1,4 @@
-function [micro_hexapod] = initializeMicroHexapodNew(args)
+function [micro_hexapod] = initializeMicroHexapod(args)
 
 arguments
     % initializeFramesPositions
diff --git a/src/initializeMicroHexapodOld.m b/src/initializeMicroHexapodOld.m
new file mode 100644
index 0000000..41da308
--- /dev/null
+++ b/src/initializeMicroHexapodOld.m
@@ -0,0 +1,196 @@
+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
diff --git a/src/initializeMirror.m b/src/initializeMirror.m
index 71b2594..5c3ae2c 100644
--- a/src/initializeMirror.m
+++ b/src/initializeMirror.m
@@ -1,17 +1,36 @@
 function [] = initializeMirror(args)
 
 arguments
-    args.shape       char   {mustBeMember(args.shape,{'spherical', 'conical'})} = 'spherical'
-    args.angle (1,1) double {mustBeNumeric, mustBePositive} = 45 % [deg]
+  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
 
 mirror = struct();
 
+switch args.type
+  case 'none'
+    mirror.type = 0;
+  case 'rigid'
+    mirror.type = 1;
+end
+
 mirror.h = 50; % Height of the mirror [mm]
+
 mirror.thickness = 25; % Thickness of the plate supporting the sample [mm]
+
 mirror.hole_rad = 120; % radius of the hole in the mirror [mm]
+
 mirror.support_rad = 100; % radius of the support plate [mm]
-mirror.jacobian = 150; % point of interest offset in z (above the top surfave) [mm]
+
+% point of interest offset in z (above the top surfave) [mm]
+switch args.type
+  case 'none'
+    mirror.jacobian = 200;
+  case 'rigid'
+    mirror.jacobian = 200 - mirror.h;
+end
+
 mirror.rad = 180; % radius of the mirror (at the bottom surface) [mm]
 
 mirror.density = 2400; % Density of the material [kg/m3]
diff --git a/src/initializeReferences.m b/src/initializeReferences.m
index edeb59e..efc9e30 100644
--- a/src/initializeReferences.m
+++ b/src/initializeReferences.m
@@ -54,9 +54,9 @@ Dyd  = zeros(length(t), 1);
 Dydd = zeros(length(t), 1);
 switch args.Dy_type
   case 'constant'
-    Dy(:) = args.Dy_amplitude;
-    Dyd(:)   = 0;
-    Dydd(:)  = 0;
+    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);
diff --git a/src/initializeRy.m b/src/initializeRy.m
index 28d8abc..b61375f 100644
--- a/src/initializeRy.m
+++ b/src/initializeRy.m
@@ -1,22 +1,32 @@
 function [ry] = initializeRy(args)
 
 arguments
-    args.x11 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.y11 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.z11 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.x12 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.y12 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.z12 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.x21 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.y21 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.z21 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.x22 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.y22 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.z22 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
+  args.x11 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.y11 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.z11 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.x12 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.y12 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.z12 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.x21 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.y21 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.z21 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.x22 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.y22 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.z22 (1,1) double {mustBeNumeric} = 0 % [m]
 end
 
 ry = struct();
 
+switch args.type
+  case 'none'
+    ry.type = 0;
+  case 'rigid'
+    ry.type = 1;
+  case 'flexible'
+    ry.type = 2;
+end
+
 % Ry - Guide for the tilt stage
 ry.guide.density = 7800; % [kg/m3]
 ry.guide.STEP    = './STEPS/ry/Tilt_Guide.STEP';
diff --git a/src/initializeRz.m b/src/initializeRz.m
index 6c23788..5eb78b8 100644
--- a/src/initializeRz.m
+++ b/src/initializeRz.m
@@ -1,15 +1,25 @@
 function [rz] = initializeRz(args)
 
 arguments
-    args.x0  (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
-    args.y0  (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
-    args.z0  (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
-    args.rx0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
-    args.ry0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
+  args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
+  args.x0  (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
+  args.y0  (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
+  args.z0  (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
+  args.rx0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
+  args.ry0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
 end
 
 rz = struct();
 
+switch args.type
+  case 'none'
+    rz.type = 0;
+  case 'rigid'
+    rz.type = 1;
+  case 'flexible'
+    rz.type = 2;
+end
+
 % Spindle - Slip Ring
 rz.slipring.density = 7800; % [kg/m3]
 rz.slipring.STEP    = './STEPS/rz/Spindle_Slip_Ring.STEP';
diff --git a/src/initializeSample.m b/src/initializeSample.m
index 6251729..6dd558a 100644
--- a/src/initializeSample.m
+++ b/src/initializeSample.m
@@ -1,18 +1,28 @@
 function [sample] = initializeSample(args)
 
 arguments
-    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.x0     (1,1) double {mustBeNumeric} = 0 % [m]
-    args.y0     (1,1) double {mustBeNumeric} = 0 % [m]
-    args.z0     (1,1) double {mustBeNumeric} = 0 % [m]
+  args.type         char   {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = '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.x0     (1,1) double {mustBeNumeric} = 0 % [m]
+  args.y0     (1,1) double {mustBeNumeric} = 0 % [m]
+  args.z0     (1,1) double {mustBeNumeric} = 0 % [m]
 end
 
 sample = struct();
 
+switch args.type
+  case 'none'
+    sample.type = 0;
+  case 'rigid'
+    sample.type = 1;
+  case 'flexible'
+    sample.type = 2;
+end
+
 sample.radius = args.radius; % [m]
 sample.height = args.height; % [m]
 sample.mass = args.mass; % [kg]
diff --git a/src/initializeSimscapeConfiguration.m b/src/initializeSimscapeConfiguration.m
new file mode 100644
index 0000000..ba93c9f
--- /dev/null
+++ b/src/initializeSimscapeConfiguration.m
@@ -0,0 +1,15 @@
+function [] = initializeSimscapeConfiguration(args)
+
+arguments
+  args.gravity logical {mustBeNumericOrLogical} = true
+end
+
+conf_simscape = struct();
+
+if args.gravity
+  conf_simscape.type = 1;
+else
+  conf_simscape.type = 2;
+end
+
+save('./mat/conf_simscape.mat', 'conf_simscape');
diff --git a/src/initializeTy.m b/src/initializeTy.m
index 237d1f1..276c849 100644
--- a/src/initializeTy.m
+++ b/src/initializeTy.m
@@ -1,18 +1,28 @@
 function [ty] = initializeTy(args)
 
 arguments
-    args.x11 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.z11 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.x21 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.z21 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.x12 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.z12 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.x22 (1,1) double {mustBeNumeric} = 0 % [m]
-    args.z22 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
+  args.x11 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.z11 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.x21 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.z21 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.x12 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.z12 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.x22 (1,1) double {mustBeNumeric} = 0 % [m]
+  args.z22 (1,1) double {mustBeNumeric} = 0 % [m]
 end
 
 ty = struct();
 
+switch args.type
+  case 'none'
+    ty.type = 0;
+  case 'rigid'
+    ty.type = 1;
+  case 'flexible'
+    ty.type = 2;
+end
+
 % Ty Granite frame
 ty.granite_frame.density = 7800; % [kg/m3] => 43kg
 ty.granite_frame.STEP    = './STEPS/Ty/Ty_Granite_Frame.STEP';