Reworked Flexor parts
This commit is contained in:
1 parent
1a6304f135
commit
1d2ff3b483
25 files changed
+362
-122
No files matched your search
@@ -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');
|
||||
Binary file not shown.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
@@ -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');
|
||||
Binary file not shown.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
Binary file not shown.
|
Before Width: | Height: | Size: 22 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 27 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 24 KiB |
Reference in new issue
Block a user