Reworked Flexor parts

index.org

@@ -41,9 +41,9 @@ In this document, Finite Element Models (FEM) of parts of the Nano-Hexapod are d
 - Section [[sec:APA300ML]]:
   A super-element of the Amplified Piezoelectric Actuator APA300ML used for the NASS is exported using Ansys and imported in Simscape.
   The static and dynamical properties of the APA300ML are then estimated using the Simscape model.
-- Section [[sec:first_flexible_joint]]:
+- Section [[sec:flexor_ID16]]:
   A first geometry of a Flexible joint is modelled and its characteristics are identified from the Stiffness matrix as well as from the Simscape model.
-- Section [[sec:optimized_flexible_joint]]:
+- Section [[sec:flexor_025]]:
   An optimized flexible joint is developed for the Nano-Hexapod and is then imported in a Simscape model.
 - Section [[sec:integral_force_feedback]]:
 - Section [[sec:strut_fem]]:
@@ -805,7 +805,10 @@ The dynamics of the Simscape simplified model is identified and compared with th
 [[file:figs/apa300ml_comp_simpler_simscape.png]]
 
 * First Flexible Joint Geometry
-<<sec:first_flexible_joint>>
+:PROPERTIES:
+:header-args:matlab+: :tangle matlab/flexor_ID16.m
+:END:
+<<sec:flexor_ID16>>
 ** Introduction                                                      :ignore:
 The studied flexor is shown in Figure [[fig:flexor_id16_screenshot]].
 
@@ -827,11 +830,16 @@ A simplified model of the flexor is then developped.
   <<matlab-init>>
 #+end_src
 
-#+begin_src matlab
+#+begin_src matlab :tangle no
-  addpath('./data/flexor_ID16/');
+  addpath('matlab/');
+  addpath('matlab/flexor_ID16/');
 #+end_src
 
-#+begin_src matlab :exports none
+#+begin_src matlab :eval no
+  addpath('flexor_ID16/');
+#+end_src
+
+#+begin_src matlab
   open('flexor_ID16.slx');
 #+end_src
 
@@ -842,41 +850,6 @@ We first extract the stiffness and mass matrices.
   M = extractMatrix('mat_M_6modes_2MDoF.matrix');
 #+end_src
 
