phd-test-bench-struts/matlab/test_struts_3_simscape_model.m

%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');

%% Path for functions, data and scripts
addpath('./mat/'); % Path for data
addpath('./src/'); % Path for functions

addpath('./STEPS/'); % Path for Simscape Model

%% Linearization options
opts = linearizeOptions;
opts.SampleTime = 0;

%% Open Simscape Model
mdl = 'test_struts_simscape'; % Name of the Simulink File
open(mdl); % Open Simscape Model

%% Colors for the figures
colors = colororder;

%% Input/Output definition of the Model
clear io; io_i = 1;
io(io_i) = linio([mdl, '/u'],  1, 'openinput');  io_i = io_i + 1; % DAC Voltage
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder
io(io_i) = linio([mdl, '/da'], 1, 'openoutput'); io_i = io_i + 1; % Interferometer

%% Frequency vector [Hz]
freqs = logspace(1, log10(2000), 1000);

% Model dynamics
% <<ssec:test_struts_comp_model>>

%% Load measured FRF for comparison
load('meas_struts_frf.mat', 'f', 'enc_frf', 'int_frf', 'iff_frf', 'strut_nums');

%% Initialize strut with 2DoF model for the APA300ML and identify the dynamics
n_hexapod = struct();
n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '4dof');
n_hexapod.flex_top = initializeTopFlexibleJoint('type', '4dof');
n_hexapod.actuator = initializeAPA('type', '2dof');

c_granite = 0; % Do not take into account damping added by the air bearing

% Run the linearization
Gs_2dof = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
Gs_2dof.InputName  = {'u'};
Gs_2dof.OutputName = {'Vs', 'de', 'da'};

%% Initialize strut with "flexible" model for the APA300ML and identify the dynamics
n_hexapod = struct();
n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '4dof');
n_hexapod.flex_top = initializeTopFlexibleJoint('type', '4dof');
n_hexapod.actuator = initializeAPA('type', 'flexible');

c_granite = 100; % Do not take into account damping added by the air bearing

% Run the linearization
Gs_flex = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
Gs_flex.InputName  = {'u'};
Gs_flex.OutputName = {'Vs', 'de', 'da'};



% Two models of the APA300ML are used here for comparison:
% - a simple two degrees of freedom model
% - a model using a super element extracted from a finite element model

% These two models of the APA300ML were tuned to best match measured frequency response functions of the APA alone.
% The flexible joints are here modelled with the 4DoF model (axial stiffness, two bending stiffnesses and one torsion stiffness).

% These two models are compared with the measured frequency responses in Figure ref:fig:test_struts_comp_frf_flexible_model.

% The model dynamics from DAC voltage $u$ to the axial motion of the strut $d_a$ (Figure ref:fig:test_struts_comp_frf_flexible_model_int) and from DAC voltage $u$ to the force sensor voltage $V_s$ (Figure ref:fig:test_struts_comp_frf_flexible_model_iff) are well matching the experimental identification.

% However, the transfer function from $u$ to encoder displacement $d_e$ are not well matching for both models.
% For the 2DoF model, this is normal as the resonances affecting the dynamics are not modelled at all (the APA300ML is modelled as infinitely rigid in all directions except the translation along it's actuation axis).
% For the flexible model, it will be shown in the next section that by adding some misalignment betwen the flexible joints and the APA300ML, this model can better represent the observed dynamics.


%% Compare the FRF and identified dynamics from u to Vs and da
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1a = nexttile([2,1]);
hold on;
plot(f, abs(int_frf(:, 1)), 'color', [0,0,0,0.2], ...
     'DisplayName', 'FRF');
for i = 2:length(strut_nums)
    plot(f, abs(int_frf(:, i)), 'color', [0,0,0,0.2], ...
         'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(Gs_2dof('da', 'u'), freqs, 'Hz'))), '-', ...
     'color', colors(1,:), 'DisplayName', '2DoF Model')
plot(freqs, abs(squeeze(freqresp(Gs_flex('da', 'u'), freqs, 'Hz'))), '-', ...
     'color', colors(2,:), 'DisplayName', 'Flex. Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_a/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2a = nexttile;
hold on;
for i = 1:length(strut_nums)
    plot(f, 180/pi*angle(int_frf(:, i)), 'color', [0,0,0,0.2]);
end
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_2dof('da', 'u'), freqs, 'Hz'))), '-', 'color', colors(1,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_flex('da', 'u'), 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:90:360); ylim([-180, 180]);

