Tangled matlab files

matlab/detail_fem_1_flexible_body.m

@@ -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

@@ -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

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