Reworked Flexor parts

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