linkaxes([ax1a,ax2a],'x');
xlim([10, 2e3]);

%% Compare the FRF and identified dynamics from u to Vs and da
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1a = nexttile([2,1]);
hold on;
plot(f, abs(enc_frf(:, 1)), 'color', [0,0,0,0.2], ...
     'DisplayName', 'FRF');
for i = 2:length(strut_nums)
    plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2], ...
         'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(Gs_2dof('de', 'u'), freqs, 'Hz'))), '-', ...
     'color', colors(1,:), 'DisplayName', '2DoF Model')
plot(freqs, abs(squeeze(freqresp(Gs_flex('de', 'u'), freqs, 'Hz'))), '-', ...
     'color', colors(2,:), 'DisplayName', 'Flex. Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2a = nexttile;
hold on;
for i = 1:length(strut_nums)
    plot(f, 180/pi*angle(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_2dof('de', 'u'), freqs, 'Hz'))), '-', 'color', colors(1,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_flex('de', 'u'), 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:90:360); ylim([-180, 180]);

linkaxes([ax1a,ax2a],'x');
xlim([10, 2e3]);

%% Compare the FRF and identified dynamics from u to Vs and da
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1a = nexttile([2,1]);
hold on;
plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2], ...
     'DisplayName', 'FRF');
for i = 1:length(strut_nums)
    plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2], ...
         'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(Gs_2dof('Vs', 'u'), freqs, 'Hz'))), '-', ...
     'color', colors(1,:), 'DisplayName', '2DoF Model')
plot(freqs, abs(squeeze(freqresp(Gs_flex('Vs', 'u'), freqs, 'Hz'))), '-', ...
     'color', colors(2,:), 'DisplayName', 'Flex. Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2a = nexttile;
hold on;
for i = 1:length(strut_nums)
    plot(f, 180/pi*angle(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_2dof('Vs', 'u'), freqs, 'Hz'))), '-', 'color', colors(1,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_flex('Vs', 'u'), 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:90:360); ylim([-180, 180]);

linkaxes([ax1a,ax2a],'x');
xlim([10, 2e3]);

% Effect of strut misalignment
% <<ssec:test_struts_effect_misalignment>>

% As was shown in Figure ref:fig:test_struts_comp_enc_plants, the identified dynamics from DAC voltage $u$ to encoder measured displacement $d_e$ are very different from one strut to the other.

% In this section, it is investigated whether poor alignment of the strut (flexible joints with respect to the APA) can explain such dynamics.
% For instance, consider Figure ref:fig:test_struts_misalign_schematic where there is a misalignment in the $y$ direction between the two flexible joints (well aligned thanks to the mounting procedure in Section ref:sec:test_struts_mounting) and the APA300ML.
% In such case, the "x-bending" mode at 200Hz (see Figure ref:fig:test_struts_meas_x_bending) can be expected to be more excited, and thus the dynamics from the actuator to the encoder should be affected at frequencies around 200Hz.

% #+name: fig:test_struts_misalign_schematic
% #+caption: Mis-alignement between the joints and the APA
% #+attr_latex: :width 0.8\linewidth
% [[file:figs/test_struts_misalign_schematic.png]]

