phd-test-bench-flexible-joints/matlab/test_joints_3_bending_stiff_meas.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

%% Colors for the figures
colors = colororder;

% Load Cell Calibration
% In order to estimate the measured errors of the load cell "FC2231", it is compared against another load cell[fn:5].
% The two load cells are measured simultaneously while they are pushed against each other (see Figure ref:fig:test_joints_force_sensor_calib_picture).
% The contact between the two load cells is well defined as one has a spherical interface while the other has a flat surface.

% The measured forces are compared in Figure ref:fig:test_joints_force_sensor_calib_fit.
% The gain mismatch between the two load cells is approximately $4\,\%$ which is higher than what was specified in the data-sheets.
% However, the estimated non-linearity is bellow $1\,\%$ for forces between $0.5\,N$ and $5\,N$.


%% Force Sensor Calibration
% Load measurement data
load('calibration_force_sensor.mat', 't', 'F', 'Fc')

% Remove any offset such that they are both measuring no force when not in contact.
F  = F  - mean(F( t > 0.5 & t < 1.0)); % FC2231 [N]
Fc = Fc - mean(Fc(t > 0.5 & t < 1.0)); % XFL212R [N]

% Only get useful stroke
F  = F( t > 1.55 & t < 2.4);
Fc = Fc(t > 1.55 & t < 2.4);

% Make a line fit
fit_F = polyfit(Fc, F, 1);

% Estimate the gain mismatch
F_gain_mismatch = 100*(1 - fit_F(1)); % in %

% Estimate non-linearity of the sensors
F_non_linearity = 100*(F - (Fc*fit_F(1) + fit_F(2)))./F; % in %

%% Measured two forces and linear fit
figure;
hold on;
plot(Fc, F, 'k-', 'DisplayName', 'Raw Data');
plot(Fc([1,end]), Fc([1,end])*fit_F(1) + fit_F(2), '--', 'DisplayName', 'Line Fit');
hold off;
xlabel('XFL212R [N]'); ylabel('FC2231 [N]');
xlim([0.5,5.5]); ylim([0.5,5.5]);
legend('location', 'southeast', 'FontSize', 8);

% Load Cell Stiffness
% The objective of this measurement is to estimate the stiffness $k_F$ of the force sensor.
% To do so, a stiff element (much stiffer than the estimated $k_F \approx 1\,N/\mu m$) is fixed in front of the force sensor as shown in Figure ref:fig:test_joints_meas_force_sensor_stiffness_picture.
% Then, the force sensor is pushed again this stiff element while the force and the encoder displacement are measured.
% Measured displacement as a function of the force is shown in Figure ref:fig:test_joints_force_sensor_stiffness_fit.
% The load cell stiffness can then be estimated by computing a linear fit, and is found to be $k_F \approx 0.75\,N/\mu m$.


%% Estimaetd load cell stiffness
% Load measurement data
load('force_sensor_stiffness_meas.mat', 't', 'F', 'd')

% Remove offset
F  = F - mean(F(t > 0.5 & t < 1.0));

% Select important part of data
F  = F( t > 4.55 & t < 7.24);
d  = d( t > 4.55 & t < 7.24); d = d - d(1);
t  = t( t > 4.55 & t < 7.24);

% Linear fit
fit_k = polyfit(F, d, 1);

%% Displacement as a function of the measured force
figure;
hold on;
plot(F, 1e6*d, 'k-', 'DisplayName', 'Raw Data');
plot(F([1,end]), 1e6*(F([1,end])*fit_k(1) + fit_k(2)), '--', 'DisplayName', sprintf('Fit, $k_F \\approx %.2f N/\\mu m$', 1e-6/fit_k(1)));
hold off;
xlabel('Force [$N$]'); ylabel('Displacement [$\mu m$]');
xlim([0,45]); ylim([0,60]);
legend('location', 'southeast', 'FontSize', 8);

% Bending Stiffness estimation
% The actual stiffness measurement in now performed by manually moving the translation stage from a start position where the force sensor is not yet in contact with the flexible joint to a position where flexible joint is on its mechanical stop.

