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

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

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

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

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

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