% To verify this assumption, the dynamics from output DAC voltage $u$ to the measured displacement by the encoder $d_e$ is computed using the Simscape model with flexible APA for several misalignment in the $y$ direction.
% Obtained dynamics are shown in Figure ref:fig:test_struts_effect_misalignment_y.
% The alignment of the APA with the flexible joints as a *huge* influence on the dynamics from actuator voltage to measured displacement by the encoder.
% The misalignment in the $y$ direction mostly influences:
% - the presence of the flexible mode at 200Hz (see mode shape in Figure ref:fig:test_struts_mode_shapes_1)
% - the location of the complex conjugate zero between the first two resonances:
%   - if $d_y < 0$: there is no zero between the two resonances and possibly not even between the second and third ones
%   - if $d_y > 0$: there is a complex conjugate zero between the first two resonances
% - the location of the high frequency complex conjugate zeros at 500Hz (secondary effect, as the axial stiffness of the joint also has large effect on the position of this zero)

% The same can be done for a misalignment in the $x$ direction.
% The obtained dynamics are shown in Figure ref:fig:test_struts_effect_misalignment_x where it is shown that misalignment in the $x$ direction mostly influences the presence of the flexible mode at 300Hz (see mode shape in Figure ref:fig:test_struts_mode_shapes_2).

% Comparing the experimental frequency response functions for all the APA in Figure ref:fig:test_struts_comp_enc_plants with the model dynamics for several $y$ misalignments in Figure ref:fig:test_struts_effect_misalignment_y indicates a clear similarity.
% This similarity suggests that the identified differences in dynamics are caused by the misalignment.


%% Effect of a misalignment in Y-Direction
% Considered misalignment in the Y direction
dy_aligns = [-0.5, -0.1, 0.1, 0.5]*1e-3; % [m]

% Transfer functions from u to de for all the misalignment in y direction
Gs_dy_align = {zeros(length(dy_aligns), 1)};

for i = 1:length(dy_aligns)
    n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align_bot', [0; dy_aligns(i); 0], 'd_align_top', [0; dy_aligns(i); 0]);

    G = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
    G.InputName  = {'u'};
    G.OutputName = {'Vs', 'de', 'da'};

    Gs_dy_align(i) = {G};
end

%% Effect of a misalignment in X-Direction
% Considered misalignment in the X direction
dx_aligns = [-0.1, -0.05, 0.05, 0.1]*1e-3; % [m]

% Transfer functions from u to de for all the misalignment in x direction
Gs_dx_align = {zeros(length(dx_aligns), 1)};

for i = 1:length(dx_aligns)
    n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align_bot', [dx_aligns(i); 0; 0], 'd_align_top', [dx_aligns(i); 0; 0]);

    G = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
    G.InputName  = {'u'};
    G.OutputName = {'Vs', 'de', 'da'};

    Gs_dx_align(i) = {G};
end

%% Transfer function from Vs to de - effect of x-misalignment
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(dy_aligns)
    plot(freqs, abs(squeeze(freqresp(Gs_dy_align{i}('de', 'u'), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$d_y = %.1f$ [mm]', 1e3*dy_aligns(i)));
end
plot(freqs, abs(squeeze(freqresp(Gs_flex('de', 'u'), freqs, 'Hz'))), 'k-', ...
        'DisplayName', 'aligned');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:length(dy_aligns)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_dy_align{i}('de', 'u'), freqs, 'Hz'))));
end
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_flex('de', 'u'), freqs, 'Hz'))), 'k-');
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, 2e3]);

%% Transfer function from Vs to de - effect of x-misalignment
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(dx_aligns)
    plot(freqs, abs(squeeze(freqresp(Gs_dx_align{i}('de', 'u'), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$d_x = %.1f$ [mm]', 1e3*dx_aligns(i)));
end
plot(freqs, abs(squeeze(freqresp(Gs_flex('de', 'u'), freqs, 'Hz'))), 'k-', ...
        'DisplayName', 'aligned');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:length(dx_aligns)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_dx_align{i}('de', 'u'), freqs, 'Hz'))));
end
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_flex('de', 'u'), freqs, 'Hz'))), 'k-');
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, 2e3]);

% Measured strut misalignment
% <<ssec:test_struts_meas_misalignment>>