% The measured force and displacement as a function of time are shown in Figure ref:fig:test_joints_meas_bend_time.
% Three regions can be observed: first the force sensor tip is not in contact with the flexible joint and the measured force is zero, then the flexible joint deforms linearly, finally the flexible joint comes in contact with the mechanical stops.

% The angular motion $\theta_{y}$ computed from the displacement $d_x$ is displacement as function of the measured torque $T_{y}$ in displayed in Figure ref:fig:test_joints_meas_F_d_lin_fit.
% The bending stiffness of the flexible joint can be estimated by computing the slope of the curve in the linear regime (red dashed line) and is found to be $k_{R_y} = 4.4\,Nm/\text{rad}$.
% The bending stroke can also be estimated as shown in Figure ref:fig:test_joints_meas_F_d_lin_fit and is found to be $\theta_{y,\text{max}} = 20.9\,\text{mrad}$.


%% Estimate the bending stiffness and stroke from the measurement - First Flexible joint
% Load Measured Data for first flexible joint
load('meas_stiff_flex_1_x.mat', 't', 'F', 'd');

% Start measurement at t = 0.2 s
d = d(t > 0.2);
F = F(t > 0.2);
t = t(t > 0.2); t = t - t(1);

% Zero the force
F = F - mean(F(t < 0.2));

% Find when the force sensor touches the flexible joint
i_l_start = find(F > 0.3, 1, 'first');

% Compute torque and angular displacement
h = 22.5e-3; % Height [m]
Tx = h * F; % Applied torque in [Nm]
thetax = atan2(d - d(i_l_start), h); % Measured angle in [rad]

% Find then the maximum torque is applied
[~, i_s_stop] = max(Tx);
% Linear region stops ~ when 90% of the stroke is reached
i_l_stop = find(thetax > 0.9*thetax(i_s_stop), 1, 'first');
% Mechanical "Stop" region start ~20Nmm before maximum torque is applied
i_s_start = find(Tx > max(Tx)-20e-3, 1, 'first');

% Linear fit in the "linear" region
fit_l = polyfit(Tx(i_l_start:i_l_stop), thetax(i_l_start:i_l_stop), 1);

% Linear fit in the "mechanical stop" region
fit_s = polyfit(Tx(i_s_start:i_s_stop), thetax(i_s_start:i_s_stop), 1);

% Reset displacement more precisely based on fit
thetax = thetax - fit_l(2);
fit_s(2) = fit_s(2) - fit_l(2);
fit_l(2) = 0;

%% Estimation of the bending stiffness
kRx_l = 1/fit_l(1); % Bending Stiffness [Nm/rad]
kRx_s = 1/fit_s(1); % Mechanical "Stop" Stiffness [Nm/rad]

%% Estimation of the bending stroke
% This is done by finding the intersection of the two linear fits
theta_max = fit_l(1)*fit_s(2)/(fit_l(1) - fit_s(1)); % Maximum angular stroke [rad]
Tx_at_theta_max = (fit_s(2) - fit_l(2))/(fit_l(1) - fit_s(1));

%% Measured force and displacement as a function of time
figure;
yyaxis left
hold on;
plot(t, F);
plot(0.8*cos(0:0.01:2*pi)+0.8, ...
     2.8*sin(0:0.01:2*pi)-1, 'k--', 'HandleVisibility', 'off');
text(1.8, -1.2, sprintf('Not in\ncontact'), 'horizontalalignment', 'left');
plot(0.4*cos(0:0.01:2*pi)+3, ...
     1.1*sin(0:0.01:2*pi)+4.8, 'k--');
text(3.5, 4.8, sprintf('Mechanical\nStop'), 'horizontalalignment', 'left');
hold off;
ylabel('Force $F_y$ [N]');
ylim([-4, 6])
ylimr = get(gca,'Ylim');

yyaxis right
plot(t, 1e3*d);
xlabel('Time [s]');
ylabel('Displacement $d_y$ [mm]');
xlim([0,5]);
% Make the force and displacement superimpose
ylim([0.364 - 4*(0.8315-0.364)/4.095, 0.364 + 6*(0.8315-0.364)/4.095])

% Regions where to plot the fitted data
Tx_l_fit = [0, Tx_at_theta_max];
Tx_s_fit = [Tx_at_theta_max, Tx(i_s_stop)];

