337 lines
13 KiB
Mathematica
337 lines
13 KiB
Mathematica
|
%% 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])
|