% During the first mounting of the struts presented in Section ref:sec:test_struts_mounting, the positioning pins used to position the APA with respect to the flexible joints in the $y$ directions were not used (not received at the time).
% Therefore, large $y$ misalignments may be expected.

% In order to estimate the misalignments between the two flexible joints and the APA:
% - the struts are fixed horizontally to the mounting bench as shown in Figure ref:fig:test_struts_mounting_step_3 but without the encoder
% - using a length gauge[fn:2], the height difference from the flexible joints surface and the APA shell surface is measured both for the top and bottom joints and on both sides
% - as the thickness of the flexible joint is $21\,mm$ and the thickness of the APA shell is $20\,mm$, $0.5\,mm$ of height different should be measured is the two are perfectly aligned

% Large variations in the $y$ misalignment are found from one strut to the other (results are summarized in Table ref:tab:test_struts_meas_y_misalignment).

% To check the validity of the measurement, it can be verified that sum of the measured thickness difference on each side is $1\,mm$ (equal to the thickness difference between the flexible joint and the APA).
% This thickness differences for all the struts were found to be between $0.94\,mm$ and $1.00\,mm$ which indicate low errors as compared to the misalignments found in Table ref:tab:test_struts_meas_y_misalignment.


%% Measurement of the y misalignment between the APA and the flexible joints
% Mesured struts
strut_nums = [1, 2, 3, 4, 5];

% Measured height differences in [mm]
% R ("red" side), B ("black" side)
%                R Top  B Top  R Bot  B Bot
strut_align = [[-0.40, -0.60, -0.16, -0.82]   % Strut 1
               [-0.67, -0.30, -0.34, -0.63]   % Strut 2
               [-0.07, -0.88, -0.16, -0.79]   % Strut 3
               [-0.48, -0.46,  0.07, -1.00]   % Strut 4
               [-0.33, -0.64, -0.48, -0.52]]; % Strut 5

% Verification that the thickness difference between the APA shell and the flexible joints is 1mm
thichness_diff_top = strut_align(:,1) + strut_align(:,2); % [mm]
thichness_diff_bot = strut_align(:,1) + strut_align(:,2); % [mm]

% Estimation of the dy misalignment
dy_bot = (strut_align(:,1) - strut_align(:,2))/2; % [mm]
dy_top = (strut_align(:,3) - strut_align(:,4))/2; % [mm]



% #+name: tab:test_struts_meas_y_misalignment
% #+caption: Measured $y$ misalignment at the top and bottom of the APA. Measurements are in $mm$
% #+attr_latex: :environment tabularx :width 0.25\linewidth :align ccc
% #+attr_latex: :center t :booktabs t
% #+RESULTS:
% | *Strut* | *Bot* | *Top* |
% |---------+-------+-------|
% |       1 |   0.1 |  0.33 |
% |       2 | -0.19 |  0.14 |
% |       3 |  0.41 |  0.32 |
% |       4 | -0.01 |  0.54 |
% |       5 |  0.15 |  0.02 |

% By using the measured $y$ misalignment in the Simscape model with the flexible APA model, the measured dynamics from $u$ to $d_e$ can be approached as shown in Figure ref:fig:test_struts_comp_dy_tuned_model_frf_enc.
% Even better match in the dynamics can be obtained by fine tuning both the $x$ and $y$ misalignments (yellow curves in Figure ref:fig:test_struts_comp_dy_tuned_model_frf_enc).


% This confirms that the misalignment between the APA and the strut axis (determined by the two flexible joints) is critical and is inducing large variations in the dynamics from DAC voltage $u$ to encoder measured displacement $d_e$.
% If encoders are to be used when fixed on the struts, it is therefore very important to properly align the APA and the flexible joints when mounting the struts.


% In the next section, the struts are re-assembled with a "positioning pin" to better align the APA with the flexible joints.
% With a better alignment, the amplitude of the spurious resonances are expected to decrease as was shown in Figure ref:fig:test_struts_effect_misalignment_y.