figure;
hold on;
plot(Tx(1:i_s_stop), 1e3*thetax(1:i_s_stop), '-k', 'DisplayName', 'Raw data')
plot(Tx_l_fit, 1e3*(Tx_l_fit*fit_l(1) + fit_l(2)), '--', 'DisplayName', sprintf('$k_{R_x} = %.1f$ Nm/rad', kRx_l))
plot(Tx_s_fit, 1e3*(Tx_s_fit*fit_s(1) + fit_s(2)), '--', 'DisplayName', sprintf('$k_{R_x,stop} = %.0f$ Nm/rad', kRx_s))
plot([0, Tx_at_theta_max], [1e3*theta_max, 1e3*theta_max], 'k--', 'HandleVisibility', 'off')
plot([0, Tx_at_theta_max], [0, 0], 'k--', 'HandleVisibility', 'off')
anArrow = annotation('doublearrow', 'LineWidth', 0.5);
anArrow.Parent = gca;
anArrow.Position = [0.05, 0, 0, 1e3*fit_l(1)*fit_s(2)/(fit_l(1) - fit_s(1))];
text(0.052, 0.4*1e3*fit_l(1)*fit_s(2)/(fit_l(1) - fit_s(1)), sprintf('$\\theta_{x,\\max} = %.1f$ mrad', 1e3*theta_max), 'horizontalalignment', 'left');
hold off;
xlabel('Torque $T_x$ [Nm]');
ylabel('Angle $\theta_x$ [mrad]');
xlim([0, 0.15]);
ylim([-5,25]);
leg = legend('location', 'southeast', 'FontSize', 8);
leg.ItemTokenSize(1) = 15;

% Measured flexible joint stiffnesses
% The same measurement is performed for all the 16 flexible joints, both in the $x$ and $y$ directions.
% The measured angular motion as a function of the applied torque are shown in Figure ref:fig:test_joints_meas_bending_all_raw_data for all the 16 flexible joints.
% This gives a first idea of the dispersion of the measured bending stiffnesses (i.e. slope of the linear region) and of the angular stroke.

% An histogram of the measured bending stiffnesses is show in Figure ref:fig:test_joints_bend_stiff_hist.
% Most of the bending stiffnesses are between $4.6\,Nm/rad$ and $5.0\,Nm/rad$.


%% Measure the bending stiffness and Stroke for all the flexible joints
figure;
hold on;

%% Start with the X-bending
% Initialize variables
Rx = zeros(1,16); % Bending stiffnesses [Nm/rad]
kSx = zeros(1,16); % Bending stiffnesses at "stop" [Nm/rad]
Rmx = zeros(1,16); % Bending stroke [rad]

for i = 1:16
    % Load the data
    load(sprintf('meas_stiff_flex_%i_x.mat', i), 't', 'F', 'd');

    % Start measurement at t = 0.2 s
    d = d(t > 0.2);
    F = F(t > 0.2);
    t = t(t > 0.2); t = t - t(1);

    % Zero the force
    F = F - mean(F(t < 0.2));

    % Find when the force sensor touches the flexible joint
    i_l_start = find(F > 0.3, 1, 'first');

    % Zero the displacement when it comes in contact
    d = d - d(i_l_start);

    % Compute torque and angular displacement
    h = 22.5e-3; % Height [m]
    Tx = h * F; % Applied torque in [Nm]
    thetax = atan2(d, h); % Measured angle in [rad]

    % Find then the maximum torque is applied
    [~, i_s_stop] = max(Tx);
    % Linear region stops ~ when 90% of the stroke is reached
    i_l_stop = find(thetax > 0.9*thetax(i_s_stop), 1, 'first');
    % Mechanical "Stop" region start ~20Nmm before maximum torque is applied
    i_s_start = find(Tx > max(Tx)-20e-3, 1, 'first');

    % Linear fit in the "linear" region
    fit_l = polyfit(Tx(i_l_start:i_l_stop), thetax(i_l_start:i_l_stop), 1);

    % Linear fit in the "mechanical stop" region
    fit_s = polyfit(Tx(i_s_start:i_s_stop), thetax(i_s_start:i_s_stop), 1);

    % Reset displacement more precisely based on fit
    thetax = thetax - fit_l(2);
    fit_s(2) = fit_s(2) - fit_l(2);
    fit_l(2) = 0;

    % Estimation of the bending stiffness and bending stroke
    kRx(i) = 1/fit_l(1); % Bending Stiffness [Nm/rad]
    kSx(i) = 1/fit_s(1); % Mechanical "Stop" Stiffness [Nm/rad]
    Rmx(i) = fit_l(1)*fit_s(2)/(fit_l(1) - fit_s(1)); % Maximum angular stroke [rad]

    if i == 1
        plot(Tx(1:10:i_s_stop), 1e3*thetax(1:10:i_s_stop), '-', 'color', [colors(1,:), 0.4], ...
            'DisplayName', '$k_{R_x}$')
    else
        plot(Tx(1:10:i_s_stop), 1e3*thetax(1:10:i_s_stop), '-', 'color', [colors(1,:), 0.4], ...
            'HandleVisibility', 'off')
    end
