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

% Required external applied force

% The bending stiffness is foreseen to be $k_{R_y} \approx k_{R_x} \approx 5\,\frac{Nm}{rad}$ and its stroke $\theta_{y,\text{max}}\approx \theta_{x,\text{max}}\approx 25\,mrad$.
% The height between the flexible point (center of the joint) and the point where external forces are applied is $h = 22.5\,mm$ (see Figure ref:fig:test_joints_bench_working_principle).


%% Parameters for study
kRx = 5;       % Bending Stiffness [Nm/rad]
Rxmax = 25e-3; % Bending Stroke [rad]
h = 22.5e-3;     % Height [m]

%% Estimation of the force to test the full stroke
Fxmax = kRx*Rxmax/h; % Force to induce maximum stroke [N]

% Required actuator stroke and sensors range

% The flexible joint is designed to allow a bending motion of $\pm 25\,mrad$.
% The corresponding stroke at the location of the force sensor is given by eqref:eq:test_joints_max_stroke.
% In order to test the full range of the flexible joint, the means of applying a force (explained in the next section) should allow a motion of at least $0.5\,mm$.
% Similarly, the measurement range of the displacement sensor should also be higher than $0.5\,mm$.

% \begin{equation}\label{eq:test_joints_max_stroke}
% d_{x,\text{max}} = h \tan(R_{x,\text{max}}) \approx 0.5\,mm
% \end{equation}


%% Estimated maximum stroke [m]
dxmax = h*tan(Rxmax);

% Introduction                                                  :ignore:
% In order to estimate the accuracy of the measured bending stiffness that can be obtained using this measurement bench, an error budget is performed.

% Based on equation eqref:eq:test_joints_stiff_displ_force, several errors can impact the accuracy of the measured bending stiffness:
% - Errors in the measured torque $M_x, M_y$: this is mainly due to inaccuracies of the load cell and of the height estimation $h$
% - Errors in the measured bending motion of the flexible joints $\theta_x, \theta_y$: errors from limited shear stiffness, from the deflection of the load cell itself, and from inaccuracy of the height estimation $h$

% Let's first estimate the displacement induced only by the bending stiffness.

% \begin{equation}\label{eq:test_joints_dbx}
%   d_{x,b} = h \tan(\theta_y) = h \tan\left( \frac{F_x \cdot h}{k_{R_y}} \right)
% \end{equation}

% # From equation eqref:eq:test_joints_dbx, we can compute the bending stiffness:
% # \begin{equation}
% #   k_{R_y} = \frac{F_x \cdot h}{\tan^{-1}\left( \frac{D_b}{h} \right)}
% # \end{equation}

% # For small displacement, we have
% # \begin{equation}
% #   \boxed{k_{R_y} \approx h^2 \frac{F_x}{d_x}}
% # \end{equation}

% # And therefore, to precisely measure $k_{R_y}$, we need to:
% # - precisely measure the motion $d_x$
% # - precisely measure the applied force $F_x$
% # - precisely now the height of the force application point $h$


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

%% Height between the joint's center and the force application point
h = 22.5e-3; % [m]

% Effect of Shear
% The applied force $F_x$ will induce some shear $d_{x,s}$ which is described by eqref:eq:test_joints_shear_displ with $k_s$ the shear stiffness of the flexible joint.

% \begin{equation}\label{eq:test_joints_shear_displ}
%   d_{x,s} = \frac{F_x}{k_s}
% \end{equation}

% The measured displacement $d_x$ is affected by the shear as shown in equation eqref:eq:test_joints_displ_shear.

% \begin{equation}\label{eq:test_joints_displ_shear}
%   d_x = d_{x,b} + d_{x,s} = h \tan\left( \frac{F_x \cdot h}{k_{R_y}} \right) + \frac{F_x}{k_s} \approx F_x \left( \frac{h^2}{k_{R_y}} + \frac{1}{k_s} \right)
% \end{equation}

% The estimated bending stiffness $k_{\text{est}}$ then depends on the shear stiffness eqref:eq:test_joints_error_shear.

% \begin{equation}\label{eq:test_joints_error_shear}
%   k_{R_y,\text{est}} = h^2 \frac{F_x}{d_x} \approx k_{R_y} \frac{1}{1 + \frac{k_{R_y}}{k_s h^2}} \approx k_{R_y} \Bigl( 1 - \underbrace{\frac{k_{R_y}}{k_s h^2}}_{\epsilon_{s}} \Bigl)
% \end{equation}

% With an estimated shear stiffness $k_s = 13\,N/\mu m$ from the finite element model and an height $h=25\,mm$, the estimation errors of the bending stiffness due to shear is $\epsilon_s < 0.1\,\%$


%% Estimated error due to shear
epsilon_s = 100*abs(1-1/(1 + kb/(ks*h^2))); % Error in %

% Effect of load cell limited stiffness
% As explained in the previous section, because the measurement of the flexible joint deflection is indirectly made with the encoder, errors will be made if the load cell experiences some compression.

% Suppose the load cell has an internal stiffness $k_f$, the same reasoning that was made for the effect of shear can be applied here.
% The estimation error of the bending stiffness due to the limited stiffness of the load cell is then described by eqref:eq:test_joints_error_load_cell_stiffness.

% \begin{equation}\label{eq:test_joints_error_load_cell_stiffness}
%   k_{R_y,\text{est}} = h^2 \frac{F_x}{d_x} \approx k_{R_y} \frac{1}{1 + \frac{k_{R_y}}{k_F h^2}} \approx k_{R_y} \Bigl( 1 - \underbrace{\frac{k_{R_y}}{k_F h^2}}_{\epsilon_f} \Bigl)
% \end{equation}

% With an estimated load cell stiffness of $k_f \approx 1\,N/\mu m$ (from the documentation), the errors due to the load cell limited stiffness is around $\epsilon_f = 1\,\%$.


%% Estimated error due to limited load cell stiffness
kF = 50/0.05e-3; % Estimated load cell stiffness [N/m]
epsilon_f = 100*abs(1-1/(1 + kb/(kF*h^2))); % Error in %