%% Idenfity the dynamics from u to de - misalignement estimated from measurement
Gs_y_align = {zeros(size(strut_align,1), 1)};

% Measured dy alignment
strut_align = 1e-3*[[-0.60, -0.82, -0.40, -0.16]
                    [-0.30, -0.63, -0.67, -0.34]
                    [-0.88, -0.79, -0.07, -0.16]
                    [-0.48,  0.07, -0.46, -1.00]
                    [-0.33, -0.48, -0.64, -0.52]
                    [-0.34, -0.42, -0.63, -0.57]];

for i = 1:size(strut_align,1)
    n_hexapod.actuator = initializeAPA('type', 'flexible', ...
                                       'd_align_bot', [0; strut_align(i, 2) - strut_align(i, 4); 0], ...
                                       'd_align_top', [0; strut_align(i, 1) - strut_align(i, 3); 0]);

    G = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
    G.InputName  = {'u'};
    G.OutputName = {'Vs', 'de', 'da'};

    Gs_y_align(i) = {G};
end

%% Idenfity the dynamics from u to de - misalignement tuned to have the best match
d_aligns = [[-0.05,  -0.3,  0];
            [ 0,      0.5,  0];
            [-0.1,   -0.3,  0];
            [ 0,      0.3,  0];
            [-0.05,   0.05, 0]]'*1e-3;

% Idenfity the transfer function from actuator to encoder for all cases
Gs_xy_align = {zeros(size(d_aligns,2), 1)};

for i = 1:5
    n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align_top', d_aligns(:,i), 'd_align_bot', d_aligns(:,i));

    G = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
    G.InputName  = {'u'};
    G.OutputName = {'Vs', 'de', 'da'};

    Gs_xy_align(i) = {G};
end

%% Comparison of the plants (encoder output) when tuning the misalignment
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(f, abs(enc_frf(:, 1)), 'DisplayName', 'Measurement');
plot(freqs, abs(squeeze(freqresp(Gs_y_align{1}('de', 'u'), freqs, 'Hz'))), ...
     'DisplayName', '$d_y$ from meas');
