Reworked Flexor parts

This commit is contained in:
Thomas Dehaeze 2020-11-12 17:46:28 +01:00
parent 1a6304f135
commit 1d2ff3b483
25 changed files with 362 additions and 122 deletions

View File

Before

Width:  |  Height:  |  Size: 22 KiB

After

Width:  |  Height:  |  Size: 22 KiB

View File

Before

Width:  |  Height:  |  Size: 27 KiB

After

Width:  |  Height:  |  Size: 27 KiB

View File

Before

Width:  |  Height:  |  Size: 24 KiB

After

Width:  |  Height:  |  Size: 24 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 160 KiB

After

Width:  |  Height:  |  Size: 87 KiB

215
index.org
View File

@ -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
addpath('./data/flexor_ID16/');
#+begin_src matlab :tangle no
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.
** Flexible Joint Characteristics
The most important parameters of the flexible joint can be directly estimated from the stiffness matrix.
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 :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* |
|----------------------------+---------+--------------------------|
| Axial Stiffness [N/um] | 119 | 60 |
| Shear Stiffness [N/um] | 11 | 0 |
| Bending Stiffness [Nm/rad] | 33 | 15 |
| Bending Stiffness [Nm/rad] | 33 | 15 |
| Torsion Stiffness [Nm/rad] | 236 | 20 |
| Total number of Nodes | 2 |
| Number of interface Nodes | 2 |
| Number of Modes | 6 |
| Size of M and K matrices | 18 |
** 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 Ry [Nm/rad] | 33 | 126 |
| Torsion Stiffness Rz [Nm/rad] | 236 | 238 |
| Bending Stiffness Rx [Nm/rad] | 33 | 33 |
| Bending Stiffness Ry [Nm/rad] | 33 | 33 |
| 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,14 +1035,21 @@ 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:
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
@ -1068,11 +1058,16 @@ The two obtained dynamics are compared in Figure
<<matlab-init>>
#+end_src
#+begin_src matlab
addpath('./data/flexor_circ_025/');
#+begin_src matlab :tangle no
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
The most important parameters of the flexible joint can be directly estimated from the stiffness matrix.
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 :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)]', {'Axial Stiffness [N/um]', 'Shear Stiffness [N/um]', 'Bending Stiffness [Nm/rad]', 'Bending Stiffness [Nm/rad]', 'Torsion Stiffness [Nm/rad]'}, {'*Caracteristic*', '*Value*'}, ' %.1f ');
#+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* |
|----------------------------+---------|
| Axial Stiffness [N/um] | 94.0 |
| Shear Stiffness [N/um] | 12.7 |
| Bending Stiffness [Nm/rad] | 4.8 |
| Bending Stiffness [Nm/rad] | 4.8 |
| Torsion Stiffness [Nm/rad] | 260.2 |
| 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 |
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

135
matlab/flexor_025.m Normal file
View File

@ -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');

BIN
matlab/flexor_025.slx Normal file

Binary file not shown.

134
matlab/flexor_ID16.m Normal file
View File

@ -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');

BIN
matlab/flexor_ID16.slx Normal file

Binary file not shown.