end

%% Continue with the Y-bending
% Initialize variables
kRy = zeros(1,16); % Bending stiffnesses [Nm/rad]
kSy = zeros(1,16); % Bending stiffnesses at "stop" [Nm/rad]
Rmy = zeros(1,16); % Bending stroke [rad]

for i = 1:16
    % Load the data
    load(sprintf('meas_stiff_flex_%i_y.mat', i), 't', 'F', 'd');

    % Start measurement at t = 0.2 s
    d = d(t > 0.2);
    F = F(t > 0.2);
    t = t(t > 0.2); t = t - t(1);

    % Zero the force
    F = F - mean(F(t < 0.2));

    % Find when the force sensor touches the flexible joint
    i_l_start = find(F > 0.3, 1, 'first');

    % Zero the displacement when it comes in contact
    d = d - d(i_l_start);

    % Compute torque and angular displacement
    h = 22.5e-3; % Height [m]
    Ty = h * F; % Applied torque in [Nm]
    thetay = atan2(d, h); % Measured angle in [rad]

    % Find then the maximum torque is applied
    [~, i_s_stop] = max(Ty);
    % Linear region stops ~ when 90% of the stroke is reached
    i_l_stop = find(thetay > 0.9*thetay(i_s_stop), 1, 'first');
    % Mechanical "Stop" region start ~20Nmm before maximum torque is applied
    i_s_start = find(Ty > max(Ty)-20e-3, 1, 'first');

    % Linear fit in the "linear" region
    fit_l = polyfit(Ty(i_l_start:i_l_stop), thetay(i_l_start:i_l_stop), 1);

    % Linear fit in the "mechanical stop" region
    fit_s = polyfit(Ty(i_s_start:i_s_stop), thetay(i_s_start:i_s_stop), 1);

    % Reset displacement more precisely based on fit
    thetay = thetay - fit_l(2);
    fit_s(2) = fit_s(2) - fit_l(2);
    fit_l(2) = 0;

    % Estimation of the bending stiffness and bending stroke
    kRy(i) = 1/fit_l(1); % Bending Stiffness [Nm/rad]
    kSy(i) = 1/fit_s(1); % Mechanical "Stop" Stiffness [Nm/rad]
    Rmy(i) = fit_l(1)*fit_s(2)/(fit_l(1) - fit_s(1)); % Maximum angular stroke [rad]

    if i == 1
        plot(Ty(1:10:i_s_stop), 1e3*thetay(1:10:i_s_stop), '-', 'color', [colors(2,:), 0.4], ...
            'DisplayName', '$k_{R_y}$')
    else
        plot(Ty(1:10:i_s_stop), 1e3*thetay(1:10:i_s_stop), '-', 'color', [colors(2,:), 0.4], ...
            'HandleVisibility', 'off')
    end
end

xlabel('Torque $T$ [Nm]');
ylabel('Angle $\theta$ [mrad]');
xlim([0, 0.15]);
ylim([-5,25]);
legend('location', 'southeast', 'FontSize', 8);

figure;
histogram([kRx, kRy], [4:0.2:5])
xlabel('Bending stiffness [Nm/rad]')
xticks([4:0.2:5])