tdehaeze/phd-nass-fem
Archived
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Tangled matlab files

Browse Source
This commit is contained in:
Thomas Dehaeze
2025-02-27 10:39:25 +01:00
parent b7f92acc79
commit 9b6dc8f9bf
16 changed files with 1339 additions and 1046 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matlab/detail_fem_1_flexible_body.m
+307
View File
@@ -0,0 +1,307 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
%% Path for functions, data and scripts
addpath('./src/'); % Path for scripts
addpath('./mat/'); % Path for data
addpath('./STEPS/'); % Path for Simscape Model
addpath('./subsystems/'); % Path for Subsystems Simulink files
%% Linearization options
opts = linearizeOptions;
opts.SampleTime = 0;
%% Open Simscape Model
mdl = 'detail_fem_super_element'; % Name of the Simulink File
open(mdl); % Open Simscape Model
%% Colors for the figures
colors = colororder;
freqs = logspace(1,3,500); % Frequency vector [Hz]
%% Estimate "Sensor Constant" - (THP5H)
d33 = 680e-12; % Strain constant [m/V]
n = 160; % Number of layers per stack
eT = 4500*8.854e-12; % Permittivity under constant stress [F/m]
sD = 21e-12; % Compliance under constant electric displacement [m2/N]
gs = d33/(eT*sD*n); % Sensor Constant [V/m]
%% Estimate "Actuator Constant" - (THP5H)
d33 = 680e-12; % Strain constant [m/V]
n = 320; % Number of layers
cE = 1/sD; % Youngs modulus [N/m^2]
A = (10e-3)^2; % Area of the stacks [m^2]
L = 40e-3; % Length of the two stacks [m]
ka = cE*A/L; % Stiffness of the two stacks [N/m]
ga = d33*n*ka; % Actuator Constant [N/V]
%% Load reduced order model
K = readmatrix('APA95ML_K.CSV'); % order: 48
M = readmatrix('APA95ML_M.CSV');
[int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('APA95ML_out_nodes_3D.txt');
%% Stiffness estimation
m = 0.0001; % block-free condition, no payload
k_support = 1e9;
c_support = 1e3;
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fd'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;
G = linearize(mdl, io);
% The inverse of the DC gain of the transfer function
% from vertical force to vertical displacement is the axial stiffness of the APA
k_est = 1/dcgain(G); % [N/m]
%% Estimated compliance of the APA95ML
freqs = logspace(2, log10(5000), 1000);
% Get first resonance indice
i_max = find(abs(squeeze(freqresp(G, freqs(2:end), 'Hz'))) - abs(squeeze(freqresp(G, freqs(1:end-1), 'Hz'))) < 0, 1);
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G, freqs, 'Hz'))), 'DisplayName', 'Compliance');
plot([freqs(1), freqs(end)], [1/k_est, 1/k_est], 'k--', 'DisplayName', sprintf('$1/k$ ($k = %.0f N/\\mu m$)', 1e-6*k_est))
xline(freqs(i_max), '--', 'linewidth', 1, 'color', [0,0,0], 'DisplayName', sprintf('$f_0 = %.0f$ Hz', freqs(i_max)))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([100, 5000]);
%% Estimation of the amplification factor and Stroke
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fa'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/d'], 1, 'openoutput'); io_i = io_i + 1;
G = linearize(mdl, io);
% Estimated amplification factor
ampl_factor = abs(dcgain(G(1,1))./dcgain(G(2,1)));
% Estimated stroke
apa_stroke = ampl_factor * 3 * 20e-6; % [m]
%% Experimental plant identification
% with PD200 amplifier (gain of 20) - 2 stacks as an actuator, 1 as a sensor
load('apa95ml_5kg_2a_1s.mat')
Va = 20*u; % Voltage amplifier gain: 20
% Spectral Analysis parameters
Ts = t(end)/(length(t)-1);
Nfft = floor(1/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);
% Identification of the transfer function from Va to di
[G_y, f] = tfestimate(detrend(Va, 0), detrend(y, 0), win, Noverlap, Nfft, 1/Ts);
[G_Vs, ~] = tfestimate(detrend(Va, 0), detrend(v, 0), win, Noverlap, Nfft, 1/Ts);
%% Plant Identification from Multi-Body model
% Load Reduced Order Matrices
K = readmatrix('APA95ML_K.CSV'); % order: 48
M = readmatrix('APA95ML_M.CSV');
[int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('APA95ML_out_nodes_3D.txt');
m = 5.5; % Mass of the suspended granite [kg]
k_support = 4e7;
c_support = 3e2;
% Compute transfer functions
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Voltage accros piezo stacks [V]
io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor stack voltage [V]
Gm = linearize(mdl, io);
Gm.InputName = {'Va'};
Gm.OutputName = {'y', 'Vs'};
%% Comparison of the identified transfer function from Va to di to the multi-body model
freqs = logspace(1, 3, 500);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_y), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5, 'DisplayName', 'Measured FRF');
plot(freqs, abs(squeeze(freqresp(Gm('y', 'Va'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', 'Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $y/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-5]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(G_y), '-', 'color' , [colors(2,:), 0.5], 'linewidth', 2.5);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm('y', 'Va'), freqs, 'Hz'))), '--', 'color', colors(2,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([-45, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 1e3]);
%% Comparison of the identified transfer function from Va to Vs to the multi-body model
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_Vs), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5, 'DisplayName', 'Measured FRF');
plot(freqs, abs(squeeze(freqresp(Gm('Vs', 'Va'), freqs, 'Hz'))), '--', 'color', colors(1,:), 'DisplayName', 'Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-3, 1e1]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(G_Vs), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm('Vs', 'Va'), freqs, 'Hz'))), '--', 'color', colors(1,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 1e3]);
%% Integral Force Feedback Controller
K_iff = (1/(s + 2*2*pi))*(s/(s + 0.5*2*pi));
K_iff.inputname = {'Vs'};
K_iff.outputname = {'u_iff'};
% New damped plant input
S1 = sumblk("u = u_iff + u_damp");
% Voltage amplifier with gain of 20
voltage_amplifier = tf(20);
voltage_amplifier.inputname = {'u'};
voltage_amplifier.outputname = {'Va'};
%% Load experimental data with IFF implemented with different gains
load('apa95ml_iff_test.mat', 'results');
% Tested gains
g_iff = [0, 10, 50, 100, 500, 1000];
% Spectral Analysis parameters
Ts = t(end)/(length(t)-1);
Nfft = floor(1/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);
%% Computed the identified FRF of the damped plants
tf_iff = {zeros(1, length(g_iff))};
for i=1:length(g_iff)
[tf_est, f] = tfestimate(results{i}.u, results{i}.y, win, Noverlap, Nfft, 1/Ts);
tf_iff(i) = {tf_est};
end
%% Estimate the damped plants from the multi-body model
Gm_iff = {zeros(1, length(g_iff))};
for i=1:length(g_iff)
K_iff_g = -K_iff*g_iff(i); K_iff_g.inputname = {'Vs'}; K_iff_g.outputname = {'u_iff'};
Gm_iff(i) = {connect(Gm, K_iff_g, S1, voltage_amplifier, {'u_damp'}, {'y'})};
end
%% Identify second order plants from the experimental data
% This is mandatory to estimate the experimental "poles"
% an place them in the root-locus plot
G_id = {zeros(1,length(results))};
f_start = 70; % [Hz]
f_end = 500; % [Hz]
for i = 1:length(results)
tf_id = tf_iff{i}(sum(f<f_start):length(f)-sum(f>f_end));
f_id = f(sum(f<f_start):length(f)-sum(f>f_end));
gfr = idfrd(tf_id, 2*pi*f_id, Ts);
G_id(i) = {procest(gfr,'P2UDZ')};
end
%% Comparison of the Root-Locus computed from the multi-body model and the identified closed-loop poles
gains = logspace(0, 5, 1000);
figure;
hold on;
plot(real( pole(Gm('Vs', 'Va'))), imag( pole(Gm('Vs', 'Va'))), 'kx', 'HandleVisibility', 'off');
plot(real(tzero(Gm('Vs', 'Va'))), imag(tzero(Gm('Vs', 'Va'))), 'ko', 'HandleVisibility', 'off');
for i = 1:length(gains)
cl_poles = pole(feedback(Gm('Vs', 'Va'), gains(i)*K_iff));
plot(real(cl_poles(imag(cl_poles)>100)), imag(cl_poles(imag(cl_poles)>100)), 'k.', 'HandleVisibility', 'off');
end
for i = 1:length(g_iff)
cl_poles = pole(Gm_iff{i});
plot(real(cl_poles(imag(cl_poles)>100)), imag(cl_poles(imag(cl_poles)>100)), '.', 'MarkerSize', 20, 'color', colors(i,:), 'HandleVisibility', 'off');
plot(real(pole(G_id{i})), imag(pole(G_id{i})), 'x', 'color', colors(i,:), 'DisplayName', sprintf('g = %0.f', g_iff(i)), 'DisplayName', sprintf('$g = %.0f$', g_iff(i)));
end
xlabel('Real Part');
ylabel('Imaginary Part');
axis equal;
ylim([-100, 2100]);
xlim([-2100,100]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
%% Experimental damped plant for several IFF gains and estimated damped plants from the model
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2, 1]);
hold on;
plot(f, abs(tf_iff{1}), '-', 'DisplayName', '$g = 0$', 'color', [0,0,0, 0.5], 'linewidth', 2.5)
plot(f, abs(squeeze(freqresp(Gm_iff{1}, f, 'Hz'))), 'k--', 'HandleVisibility', 'off')
for i = 2:length(results)
plot(f, abs(tf_iff{i}), '-', 'DisplayName', sprintf('g = %0.f', g_iff(i)), 'color', [colors(i-1,:), 0.5], 'linewidth', 2.5)
end
for i = 2:length(results)
plot(f, abs(squeeze(freqresp(Gm_iff{i}, f, 'Hz'))), '--', 'color', colors(i-1,:), 'HandleVisibility', 'off')
end
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude $y/V_a$ [m/N]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-6, 2e-4]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(tf_iff{1}./squeeze(freqresp(exp(-s*2e-4), f, 'Hz'))), '-', 'color', [0,0,0, 0.5], 'linewidth', 2.5)
plot(f, 180/pi*angle(squeeze(freqresp(Gm_iff{1}, f, 'Hz'))), 'k--')
for i = 2:length(results)
plot(f, 180/pi*angle(tf_iff{i}./squeeze(freqresp(exp(-s*2e-4), f, 'Hz'))), '-', 'color', [colors(i-1,:), 0.5], 'linewidth', 2.5)
plot(f, 180/pi*angle(squeeze(freqresp(Gm_iff{i}, f, 'Hz'))), '--', 'color', colors(i-1,:))
end
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
hold off;
yticks(-360:45:360);
ylim([-10, 190]);
linkaxes([ax1,ax2], 'x');
xlim([150, 500]);
matlab/detail_fem_2_actuators.m
+318
View File
@@ -0,0 +1,318 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
%% Path for functions, data and scripts
addpath('./src/'); % Path for scripts
addpath('./mat/'); % Path for data
addpath('./STEPS/'); % Path for Simscape Model
addpath('./subsystems/'); % Path for Subsystems Simulink files
%% Linearization options
opts = linearizeOptions;
opts.SampleTime = 0;
%% Open Simscape Model
mdl = 'detail_fem_APA300ML'; % Name of the Simulink File
open(mdl); % Open Simscape Model
% Piezoelectric parameters
ga = -25.9; % [N/V]
gs = -5.08e6; % [V/m]
%% Colors for the figures
colors = colororder;
freqs = logspace(1,3,500); % Frequency vector [Hz]
%% Identify dynamics with "Reduced Order Flexible Body"
K = readmatrix('APA300ML_mat_K.CSV');
M = readmatrix('APA300ML_mat_M.CSV');
[int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('APA300ML_out_nodes_3D.txt');
m = 5; % [kg]
ga = 25.9; % [N/V]
gs = 5.08e6; % [V/m]
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Fd'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1;
G_fem = linearize(mdl, io);
G_fem_z = G_fem('z','Va');
G_fem_Vs = G_fem('Vs', 'Va');
G_fem_comp = G_fem('z', 'Fd');
%% Determine c1 and k1 from the zero
G_zeros = zero(minreal(G_fem_Vs));
G_zeros = G_zeros(imag(G_zeros)>0);
[~, i_sort] = sort(imag(G_zeros));
G_zeros = G_zeros(i_sort);
G_zero = G_zeros(1);
% Solving 2nd order equations
c1 = -2*m*real(G_zero);
k1 = m*(imag(G_zero)^2 + real(G_zero)^2);
%% Determine ka, ke, ca, ce from the first pole
G_poles = pole(minreal(G_fem_z));
G_poles = G_poles(imag(G_poles)>0);
[~, i_sort] = sort(imag(G_poles));
G_poles = G_poles(i_sort);
G_pole = G_poles(1);
% Solving 2nd order equations
ce = 3*(-2*m*real(G_pole(1)) - c1);
ca = 1/2*ce;
ke = 3*(m*(imag(G_pole)^2 + real(G_pole)^2) - k1);
ka = 1/2*ke;
%% Matching sensor/actuator constants
% ga = dcgain(G_fem_z) / (1/(ka + k1*ke/(k1 + ke)));
clear io; io_i = 1;
io(io_i) = linio([mdl, '_2dof', '/Fa'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '_2dof', '/z'], 1, 'openoutput'); io_i = io_i + 1;
ga = dcgain(G_fem_z)/dcgain(linearize([mdl, '_2dof'], io));
clear io; io_i = 1;
io(io_i) = linio([mdl, '_2dof', '/Va'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '_2dof', '/dL'], 1, 'openoutput'); io_i = io_i + 1;
gs = dcgain(G_fem_Vs)/dcgain(linearize([mdl, '_2dof'], io));
%% Identify dynamics with tuned 2DoF model
clear io; io_i = 1;
io(io_i) = linio([mdl, '_2dof', '/Va'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '_2dof', '/Fd'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '_2dof', '/z'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '_2dof', '/Vs'], 1, 'openoutput'); io_i = io_i + 1;
G_2dof = linearize([mdl, '_2dof'], io);
G_2dof_z = G_2dof('z','Va');
G_2dof_Vs = G_2dof('Vs', 'Va');
G_2dof_comp = G_2dof('z', 'Fd');
%% Comparison of the transfer functions from Va to vertical motion - FEM vs 2DoF
freqs = logspace(1, 3, 500);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_fem_z, freqs, 'Hz'))), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5, 'DisplayName', 'FEM')
plot(freqs, abs(squeeze(freqresp(G_2dof_z, freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '2DoF Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $y/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 2e-4]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_fem_z, freqs, 'Hz')))), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_2dof_z, freqs, 'Hz')))), '--', 'color', colors(2,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360); ylim([-20, 200]);
linkaxes([ax1,ax2],'x');
xlim([10, 1e3]);
%% Comparison of the transfer functions from Va to Vs - FEM vs 2DoF
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_fem_Vs, freqs, 'Hz'))), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5, 'DisplayName', 'FEM');
plot(freqs, abs(squeeze(freqresp(G_2dof_Vs, freqs, 'Hz'))), '--', 'color', colors(1,:), 'DisplayName', '2DoF Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([6e-4, 3e1]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_fem_Vs, freqs, 'Hz')))), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_2dof_Vs, freqs, 'Hz')))), '--', 'color', colors(1,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360); ylim([-20, 200]);
linkaxes([ax1,ax2],'x');
xlim([10, 1e3]);
%% Effect of electrical boundaries on the
oc = load('detail_fem_apa95ml_open_circuit.mat', 't', 'encoder', 'u');
sc = load('detail_fem_apa95ml_short_circuit.mat', 't', 'encoder', 'u');
% Spectral Analysis parameters
Ts = sc.t(end)/(length(sc.t)-1);
Nfft = floor(2/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);
% Identification of the transfer function from Va to di
[G_oc, f] = tfestimate(detrend(oc.u, 0), detrend(oc.encoder, 0), win, Noverlap, Nfft, 1/Ts);
[G_sc, f] = tfestimate(detrend(sc.u, 0), detrend(sc.encoder, 0), win, Noverlap, Nfft, 1/Ts);
% Find resonance frequencies
[~, i_oc] = max(abs(G_oc(f<300)));
[~, i_sc] = max(abs(G_sc(f<300)));
%% Effect of the electrical bondaries of the force sensor stack on the APA95ML resonance frequency
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_oc), '-', 'DisplayName', sprintf('Open-Circuit - $f_0 = %.1f Hz$', f(i_oc)))
plot(f, abs(G_sc), '-', 'DisplayName', sprintf('Short-Circuit - $f_0 = %.1f Hz$', f(i_sc)))
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-6, 1e-4]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(G_oc), '-')
plot(f, 180/pi*angle(G_sc), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;
yticks(-360:45:360);
ylim([-20, 200]);
axis padded 'auto x'
linkaxes([ax1,ax2], 'x');
xlim([100, 300]);
%% Compare Dynamics between "Reduced Order" flexible joints and "2-dof and 3-dof" joints
% Let's initialize all the stages with default parameters.
initializeGround('type', 'rigid');
initializeGranite('type', 'rigid');
initializeTy('type', 'rigid');
initializeRy('type', 'rigid');
initializeRz('type', 'rigid');
initializeMicroHexapod('type', 'rigid');
initializeSample('m', 50);
initializeSimscapeConfiguration();
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();
mdl = 'detail_fem_nass';
% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Errors in the frame of the struts
io(io_i) = linio([mdl, '/NASS'], 3, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensors
% Flexible actuators
initializeSimplifiedNanoHexapod('actuator_type', 'flexible', ...
'flex_type_F', '2dof', ...
'flex_type_M', '3dof');
G_flex = linearize(mdl, io);
G_flex.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_flex.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6', 'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
% Actuators modeled as 2DoF system
initializeSimplifiedNanoHexapod('actuator_type', 'apa300ml', ...
'flex_type_F', '2dof', ...
'flex_type_M', '3dof');
G_ideal = linearize(mdl, io);
G_ideal.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_ideal.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6', 'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
%% Comparison of the dynamics for actuators modeled using "reduced order flexible body" and using 2DoF system - HAC plant
freqs = logspace(1, 4, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for j = 1:5
for k = j+1:6
plot(freqs, abs(squeeze(freqresp(G_flex("l"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_ideal("l"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
'HandleVisibility', 'off');
end
end
plot(freqs, abs(squeeze(freqresp(G_flex("l1","f1"), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'FEM');
plot(freqs, abs(squeeze(freqresp(G_ideal("l1","f1"), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', '2-DoF');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([1e-10, 1e-4]);
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_flex("l1","f1"), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ideal("l1","f1"), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([-360:45:360]);
linkaxes([ax1,ax2],'x');
%% Comparison of the dynamics for actuators modeled using "reduced order flexible body" and using 2DoF system - IFF plant
freqs = logspace(0, 3, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for j = 1:5
for k = j+1:6
plot(freqs, abs(squeeze(freqresp(G_flex("fm"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_ideal("fm"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
'HandleVisibility', 'off');
end
end
plot(freqs, abs(squeeze(freqresp(G_flex("fm1","f1"), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'FEM');
plot(freqs, abs(squeeze(freqresp(G_ideal("fm1","f1"), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', '2-DoF');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([1e-5, 1e1]);
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_flex("fm1","f1"), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ideal("fm1","f1"), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([-360:45:360]);
linkaxes([ax1,ax2],'x');
matlab/detail_fem_3_flexible_joints.m
+623
View File
@@ -0,0 +1,623 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
%% Path for functions, data and scripts
addpath('./src/'); % Path for scripts
addpath('./mat/'); % Path for data
addpath('./STEPS/'); % Path for Simscape Model
addpath('./subsystems/'); % Path for Subsystems Simulink files
%% Linearization options
opts = linearizeOptions;
opts.SampleTime = 0;
%% Open Simscape Model
mdl = 'detail_fem_nass'; % Name of the Simulink File
open(mdl); % Open Simscape Model
%% Colors for the figures
colors = colororder;
freqs = logspace(1,3,500); % Frequency vector [Hz]
%% Identify the dynamics for several considered bending stiffnesses
% Let's initialize all the stages with default parameters.
initializeGround('type', 'rigid');
initializeGranite('type', 'rigid');
initializeTy('type', 'rigid');
initializeRy('type', 'rigid');
initializeRz('type', 'rigid');
initializeMicroHexapod('type', 'rigid');
initializeSample('m', 50);
initializeSimscapeConfiguration();
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();
% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Errors in the frame of the struts
io(io_i) = linio([mdl, '/NASS'], 3, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensors
% Effect of bending stiffness
Kf = [0, 50, 100, 500]; % [Nm/rad]
G_Kf = {zeros(length(Kf), 1)};
for i = 1:length(Kf)
% Limited joint axial compliance
initializeSimplifiedNanoHexapod('actuator_type', '1dof', ...
'flex_type_F', '2dof', ...
'flex_type_M', '3dof', ...
'actuator_k', 1e6, ...
'actuator_c', 1e1, ...
'actuator_kp', 0, ...
'actuator_cp', 0, ...
'Fsm', 56e-3, ... % APA300ML weight 112g
'Msm', 56e-3, ...
'Kf_F', Kf(i), ...
'Kf_M', Kf(i));
G_Kf(i) = {linearize(mdl, io)};
G_Kf{i}.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_Kf{i}.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6', 'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
end
freqs = logspace(0, 3, 1000);
%% Effect of the flexible joint bending stiffness on the HAC-plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(Kf)
for j = 1:5
for k = j+1:6
plot(freqs, abs(squeeze(freqresp(G_Kf{i}("l"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(i,:), 0.1], ...
'HandleVisibility', 'off');
end
end
plot(freqs, abs(squeeze(freqresp(G_Kf{i}("l1","f1"), freqs, 'Hz'))), 'color', colors(i,:), 'DisplayName', sprintf('$k_f = %.0f $ [Nm/rad]', Kf(i)));
for j = 2:6
plot(freqs, abs(squeeze(freqresp(G_Kf{i}("l"+j,"f"+j), freqs, 'Hz'))), 'color', colors(i,:), 'HandleVisibility', 'off');
end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([1e-10, 1e-4]);
ax2 = nexttile;
hold on;
for i = 1:length(Kf)
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_Kf{i}(1, 1), freqs, 'Hz')))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-360:45:360]);
linkaxes([ax1,ax2],'x');
%% Effect of the flexible joint bending stiffness on the IFF plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(Kf)
for j = 1:5
for k = j+1:6
plot(freqs, abs(squeeze(freqresp(G_Kf{i}("fm"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(i,:), 0.1], ...
'HandleVisibility', 'off');
end
end
plot(freqs, abs(squeeze(freqresp(G_Kf{i}("fm1","f1"), freqs, 'Hz'))), 'color', colors(i,:), 'DisplayName', sprintf('$k_f = %.0f $ [Nm/rad]', Kf(i)));
for j = 2:6
plot(freqs, abs(squeeze(freqresp(G_Kf{i}("fm"+j,"f"+j), freqs, 'Hz'))), 'color', colors(i,:), 'HandleVisibility', 'off');
end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([1e-4, 1e2]);
ax2 = nexttile();
hold on;
for i = 1:length(Kf)
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_Kf{i}("fm1", "f1"), freqs, 'Hz')))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([-360:45:360]);
linkaxes([ax1,ax2],'x');
%% Decentalized IFF
Kiff = -200 * ... % Gain
1/s * ... % LPF: provides integral action
eye(6); % Diagonal 6x6 controller (i.e. decentralized)
Kiff.InputName = {'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
Kiff.OutputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
%% Root Locus for decentralized IFF - 1dof actuator - Effect of joint bending stiffness
gains = logspace(-1, 2, 400);
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
for i = 1:length(Kf)
plot(real(pole(G_Kf{i}({"fm1", "fm2", "fm3", "fm4", "fm5", "fm6"}, {"f1", "f2", "f3", "f4", "f5", "f6"}))), imag(pole(G_Kf{i}({"fm1", "fm2", "fm3", "fm4", "fm5", "fm6"}, {"f1", "f2", "f3", "f4", "f5", "f6"}))), 'x', 'color', colors(i,:), ...
'DisplayName', sprintf('$k_f = %.0f$ Nm/rad', Kf(i)));
plot(real(tzero(G_Kf{i}({"fm1", "fm2", "fm3", "fm4", "fm5", "fm6"}, {"f1", "f2", "f3", "f4", "f5", "f6"}))), imag(tzero(G_Kf{i}({"fm1", "fm2", "fm3", "fm4", "fm5", "fm6"}, {"f1", "f2", "f3", "f4", "f5", "f6"}))), 'o', 'color', colors(i,:), ...
'HandleVisibility', 'off');
for g = gains
clpoles = pole(feedback(G_Kf{i}({"fm1", "fm2", "fm3", "fm4", "fm5", "fm6"}, {"f1", "f2", "f3", "f4", "f5", "f6"}), g*Kiff, +1));
plot(real(clpoles), imag(clpoles), '.', 'color', colors(i,:), ...
'HandleVisibility', 'off');
end
end
xline(0, 'HandleVisibility', 'off'); yline(0, 'HandleVisibility', 'off');
hold off;
axis equal;
xlim(1.1*[-900, 100]); ylim(1.1*[-100, 900]);
xticks(1.1*[-900:100:0]);
yticks(1.1*[0:100:900]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
%% Identify the dynamics for several considered bending stiffnesses - APA300ML
G_Kf_apa300ml = {zeros(length(Kf), 1)};
for i = 1:length(Kf)
% Limited joint axial compliance
initializeSimplifiedNanoHexapod('actuator_type', 'apa300ml', ...
'flex_type_F', '2dof', ...
'flex_type_M', '3dof', ...
'Fsm', 56e-3, ... % APA300ML weight 112g
'Msm', 56e-3, ...
'Kf_F', Kf(i), ...
'Kf_M', Kf(i));
G_Kf_apa300ml(i) = {linearize(mdl, io)};
G_Kf_apa300ml{i}.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_Kf_apa300ml{i}.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6', 'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
end
Kiff = -1000 * ... % Gain
1/(s) * ... % LPF: provides integral action
eye(6); % Diagonal 6x6 controller (i.e. decentralized)
Kiff.InputName = {'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
Kiff.OutputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
%% Root Locus for decentralized IFF - APA300ML actuator - Effect of joint bending stiffness
gains = logspace(-1, 2, 300);
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
for i = 1:length(Kf)
plot(real(pole(G_Kf_apa300ml{i}({"fm1", "fm2", "fm3", "fm4", "fm5", "fm6"}, {"f1", "f2", "f3", "f4", "f5", "f6"}))), imag(pole(G_Kf_apa300ml{i}({"fm1", "fm2", "fm3", "fm4", "fm5", "fm6"}, {"f1", "f2", "f3", "f4", "f5", "f6"}))), 'x', 'color', colors(i,:), ...
'DisplayName', sprintf('$k_f = %.0f$ [Nm/rad]', Kf(i)));
plot(real(tzero(G_Kf_apa300ml{i}({"fm1", "fm2", "fm3", "fm4", "fm5", "fm6"}, {"f1", "f2", "f3", "f4", "f5", "f6"}))), imag(tzero(G_Kf_apa300ml{i}({"fm1", "fm2", "fm3", "fm4", "fm5", "fm6"}, {"f1", "f2", "f3", "f4", "f5", "f6"}))), 'o', 'color', colors(i,:), ...
'HandleVisibility', 'off');
for g = gains
clpoles = pole(feedback(G_Kf_apa300ml{i}({"fm1", "fm2", "fm3", "fm4", "fm5", "fm6"}, {"f1", "f2", "f3", "f4", "f5", "f6"}), g*Kiff, +1));
plot(real(clpoles), imag(clpoles), '.', 'color', colors(i,:), ...
'HandleVisibility', 'off');
end
end
xline(0, 'HandleVisibility', 'off'); yline(0, 'HandleVisibility', 'off');
hold off;
axis equal;
xlim(1.4*[-900, 100]); ylim(1.4*[-100, 900]);
xticks(1.4*[-900:100:0]);
yticks(1.4*[0:100:900]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
%% Identify the dynamics for several considered axial stiffnesses
% Let's initialize all the stages with default parameters.
initializeGround('type', 'rigid');
initializeGranite('type', 'rigid');
initializeTy('type', 'rigid');
initializeRy('type', 'rigid');
initializeRz('type', 'rigid');
initializeMicroHexapod('type', 'rigid');
initializeSample('m', 50);
initializeSimscapeConfiguration();
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();
% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Errors in the frame of the struts
io(io_i) = linio([mdl, '/NASS'], 3, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensors
% Effect of bending stiffness
Ka = 1e6*[1000, 100, 10, 1]; % [Nm/rad]
G_Ka = {zeros(length(Ka), 1)};
for i = 1:length(Ka)
% Limited joint axial compliance
initializeSimplifiedNanoHexapod('actuator_type', '1dof', ...
'flex_type_F', '2dof_axial', ...
'flex_type_M', '4dof', ...
'actuator_k', 1e6, ...
'actuator_c', 1e1, ...
'actuator_kp', 0, ...
'actuator_cp', 0, ...
'Fsm', 56e-3, ... % APA300ML weight 112g
'Msm', 56e-3, ...
'Ca_F', 1, ...
'Ca_M', 1, ...
'Ka_F', Ka(i), ...
'Ka_M', Ka(i));
G_Ka(i) = {linearize(mdl, io)};
G_Ka{i}.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_Ka{i}.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6', 'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
end
freqs = logspace(1, 4, 1000);
%% Effect of the flexible joint axial stiffness on the HAC-plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(Ka)
for j = 1:5
for k = j+1:6
plot(freqs, abs(squeeze(freqresp(G_Ka{i}("l"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(i,:), 0.1], ...
'HandleVisibility', 'off');
end
end
end
for i = 1:length(Ka)
plot(freqs, abs(squeeze(freqresp(G_Ka{i}("l1","f1"), freqs, 'Hz'))), 'color', colors(i,:), 'DisplayName', sprintf('$k_a = %.0f$ [N/$\\mu$m]', 1e-6*Ka(i)));
% for j = 2:6
% plot(freqs, abs(squeeze(freqresp(G_Ka{i}("l"+j,"f"+j), freqs, 'Hz'))), 'color', colors(i,:), 'HandleVisibility', 'off');
% end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([1e-10, 1e-4]);
ax2 = nexttile;
hold on;
for i = 1:length(Ka)
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_Ka{i}(1, 1), freqs, 'Hz')))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-360:45:360]);
linkaxes([ax1,ax2],'x');
%% Effect of the flexible joint axial stiffness on the IFF plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(Ka)
for j = 1:5
for k = j+1:6
plot(freqs, abs(squeeze(freqresp(G_Ka{i}("fm"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(i,:), 0.1], ...
'HandleVisibility', 'off');
end
end
end
for i = 1:length(Ka)
plot(freqs, abs(squeeze(freqresp(G_Ka{i}("fm1","f1"), freqs, 'Hz'))), 'color', colors(i,:), 'DisplayName', sprintf('$k_a = %.0f$ [N/$\\mu$m]', 1e-6*Ka(i)));
% for j = 2:6
% plot(freqs, abs(squeeze(freqresp(G_Ka{i}("fm"+j,"f"+j), freqs, 'Hz'))), 'color', colors(i,:), 'HandleVisibility', 'off');
% end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([1e-4, 1e2]);
ax2 = nexttile();
hold on;
for i = 1:length(Ka)
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_Ka{i}("fm1", "f1"), freqs, 'Hz')))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([-360:45:360]);
linkaxes([ax1,ax2],'x');
%% Decentalized IFF
Kiff = -200 * ... % Gain
1/s * ... % LPF: provides integral action
eye(6); % Diagonal 6x6 controller (i.e. decentralized)
Kiff.InputName = {'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
Kiff.OutputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
%% Root Locus for decentralized IFF - 1dof actuator - Effect of joint bending stiffness
gains = logspace(-1, 2, 400);
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
for i = 1:length(Ka)
plot(real(pole(G_Ka{i}({"fm1", "fm2", "fm3", "fm4", "fm5", "fm6"}, {"f1", "f2", "f3", "f4", "f5", "f6"}))), imag(pole(G_Ka{i}({"fm1", "fm2", "fm3", "fm4", "fm5", "fm6"}, {"f1", "f2", "f3", "f4", "f5", "f6"}))), 'x', 'color', colors(i,:), ...
'DisplayName', sprintf('$k_a = %.0f$ N/$\\mu$m', 1e-6*Ka(i)));
plot(real(tzero(G_Ka{i}({"fm1", "fm2", "fm3", "fm4", "fm5", "fm6"}, {"f1", "f2", "f3", "f4", "f5", "f6"}))), imag(tzero(G_Ka{i}({"fm1", "fm2", "fm3", "fm4", "fm5", "fm6"}, {"f1", "f2", "f3", "f4", "f5", "f6"}))), 'o', 'color', colors(i,:), ...
'HandleVisibility', 'off');
for g = gains
clpoles = pole(feedback(G_Ka{i}({"fm1", "fm2", "fm3", "fm4", "fm5", "fm6"}, {"f1", "f2", "f3", "f4", "f5", "f6"}), g*Kiff, +1));
plot(real(clpoles), imag(clpoles), '.', 'color', colors(i,:), ...
'HandleVisibility', 'off');
end
end
xline(0, 'HandleVisibility', 'off'); yline(0, 'HandleVisibility', 'off');
hold off;
axis equal;
xlim(1.1*[-900, 100]); ylim(1.1*[-100, 900]);
xticks(1.1*[-900:100:0]);
yticks(1.1*[0:100:900]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
%% Compute the damped plants
Kiff = -500 * ... % Gain
1/(s + 2*pi*0.1) * ... % LPF: provides integral action
eye(6); % Diagonal 6x6 controller (i.e. decentralized)
Kiff.InputName = {'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
Kiff.OutputName = {'u1iff', 'u2iff', 'u3iff', 'u4iff', 'u5iff', 'u6iff'};
% New damped plant input
S1 = sumblk("f1 = u1iff + u1");
S2 = sumblk("f2 = u2iff + u2");
S3 = sumblk("f3 = u3iff + u3");
S4 = sumblk("f4 = u4iff + u4");
S5 = sumblk("f5 = u5iff + u5");
S6 = sumblk("f6 = u6iff + u6");
G_Ka_iff = {zeros(1,length(Ka))};
for i=1:length(Ka)
G_Ka_iff(i) = {connect(G_Ka{i}, Kiff, S1, S2, S3, S4, S5, S6, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}, {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'})};
end
%% Interaction Analysis - RGA Number
rga = zeros(length(Ka), length(freqs));
for i = 1:length(Ka)
for j = 1:length(freqs)
rga(i,j) = sum(sum(abs(inv(evalfr(G_Ka_iff{i}({"l1", "l2", "l3", "l4", "l5", "l6"}, {"u1", "u2", "u3", "u4", "u5", "u6"}), 1j*2*pi*freqs(j)).').*evalfr(G_Ka_iff{i}({"l1", "l2", "l3", "l4", "l5", "l6"}, {"u1", "u2", "u3", "u4", "u5", "u6"}), 1j*2*pi*freqs(j)) - eye(6))));
end
end
%% RGA number for the damped plants - Effect of the flexible joint axial stiffness
figure;
hold on;
for i = 1:length(Ka)
plot(freqs, rga(i,:), 'DisplayName', sprintf('$k_a = %.0f$ N/$\\mu$m', 1e-6*Ka(i)))
end
hold off;
xlabel('Frequency [Hz]'); ylabel('RGA number');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylim([0, 10]); xlim([10, 5e3]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
%% Extract stiffness of the joint from the reduced order model
% We first extract the stiffness and mass matrices.
K = readmatrix('flex025_mat_K.CSV');
M = readmatrix('flex025_mat_M.CSV');
% Then, we extract the coordinates of the interface nodes.
[int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('flex025_out_nodes_3D.txt');
m = 1;
%% Name of the Simulink File
mdl = 'detail_fem_joint';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/T'], 1, 'openinput'); io_i = io_i + 1; % Forces and Torques
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Translations and Rotations
G = linearize(mdl, io);
% Stiffness extracted from the Simscape model
k_a = 1/dcgain(G(3,3)); % Axial stiffness [N/m]
k_f = 1/dcgain(G(4,4)); % Bending stiffness [N/m]
k_t = 1/dcgain(G(6,6)); % Torsion stiffness [N/m]
% Stiffness extracted from the Stiffness matrix
k_s = K(1,1); % shear [N/m]
% k_s = K(2,2); % shear [N/m]
k_a = K(3,3); % axial [N/m]
k_f = K(4,4); % bending [Nm/rad]
% k_f = K(5,5); % bending [Nm/rad]
k_t = K(6,6); % torsion [Nm/rad]
%% Compare Dynamics between "Reduced Order" flexible joints and "2-dof and 3-dof" joints
% Let's initialize all the stages with default parameters.
initializeGround('type', 'rigid');
initializeGranite('type', 'rigid');
initializeTy('type', 'rigid');
initializeRy('type', 'rigid');
initializeRz('type', 'rigid');
initializeMicroHexapod('type', 'rigid');
initializeSample('m', 50);
initializeSimscapeConfiguration();
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();
mdl = 'detail_fem_nass';
% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Errors in the frame of the struts
io(io_i) = linio([mdl, '/NASS'], 3, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensors
% Fully flexible joints
initializeSimplifiedNanoHexapod('actuator_type', 'apa300ml', ...
'flex_type_F', 'flexible', ...
'flex_type_M', 'flexible', ...
'Fsm', 56e-3, ... % APA300ML weight 112g
'Msm', 56e-3);
G_flex = linearize(mdl, io);
G_flex.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_flex.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6', 'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
% Flexible joints modelled by 2DoF and 3DoF joints
initializeSimplifiedNanoHexapod('actuator_type', 'apa300ml', ...
'flex_type_F', '2dof_axial', ...
'flex_type_M', '4dof', ...
'Kf_F', k_f, ...
'Kt_F', k_t, ...
'Ka_F', k_a, ...
'Kf_M', k_f, ...
'Kt_M', k_t, ...
'Ka_M', k_a, ...
'Cf_F', 1e-2, ...
'Ct_F', 1e-2, ...
'Ca_F', 1e-2, ...
'Cf_M', 1e-2, ...
'Ct_M', 1e-2, ...
'Ca_M', 1e-2, ...
'Fsm', 56e-3, ... % APA300ML weight 112g
'Msm', 56e-3);
G_ideal = linearize(mdl, io);
G_ideal.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_ideal.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6', 'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
%% Comparison of the dynamics with joints modelled with FEM and modelled with "ideal joints" - HAC plant
freqs = logspace(1, 4, 1000);
%% Effect of the flexible joint axial stiffness on the HAC-plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for j = 1:5
for k = j+1:6
plot(freqs, abs(squeeze(freqresp(G_flex("l"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_ideal("l"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
'HandleVisibility', 'off');
end
end
plot(freqs, abs(squeeze(freqresp(G_flex("l1","f1"), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'Reduced Order Flexible Joints');
plot(freqs, abs(squeeze(freqresp(G_ideal("l1","f1"), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', 'Bot: $k_f$, $k_a$, Top: $k_f$, $k_t$, $k_a$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([1e-10, 1e-4]);
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_flex("l1","f1"), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ideal("l1","f1"), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([-360:45:360]);
linkaxes([ax1,ax2],'x');
freqs = logspace(0, 3, 1000);
%% Effect of the flexible joint axial stiffness on the HAC-plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for j = 1:5
for k = j+1:6
plot(freqs, abs(squeeze(freqresp(G_flex("fm"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_ideal("fm"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
'HandleVisibility', 'off');
end
end
plot(freqs, abs(squeeze(freqresp(G_flex("fm1","f1"), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'Reduced Order Flexible Joints');
plot(freqs, abs(squeeze(freqresp(G_ideal("fm1","f1"), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', 'Bot: $k_f$, $k_a$, Top: $k_f$, $k_t$, $k_a$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([1e-5, 1e1]);
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_flex("fm1","f1"), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ideal("fm1","f1"), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([-360:45:360]);
linkaxes([ax1,ax2],'x');
matlab/detail_fem_APA300ML.slx
BIN
View File
Binary file not shown.
matlab/detail_fem_super_element.slx
BIN
View File
Binary file not shown.
matlab/mat/detail_fem_apa95ml_open_circuit.mat
BIN
View File
Binary file not shown.
matlab/mat/detail_fem_apa95ml_short_circuit.mat
BIN
View File
Binary file not shown.
matlab/mat/nass_model_controller.mat
BIN
View File
Binary file not shown.
matlab/mat/nass_model_disturbances.mat
BIN
View File
Binary file not shown.
matlab/mat/nass_model_stages.mat
BIN
View File
Binary file not shown.