plot(freqs, abs(squeeze(freqresp(Gs_xy_align{1}('de', 'u'), freqs, 'Hz'))), ...
     'DisplayName', 'Tuned $d_x$, $d_y$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
title('Strut 1');
xticks([1e1, 1e2, 1e3]);

ax2 = nexttile();
hold on;
plot(f, abs(enc_frf(:, 2)), 'DisplayName', 'Measurement');
plot(freqs, abs(squeeze(freqresp(Gs_y_align{2}('de', 'u'), freqs, 'Hz'))), ...
     'DisplayName', '$d_y$ from meas');
plot(freqs, abs(squeeze(freqresp(Gs_xy_align{2}('de', 'u'), freqs, 'Hz'))), ...
     'DisplayName', 'Tuned $d_x$, $d_y$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
title('Strut 2');
xticks([1e1, 1e2, 1e3]);

ax3 = nexttile();
hold on;
plot(f, abs(enc_frf(:, 3)), 'DisplayName', 'Measuremnet');
plot(freqs, abs(squeeze(freqresp(Gs_y_align{3}('de', 'u'), freqs, 'Hz'))), ...
     'DisplayName', '$d_y$ from meas');
plot(freqs, abs(squeeze(freqresp(Gs_xy_align{3}('de', 'u'), freqs, 'Hz'))), ...
     'DisplayName', 'Tuned $d_x$, $d_y$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
title('Strut 3');
xticks([1e1, 1e2, 1e3]);

linkaxes([ax1,ax2,ax3],'xy');
xlim([10, 2e3]); ylim([1e-8, 1e-3]);

% Proper struts alignment
% <<sec:test_struts_meas_all_aligned_struts>>

% After the positioning pins had been received, the struts were mounted again with the positioning pins.
% This should make the APA better aligned with the two flexible joints.

% This alignment is then estimated using a length gauge as in the previous sections.
% Measured $y$ alignments are summarized in Table ref:tab:test_struts_meas_y_misalignment_with_pin and are found to be bellow $55\mu m$ for all the struts which is much better than better (see Table ref:tab:test_struts_meas_y_misalignment).


%% Measurement of the y misalignment between the APA and the flexible joints after strut better alignment

% Numbers of the measured legs
strut_align_nums = [1 2 3 4 5 6];

% Measured height differences in [mm]
% R ("red" side), B ("black" side)
%                R Top  B Top  R Bot  B Bot
strut_align = [[-0.54, -0.50, -0.50, -0.52]   % strut 1
               [-0.44, -0.55, -0.49, -0.49]   % strut 2
               [-0.48, -0.50, -0.50, -0.46]   % strut 3
               [-0.45, -0.51, -0.51, -0.45]   % strut 4
               [-0.50, -0.50, -0.50, -0.50]   % strut 5
               [-0.50, -0.49, -0.43, -0.54]]; % strut 6

% Verification that the thickness difference between the APA shell and the flexible joints is 1mm
thichness_diff_top = strut_align(:,1) + strut_align(:,2); % [mm]
thichness_diff_bot = strut_align(:,1) + strut_align(:,2); % [mm]

% Estimation of the dy misalignment
dy_bot = (strut_align(:,1) - strut_align(:,2))/2; % [mm]
dy_top = (strut_align(:,3) - strut_align(:,4))/2; % [mm]



% #+name: tab:test_struts_meas_y_misalignment_with_pin
% #+caption: Measured $y$ misalignment at the top and bottom of the APA after realigning the struts using a positioning pin. Measurements are in $mm$.
% #+attr_latex: :environment tabularx :width 0.25\linewidth :align ccc
% #+attr_latex: :center t :booktabs t
% #+RESULTS:
% | *Strut* |  *Bot* | *Top* |
% |---------+--------+-------|
% |       1 |  -0.02 |  0.01 |
% |       2 |  0.055 |   0.0 |
% |       3 |   0.01 | -0.02 |
% |       4 |   0.03 | -0.03 |
% |       5 |    0.0 |   0.0 |
% |       6 | -0.005 | 0.055 |

% The dynamics of the re-aligned struts are then measured using the same test bench (Figure ref:fig:test_struts_bench_leg).
% The comparison of the initial strut dynamics and the dynamics of the re-aligned struts (i.e. with the positioning pin) is made in Figure ref:fig:test_struts_comp_enc_frf_realign.

% Even though the struts are now much better aligned, not much improvement can be observed.
% The dynamics of the six aligned struts are quite different from one another.

% Having the encoders fixed to the struts may prove to be difficult to use.
% Therefore, the encoders may be fixed to the nano-hexapod plates instead.


%% New dynamical identified with re-aligned struts
% Load the identification data
leg_noise = {};
for i = 1:length(strut_align_nums)
    leg_noise(i) = {load(sprintf('frf_struts_align_%i_noise.mat', strut_align_nums(i)), 'u', 'Vs', 'de')};
end

% Parameters for Frequency Analysis
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(1/Ts); % Number of points for the FFT computation
win = hanning(Nfft); % Hanning window
Noverlap = floor(Nfft/2); % Overlap between frequency analysis

% Only used to have the frequency vector "f"
[~, f] = tfestimate(leg_noise{1}.u, leg_noise{1}.de, win, Noverlap, Nfft, 1/Ts);

% Transfer function from u to de (encoder)
enc_frf_aligned = zeros(length(f), length(strut_align_nums));

for i = 1:length(strut_align_nums)
    enc_frf_aligned(:, i) = tfestimate(leg_noise{i}.u,    leg_noise{i}.de,    win, Noverlap, Nfft, 1/Ts);
end

%% Bode plot of the FRF from u to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(enc_frf(:, 1)), 'color', [colors(1,:), 0.5], ...
        'DisplayName', 'Initial alignment');
for i = 1:length(strut_nums)
    plot(f, abs(enc_frf(:, i)), 'color', [colors(1,:), 0.5], ...
         'HandleVisibility', 'off');
end
plot(f, abs(enc_frf_aligned(:, 1)), 'color', [colors(2,:), 0.5], ...
        'DisplayName', 'With positioning pin');
for i = 1:length(strut_align_nums)
    plot(f, abs(enc_frf_aligned(:, i)), 'color', [colors(2,:), 0.5], ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
ylim([1e-8, 1e-3]);

ax2 = nexttile;
hold on;
for i = 1:length(strut_nums)
    plot(f, 180/pi*angle(enc_frf(:, i)), 'color', [colors(1,:), 0.5]);
end
for i = 1:length(strut_align_nums)
    plot(f, 180/pi*angle(enc_frf_aligned(:, i)), 'color', [colors(2,:), 0.5]);
end
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, 2e3]);

% Effect of the flexible joint
% <<ssec:test_struts_effect_joint_stiffness>>

% As the struts are composed of one APA and two flexible joints, it is expected that the flexible joint characteristics will change the dynamic behavior of the struts.

% Using the Simscape model, the effect of the flexible joint's characteristics on the dynamics as measured on the test bench are studied.
% The studied dynamics is between $u$ and the encoder displacement $d_e$.

% Let's initialize an APA which is a little bit misaligned.

% APA Initialization
n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align_bot', [0.1e-3; 0.5e-3; 0], 'd_align_top', [0.1e-3; 0.5e-3; 0]);

%% Study the effect of the bending stiffness of the Flexible joints
% Tested bending stiffnesses [Nm/rad]
kRs = [3, 4, 5, 6, 7];

% Idenfity the transfer function from actuator to encoder for all bending stiffnesses
Gs_kRs = {zeros(length(kRs), 1)};

for i = 1:length(kRs)
    n_hexapod.flex_bot = initializeBotFlexibleJoint(...
        'type', '4dof', ...
        'kRx', kRs(i), ...
        'kRy', kRs(i));
    n_hexapod.flex_top = initializeTopFlexibleJoint(...
        'type', '4dof', ...
        'kRx', kRs(i), ...
        'kRy', kRs(i));

    G = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
    G.InputName  = {'u'};
    G.OutputName = {'Vs', 'de', 'da'};

    Gs_kRs(i) = {G};
end

%% Study the effect of the axial stiffness of the Flexible joints
% Tested axial stiffnesses [N/m]
kzs = [5e7 7.5e7 1e8 2.5e8];

% Idenfity the transfer function from actuator to encoder for all bending stiffnesses
Gs_kzs = {zeros(length(kzs), 1)};

for i = 1:length(kzs)
    n_hexapod.flex_bot = initializeBotFlexibleJoint(...
        'type', '4dof', ...
        'kz', kzs(i));
    n_hexapod.flex_top = initializeTopFlexibleJoint(...
        'type', '4dof', ...
        'kz', kzs(i));

    G = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
    G.InputName  = {'u'};
    G.OutputName = {'Vs', 'de', 'da'};

    Gs_kzs(i) = {G};
end

%% Plot the obtained transfer functions for all the bending stiffnesses
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(kRs)
    plot(freqs, abs(squeeze(freqresp(Gs_kRs{i}('de', 'u'), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$k_R = %.0f$ [Nm/rad]', kRs(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile;
hold on;
for i = 1:length(kRs)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_kRs{i}('de', 'u'), freqs, 'Hz'))));
end
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([20, 2e3]);

%% Plot the obtained transfer functions for all the axial stiffnesses
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(kzs)
    plot(freqs, abs(squeeze(freqresp(Gs_kzs{i}('de', 'u'), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$k_z = %.1e$ [N/m]', kzs(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile;
hold on;
for i = 1:length(kzs)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_kzs{i}('de', 'u'), freqs, 'Hz'))));
end
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([20, 2e3]);