135 lines
4.9 KiB
Matlab
135 lines
4.9 KiB
Matlab
%% 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');
|