%% 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;

% Finite Element Model
% From the Finite Element Model, the stiffness and stroke of the flexible joint have been computed and summarized in Tables ref:tab:test_joints_axial_shear_prop and ref:tab:test_joints_bending_torsion_prop.


%% Stiffness
ka = 94e6; % Axial Stiffness [N/m]
ks = 13e6; % Shear Stiffness [N/m]
kb = 5;    % Bending Stiffness [Nm/rad]
kt = 260;  % Torsional Stiffness [Nm/rad]

%% Maximum force
Fa = 469;    % Axial Force before yield [N]
Fs = 242;    % Shear Force before yield [N]
Fb = 0.118;  % Bending Force before yield [Nm]
Ft = 1.508; % Torsional Force before yield [Nm]

%% Compute the corresponding stroke
Xa = Fa/ka; % Axial Stroke before yield [m]
Xs = Fs/ks; % Shear Stroke before yield [m]
Xb = Fb/kb; % Bending Stroke before yield [rad]
Xt = Ft/kt; % Torsional Stroke before yield [rad]

% Setup

% The setup is schematically represented in Figure ref:fig:test_joints_bench_side_bis.

% The force is applied on top of the flexible joint with a distance $h$ with the joint's center.
% The displacement of the flexible joint is also measured at the same height.

% The height between the joint's center and the force application point is:

h = 25e-3; % Height [m]

% Estimation error due to force sensor compression
% The measured displacement is not done directly at the joint's location.
% The force sensor compression will then induce an error on the joint's stiffness.

% The force sensor stiffness $k_F$ is estimated to be around:

kF = 50/0.05e-3; % [N/m]

sprintf('k_F = %.1e [N/m]', kF)

% Estimation error due to height estimation error
% Let's consider an error in the estimation of the height from the application of the force to the joint's center:
% \begin{equation}
% h_{\text{est}} = h (1 + \epsilon)
% \end{equation}

% The computed bending stiffness will be:
% \begin{equation}
%   k_\text{est} \approx h_{\text{est}}^2 \frac{F_x}{d_x}
% \end{equation}

% And the stiffness estimation error is:
% \begin{equation}
% \frac{k_{\text{est}}}{k_{R_y}} = (1 + \epsilon)^2
% \end{equation}


h_err = 0.2e-3; % Height estimation error [m]