-Then, we extract the coordinates of the interface nodes.
-#+begin_src matlab
-  [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('out_nodes_3D.txt');
-#+end_src
-
-#+begin_src matlab
-  save('./mat/flexor_ID16.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K');
-#+end_src
-
-** Output parameters
-#+begin_src matlab
-  load('./mat/flexor_ID16.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K');
-#+end_src
-
-#+begin_src matlab :exports results :results value table replace :tangle no
-  data2orgtable([length(n_i); length(int_i); size(M,1) - 6*length(int_i); size(M,1)], {'Total number of Nodes', 'Number of interface Nodes', 'Number of Modes', 'Size of M and K matrices'}, {}, ' %.0f ');
-#+end_src
-
-#+RESULTS:
-| Total number of Nodes     |  2 |
-| Number of interface Nodes |  2 |
-| Number of Modes           |  6 |
-| Size of M and K matrices  | 18 |
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-  data2orgtable([[1:length(int_i)]', int_i, int_xyz], {}, {'Node i', 'Node Number', 'x [m]', 'y [m]', 'z [m]'}, ' %f ');
-#+end_src
-
-#+caption: Coordinates of the interface nodes
-#+RESULTS:
-| Node i | Node Number | x [m] | y [m] | z [m] |
-|--------+-------------+-------+-------+-------|
-|    1.0 |    181278.0 |   0.0 |   0.0 |   0.0 |
-|    2.0 |    181279.0 |   0.0 |   0.0 |  -0.0 |
-
 #+begin_src matlab :exports results :results value table replace :tangle no
   data2orgtable(K(1:10, 1:10), {}, {}, ' %.2e ');
 #+end_src
@@ -894,7 +867,6 @@ Then, we extract the coordinates of the interface nodes.
 |     -2220.0 |     -1290.0 | -119000000.0 |     -1.31 |    -1.49 |     -1.79 |      1640.0 |      1290.0 |  119000000.0 |     1.32 |
 |       0.147 |       148.0 |        -1.31 |     -33.0 |  0.00026 |  0.000379 |       120.0 |       -72.0 |         1.32 |     34.7 |
 
-
 #+begin_src matlab :exports results :results value table replace :tangle no
   data2orgtable(M(1:10, 1:10), {}, {}, ' %.1g ');
 #+end_src
@@ -912,25 +884,35 @@ Then, we extract the coordinates of the interface nodes.
 |  7e-08 |   3e-07 |  0.003 |  -8e-10 |  2e-09 | -1e-09 | -3e-07 | -4e-08 |   0.02 | -1e-09 |
 |  1e-10 |   1e-05 |  1e-09 |  -1e-09 | -3e-13 |  2e-12 | -2e-10 | 0.0002 | -1e-09 |  2e-06 |
 
-Using =K=, =M= and =int_xyz=, we can use the =Reduced Order Flexible Solid= simscape block.
+Then, we extract the coordinates of the interface nodes.
-
+#+begin_src matlab
-** Flexible Joint Characteristics
+  [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('out_nodes_3D.txt');
-The most important parameters of the flexible joint can be directly estimated from the stiffness matrix.
+#+end_src
 
 #+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-  data2orgtable([1e-6*K(3,3), 1e-6*K(2,2), K(4,4), K(5,5), K(6,6); 60, 0, 15, 15, 20]', {'Axial Stiffness [N/um]', 'Shear Stiffness [N/um]', 'Bending Stiffness [Nm/rad]', 'Bending Stiffness [Nm/rad]', 'Torsion Stiffness [Nm/rad]'}, {'*Caracteristic*', '*Value*', '*Estimation by Francois*'}, ' %0.f ');
+  data2orgtable([[1:length(int_i)]', int_i, int_xyz], {}, {'Node i', 'Node Number', 'x [m]', 'y [m]', 'z [m]'}, ' %f ');
+#+end_src
+
+#+caption: Coordinates of the interface nodes
+#+RESULTS:
+| Node i | Node Number | x [m] | y [m] | z [m] |
+|--------+-------------+-------+-------+-------|
+|    1.0 |    181278.0 |   0.0 |   0.0 |   0.0 |
+|    2.0 |    181279.0 |   0.0 |   0.0 |  -0.0 |
+
+#+begin_src matlab :exports results :results value table replace :tangle no
+  data2orgtable([length(n_i); length(int_i); size(M,1) - 6*length(int_i); size(M,1)], {'Total number of Nodes', 'Number of interface Nodes', 'Number of Modes', 'Size of M and K matrices'}, {}, ' %.0f ');
 #+end_src
 
 #+RESULTS:
-| *Caracteristic*            | *Value* | *Estimation by Francois* |
+| Total number of Nodes     |  2 |
-|----------------------------+---------+--------------------------|
+| Number of interface Nodes |  2 |
-| Axial Stiffness [N/um]     |     119 |                       60 |
+| Number of Modes           |  6 |
-| Shear Stiffness [N/um]     |      11 |                        0 |
+| Size of M and K matrices  | 18 |
-| Bending Stiffness [Nm/rad] |      33 |                       15 |
-| Bending Stiffness [Nm/rad] |      33 |                       15 |
-| Torsion Stiffness [Nm/rad] |     236 |                       20 |
 
-** Identification of the parameters using Simscape
+Using =K=, =M= and =int_xyz=, we can use the =Reduced Order Flexible Solid= simscape block.
+
+** Identification of the parameters using Simscape and looking at the Stiffness Matrix
 The flexor is now imported into Simscape and its parameters are estimated using an identification.
 
 #+begin_src matlab :exports none
@@ -960,9 +942,9 @@ And we find the same parameters as the one estimated from the Stiffness matrix.
 | *Caracteristic*               | *Value* | *Identification* |
 |-------------------------------+---------+------------------|
 | Axial Stiffness Dz [N/um]     |     119 |              119 |
-| Bending Stiffness Rx [Nm/rad] |      33 |               34 |
+| Bending Stiffness Rx [Nm/rad] |      33 |               33 |
-| Bending Stiffness Ry [Nm/rad] |      33 |              126 |
+| Bending Stiffness Ry [Nm/rad] |      33 |               33 |
-| Torsion Stiffness Rz [Nm/rad] |     236 |              238 |
+| Torsion Stiffness Rz [Nm/rad] |     236 |              236 |
 
 ** Simpler Model
 Let's now model the flexible joint with a "perfect" Bushing joint as shown in Figure [[fig:flexible_joint_simscape]].
@@ -1000,8 +982,9 @@ The two obtained dynamics are compared in Figure
   freqs = logspace(0, 5, 1000);
 
   figure;
+  tiledlayout(1, 2, 'TileSpacing', 'None', 'Padding', 'None');
 
-  ax1 = subplot(1,2,1);
+  ax1 = nexttile;
   hold on;
   set(gca,'ColorOrderIndex',1)
   plot(freqs, abs(squeeze(freqresp(G(1,1), freqs, 'Hz'))), '-', 'DisplayName', '$D_x/F_x$');
@@ -1019,9 +1002,9 @@ The two obtained dynamics are compared in Figure
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
   xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
   hold off;
-  legend('location', 'northeast');
+  legend('location', 'southwest');
 
-  ax2 = subplot(1,2,2);
+  ax2 = nexttile;
   hold on;
   set(gca,'ColorOrderIndex',1)
   plot(freqs, abs(squeeze(freqresp(G(4,4), freqs, 'Hz'))), '-', 'DisplayName', '$R_x/M_x$');
@@ -1039,11 +1022,11 @@ The two obtained dynamics are compared in Figure
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
   xlabel('Frequency [Hz]'); ylabel('Amplitude [rad/Nm]');
   hold off;
-  legend('location', 'northeast');
+  legend('location', 'southwest');
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/flexor_ID16_compare_bushing_joint.pdf', 'width', 'full', 'height', 'tall');
+  exportFig('figs/flexor_ID16_compare_bushing_joint.pdf', 'width', 'wide', 'height', 'normal');
 #+end_src
 
 #+name: fig:flexor_ID16_compare_bushing_joint
@@ -1052,12 +1035,19 @@ The two obtained dynamics are compared in Figure
 [[file:figs/flexor_ID16_compare_bushing_joint.png]]
 
 * Optimized Flexible Joint
-<<sec:optimized_flexible_joint>>
+:PROPERTIES:
+:header-args:matlab+: :tangle matlab/flexor_025.m
+:END:
+<<sec:flexor_025>>
 ** Introduction                                                      :ignore:
 
+The joint geometry has been optimized using Ansys to have lower bending stiffness while keeping a large axial stiffness.
+
+The obtained geometry is shown in Figure [[fig:optimal_flexor]].
+
 #+name: fig:optimal_flexor
 #+caption: Flexor studied
-[[file:data/flexor_circ_025/CS.jpg]]
+[[file:figs/flexor_025_MDoF.jpg]]
 
 ** Matlab Init                                             :noexport:ignore:
 #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
@@ -1068,11 +1058,16 @@ The two obtained dynamics are compared in Figure
   <<matlab-init>>
 #+end_src
 
-#+begin_src matlab
+#+begin_src matlab :tangle no
-  addpath('./data/flexor_circ_025/');
+  addpath('matlab/');
+  addpath('matlab/flexor_025/');
 #+end_src
 
-#+begin_src matlab :exports none
+#+begin_src matlab :eval no
+  addpath('flexor_025/');
+#+end_src
+
+#+begin_src matlab
   open('flexor_025.slx');
 #+end_src
 
@@ -1083,41 +1078,6 @@ We first extract the stiffness and mass matrices.
   M = readmatrix('mat_M.CSV');
 #+end_src
 
-Then, we extract the coordinates of the interface nodes.
-#+begin_src matlab
-  [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('out_nodes_3D.txt');
-#+end_src
-
-#+begin_src matlab
-  save('./mat/flexor_025.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K');
-#+end_src
-
-** Output parameters
-#+begin_src matlab
-  load('./mat/flexor_025.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K');
-#+end_src
-
-#+begin_src matlab :exports results :results value table replace :tangle no
-  data2orgtable([length(n_i); length(int_i); size(M,1) - 6*length(int_i); size(M,1)], {'Total number of Nodes', 'Number of interface Nodes', 'Number of Modes', 'Size of M and K matrices'}, {}, ' %.0f ');
-#+end_src
-
-#+RESULTS:
-| Total number of Nodes     |  2 |
-| Number of interface Nodes |  2 |
-| Number of Modes           |  6 |
-| Size of M and K matrices  | 18 |
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-  data2orgtable([[1:length(int_i)]', int_i, int_xyz], {}, {'Node i', 'Node Number', 'x [m]', 'y [m]', 'z [m]'}, ' %f ');
-#+end_src
-
-#+caption: Coordinates of the interface nodes
-#+RESULTS:
-| Node i | Node Number | x [m] | y [m] | z [m] |
-|--------+-------------+-------+-------+-------|
-|    1.0 |    528875.0 |   0.0 |   0.0 |   0.0 |
-|    2.0 |    528876.0 |   0.0 |   0.0 |  -0.0 |
-
 #+begin_src matlab :exports results :results value table replace :tangle no
   data2orgtable(K(1:10, 1:10), {}, {}, ' %.2e ');
 #+end_src
@@ -1153,23 +1113,34 @@ Then, we extract the coordinates of the interface nodes.
 |  9e-09 | -5e-08 |  0.003 | -1e-08 |  6e-11 | -1e-11 |  1e-07 | -8e-08 |   0.01 | -6e-08 |
 |  2e-12 |  3e-09 | -1e-08 |  3e-10 | -6e-16 |  1e-13 | -4e-12 |  3e-05 | -6e-08 |  2e-07 |
 
-Using =K=, =M= and =int_xyz=, we can use the =Reduced Order Flexible Solid= simscape block.
 
-** Flexible Joint Characteristics
+Then, we extract the coordinates of the interface nodes.
-The most important parameters of the flexible joint can be directly estimated from the stiffness matrix.
+#+begin_src matlab
+  [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('out_nodes_3D.txt');
+#+end_src
 
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+#+begin_src matlab :exports results :results value table replace :tangle no
-  data2orgtable([1e-6*K(3,3), 1e-6*K(2,2), K(4,4), K(5,5), K(6,6)]', {'Axial Stiffness [N/um]', 'Shear Stiffness [N/um]', 'Bending Stiffness [Nm/rad]', 'Bending Stiffness [Nm/rad]', 'Torsion Stiffness [Nm/rad]'}, {'*Caracteristic*', '*Value*'}, ' %.1f ');
+  data2orgtable([length(n_i); length(int_i); size(M,1) - 6*length(int_i); size(M,1)], {'Total number of Nodes', 'Number of interface Nodes', 'Number of Modes', 'Size of M and K matrices'}, {}, ' %.0f ');
 #+end_src
 
 #+RESULTS:
-| *Caracteristic*            | *Value* |
+| Total number of Nodes     |  2 |
-|----------------------------+---------|
+| Number of interface Nodes |  2 |
-| Axial Stiffness [N/um]     |    94.0 |
+| Number of Modes           |  6 |
-| Shear Stiffness [N/um]     |    12.7 |
+| Size of M and K matrices  | 18 |
-| Bending Stiffness [Nm/rad] |     4.8 |
+
-| Bending Stiffness [Nm/rad] |     4.8 |
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-| Torsion Stiffness [Nm/rad] |   260.2 |
+  data2orgtable([[1:length(int_i)]', int_i, int_xyz], {}, {'Node i', 'Node Number', 'x [m]', 'y [m]', 'z [m]'}, ' %f ');
+#+end_src
+
+#+caption: Coordinates of the interface nodes
+#+RESULTS:
+| Node i | Node Number | x [m] | y [m] | z [m] |
+|--------+-------------+-------+-------+-------|
+|    1.0 |    528875.0 |   0.0 |   0.0 |   0.0 |
+|    2.0 |    528876.0 |   0.0 |   0.0 |  -0.0 |
+
+Using =K=, =M= and =int_xyz=, we can use the =Reduced Order Flexible Solid= simscape block.
 
 ** Identification of the parameters using Simscape
 The flexor is now imported into Simscape and its parameters are estimated using an identification.
@@ -1206,7 +1177,6 @@ And we find the same parameters as the one estimated from the Stiffness matrix.
 | Torsion Stiffness Rz [Nm/rad] |   260.2 |            260.2 |
 
 ** Simpler Model
-
 Let's now model the flexible joint with a "perfect" Bushing joint as shown in Figure [[fig:flexible_joint_simscape]].
 
 #+name: fig:flexible_joint_simscape
@@ -1242,8 +1212,9 @@ The two obtained dynamics are compared in Figure
   freqs = logspace(0, 5, 1000);
 
   figure;
+  tiledlayout(1, 2, 'TileSpacing', 'None', 'Padding', 'None');
 
-  ax1 = subplot(1,2,1);
+  ax1 = nexttile;
   hold on;
   set(gca,'ColorOrderIndex',1)
   plot(freqs, abs(squeeze(freqresp(G(1,1), freqs, 'Hz'))), '-', 'DisplayName', '$D_x/F_x$');
@@ -1261,9 +1232,9 @@ The two obtained dynamics are compared in Figure
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
   xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
   hold off;
-  legend('location', 'northeast');
+  legend('location', 'southwest');
 
-  ax2 = subplot(1,2,2);
+  ax2 = nexttile;
   hold on;
   set(gca,'ColorOrderIndex',1)
   plot(freqs, abs(squeeze(freqresp(G(4,4), freqs, 'Hz'))), '-', 'DisplayName', '$R_x/M_x$');
@@ -1281,11 +1252,11 @@ The two obtained dynamics are compared in Figure
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
   xlabel('Frequency [Hz]'); ylabel('Amplitude [rad/Nm]');
   hold off;
-  legend('location', 'northeast');
+  legend('location', 'southwest');
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/flexor_ID16_compare_bushing_joint.pdf', 'width', 'full', 'height', 'tall');
+  exportFig('figs/flexor_ID16_compare_bushing_joint.pdf', 'width', 'wide', 'height', 'normal');
 #+end_src
 
 #+name: fig:flexor_ID16_compare_bushing_joint

matlab/flexor_025.m

@@ -0,0 +1,135 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+addpath('flexor_025/');
+
+open('flexor_025.slx');
+
+% Import Mass Matrix, Stiffness Matrix, and Interface Nodes Coordinates
+% We first extract the stiffness and mass matrices.
+
+K = readmatrix('mat_K.CSV');
+M = readmatrix('mat_M.CSV');
+
+
+
+% #+caption: First 10x10 elements of the Mass matrix
+% #+RESULTS:
+% |  0.006 |  8e-09 | -2e-08 | -1e-10 |  3e-05 |  3e-08 |  0.003 | -3e-09 |  9e-09 |  2e-12 |
+% |  8e-09 |   0.02 |  1e-07 | -3e-05 |  1e-11 |  6e-10 |  1e-08 |  0.003 | -5e-08 |  3e-09 |
+% | -2e-08 |  1e-07 |   0.01 | -6e-08 | -6e-11 | -8e-12 | -1e-07 |  1e-08 |  0.003 | -1e-08 |
+% | -1e-10 | -3e-05 | -6e-08 |  1e-06 |  7e-14 |  6e-13 |  1e-10 |  1e-06 | -1e-08 |  3e-10 |
+% |  3e-05 |  1e-11 | -6e-11 |  7e-14 |  2e-07 |  1e-10 |  3e-08 | -7e-12 |  6e-11 | -6e-16 |
+% |  3e-08 |  6e-10 | -8e-12 |  6e-13 |  1e-10 |  5e-07 |  1e-08 | -5e-10 | -1e-11 |  1e-13 |
+% |  0.003 |  1e-08 | -1e-07 |  1e-10 |  3e-08 |  1e-08 |   0.02 | -2e-08 |  1e-07 | -4e-12 |
+% | -3e-09 |  0.003 |  1e-08 |  1e-06 | -7e-12 | -5e-10 | -2e-08 |  0.006 | -8e-08 |  3e-05 |
+% |  9e-09 | -5e-08 |  0.003 | -1e-08 |  6e-11 | -1e-11 |  1e-07 | -8e-08 |   0.01 | -6e-08 |
+% |  2e-12 |  3e-09 | -1e-08 |  3e-10 | -6e-16 |  1e-13 | -4e-12 |  3e-05 | -6e-08 |  2e-07 |
+
+
+% Then, we extract the coordinates of the interface nodes.
+
+[int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('out_nodes_3D.txt');
+
+% Identification of the parameters using Simscape
+% The flexor is now imported into Simscape and its parameters are estimated using an identification.
+
+
+m = 1;
+
+
+
+% The dynamics is identified from the applied force/torque to the measured displacement/rotation of the flexor.
+
+%% Name of the Simulink File
+mdl = 'flexor_025';
+
+%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/T'], 1, 'openinput');  io_i = io_i + 1;
+io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1;
+
+G = linearize(mdl, io);
+
+% Simpler Model
+% Let's now model the flexible joint with a "perfect" Bushing joint as shown in Figure [[fig:flexible_joint_simscape]].
+
+% #+name: fig:flexible_joint_simscape
+% #+caption: Bushing Joint used to model the flexible joint
+% [[file:figs/flexible_joint_simscape.png]]
+
+% The parameters of the Bushing joint (stiffnesses) are estimated from the Stiffness matrix that was computed from the FEM.
+
+Kx = K(1,1); % [N/m]
+Ky = K(2,2); % [N/m]
+Kz = K(3,3); % [N/m]
+Krx = K(4,4); % [Nm/rad]
+Kry = K(5,5); % [Nm/rad]
+Krz =  K(6,6); % [Nm/rad]
+
+
+
+% The dynamics from the applied force/torque to the measured displacement/rotation of the flexor is identified again for this simpler model.
+
+%% Name of the Simulink File
+mdl = 'flexor_025_simplified';
+
+%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/T'], 1, 'openinput');  io_i = io_i + 1;
+io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1;
+
+Gs = linearize(mdl, io);
+
+
+
+% The two obtained dynamics are compared in Figure
+
+
+freqs = logspace(0, 5, 1000);
+
+figure;
+tiledlayout(1, 2, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, abs(squeeze(freqresp(G(1,1), freqs, 'Hz'))), '-', 'DisplayName', '$D_x/F_x$');
+set(gca,'ColorOrderIndex',1)
+plot(freqs, abs(squeeze(freqresp(Gs(1,1), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(G(2,2), freqs, 'Hz'))), '-', 'DisplayName', '$D_y/F_y$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(Gs(2,2), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca,'ColorOrderIndex',3)
+plot(freqs, abs(squeeze(freqresp(G(3,3), freqs, 'Hz'))), '-', 'DisplayName', '$D_z/F_z$');
+set(gca,'ColorOrderIndex',3)
+plot(freqs, abs(squeeze(freqresp(Gs(3,3), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
+hold off;
+legend('location', 'southwest');
+
+ax2 = nexttile;
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, abs(squeeze(freqresp(G(4,4), freqs, 'Hz'))), '-', 'DisplayName', '$R_x/M_x$');
+set(gca,'ColorOrderIndex',1)
+plot(freqs, abs(squeeze(freqresp(Gs(4,4), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(G(5,5), freqs, 'Hz'))), '-', 'DisplayName', '$R_y/M_y$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(Gs(5,5), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca,'ColorOrderIndex',3)
+plot(freqs, abs(squeeze(freqresp(G(6,6), freqs, 'Hz'))), '-', 'DisplayName', '$R_z/M_z$');
+set(gca,'ColorOrderIndex',3)
+plot(freqs, abs(squeeze(freqresp(Gs(6,6), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude [rad/Nm]');
+hold off;
+legend('location', 'southwest');

matlab/flexor_ID16.m

@@ -0,0 +1,134 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+addpath('flexor_ID16/');
+
+open('flexor_ID16.slx');
+
+% Import Mass Matrix, Stiffness Matrix, and Interface Nodes Coordinates
+% We first extract the stiffness and mass matrices.
+
+K = extractMatrix('mat_K_6modes_2MDoF.matrix');
+M = extractMatrix('mat_M_6modes_2MDoF.matrix');
+
+
+
+% #+caption: First 10x10 elements of the Mass matrix
+% #+RESULTS:
+% |   0.02 |   1e-09 | -4e-08 |  -1e-10 | 0.0002 | -3e-11 |  0.004 |  5e-08 |  7e-08 |  1e-10 |
+% |  1e-09 |    0.02 | -3e-07 | -0.0002 | -1e-10 | -2e-09 |  2e-08 |  0.004 |  3e-07 |  1e-05 |
+% | -4e-08 |  -3e-07 |   0.02 |   7e-10 | -2e-09 |  1e-09 |  3e-07 |  7e-08 |  0.003 |  1e-09 |
+% | -1e-10 | -0.0002 |  7e-10 |   4e-06 | -1e-12 | -6e-13 |  2e-10 | -7e-06 | -8e-10 | -1e-09 |
+% | 0.0002 |  -1e-10 | -2e-09 |  -1e-12 |  3e-06 |  2e-13 |  9e-06 |  4e-11 |  2e-09 | -3e-13 |
+% | -3e-11 |  -2e-09 |  1e-09 |  -6e-13 |  2e-13 |  4e-07 |  8e-11 |  9e-10 | -1e-09 |  2e-12 |
+% |  0.004 |   2e-08 |  3e-07 |   2e-10 |  9e-06 |  8e-11 |   0.02 | -7e-08 | -3e-07 | -2e-10 |
+% |  5e-08 |   0.004 |  7e-08 |  -7e-06 |  4e-11 |  9e-10 | -7e-08 |   0.01 | -4e-08 | 0.0002 |
+% |  7e-08 |   3e-07 |  0.003 |  -8e-10 |  2e-09 | -1e-09 | -3e-07 | -4e-08 |   0.02 | -1e-09 |
+% |  1e-10 |   1e-05 |  1e-09 |  -1e-09 | -3e-13 |  2e-12 | -2e-10 | 0.0002 | -1e-09 |  2e-06 |
+
+% Then, we extract the coordinates of the interface nodes.
+
+[int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('out_nodes_3D.txt');
+
+% Identification of the parameters using Simscape and looking at the Stiffness Matrix
+% The flexor is now imported into Simscape and its parameters are estimated using an identification.
+
+
+m = 1;
+
+
+
+% The dynamics is identified from the applied force/torque to the measured displacement/rotation of the flexor.
+
+%% Name of the Simulink File
+mdl = 'flexor_ID16';
+
+%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/T'], 1, 'openinput');  io_i = io_i + 1;
+io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1;
+
+G = linearize(mdl, io);
+
+% Simpler Model
+% Let's now model the flexible joint with a "perfect" Bushing joint as shown in Figure [[fig:flexible_joint_simscape]].
+
+% #+name: fig:flexible_joint_simscape
+% #+caption: Bushing Joint used to model the flexible joint
+% [[file:figs/flexible_joint_simscape.png]]
+
+% The parameters of the Bushing joint (stiffnesses) are estimated from the Stiffness matrix that was computed from the FEM.
+
+Kx = K(1,1); % [N/m]
+Ky = K(2,2); % [N/m]
+Kz = K(3,3); % [N/m]
+Krx = K(4,4); % [Nm/rad]
+Kry = K(5,5); % [Nm/rad]
+Krz =  K(6,6); % [Nm/rad]
+
+
+
+% The dynamics from the applied force/torque to the measured displacement/rotation of the flexor is identified again for this simpler model.
+
+%% Name of the Simulink File
+mdl = 'flexor_ID16_simplified';
+
+%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/T'], 1, 'openinput');  io_i = io_i + 1;
+io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1;
+
+Gs = linearize(mdl, io);
+
+
+
+% The two obtained dynamics are compared in Figure
+
+
+freqs = logspace(0, 5, 1000);
+
+figure;
+tiledlayout(1, 2, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, abs(squeeze(freqresp(G(1,1), freqs, 'Hz'))), '-', 'DisplayName', '$D_x/F_x$');
+set(gca,'ColorOrderIndex',1)
+plot(freqs, abs(squeeze(freqresp(Gs(1,1), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(G(2,2), freqs, 'Hz'))), '-', 'DisplayName', '$D_y/F_y$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(Gs(2,2), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca,'ColorOrderIndex',3)
+plot(freqs, abs(squeeze(freqresp(G(3,3), freqs, 'Hz'))), '-', 'DisplayName', '$D_z/F_z$');
+set(gca,'ColorOrderIndex',3)
+plot(freqs, abs(squeeze(freqresp(Gs(3,3), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
+hold off;
+legend('location', 'southwest');
+
+ax2 = nexttile;
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, abs(squeeze(freqresp(G(4,4), freqs, 'Hz'))), '-', 'DisplayName', '$R_x/M_x$');
+set(gca,'ColorOrderIndex',1)
+plot(freqs, abs(squeeze(freqresp(Gs(4,4), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(G(5,5), freqs, 'Hz'))), '-', 'DisplayName', '$R_y/M_y$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(Gs(5,5), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca,'ColorOrderIndex',3)
+plot(freqs, abs(squeeze(freqresp(G(6,6), freqs, 'Hz'))), '-', 'DisplayName', '$R_z/M_z$');
+set(gca,'ColorOrderIndex',3)
+plot(freqs, abs(squeeze(freqresp(Gs(6,6), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude [rad/Nm]');
+hold off;
+legend('location', 'southwest');