% R_RRT_RTR Mechanism
% force analysis

clear; clc; close all

PIV_2; % position, velocity, acceleration PIV_2.m

rC1_ = rB_/2; 
fprintf('rC1_ = [%6.3f,%6.3f,%d] (m)\n', rC1_)
rC2_ = (rB_+rD_)/2;
fprintf('rC2_ = [%6.3f,%6.3f,%d] (m)\n', rC2_)
rC3_ = rD_; 
fprintf('rC3_ = [%6.3f,%6.3f,%d] (m)\n', rC3_)
rC4_ = (rC_+rF_)/2; 
fprintf('rC4_ = [%6.3f,%6.3f,%d] (m)\n', rC4_)
rC5_ = rE_; 
fprintf('rC5_ = [%6.3f,%6.3f,%d] (m)\n', rC5_)

aC1_ = aB_/2; 
fprintf('aC1_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC1_)
aC2_ = (aB_+aD_)/2;
fprintf('aC2_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC2_)
aC3_ = aD_; 
fprintf('aC3_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC3_)
aC4_ = (aC_+aF_)/2; 
fprintf('aC4_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC4_)
aC5_ = [0 0 0]; 
fprintf('aC5_ = [%6.3f,%d,%d] (m/s^2)\n', aC5_)

fprintf...
('alpha1_ = [%g,%g,%6.3f](rad/s^2)\n', alpha1_) 
fprintf...
('alpha2_ = [%g,%g,%6.3f](rad/s^2)\n', alpha2_) 
fprintf...
('alpha3_ = [%g,%g,%6.3f](rad/s^2)\n', alpha3_) 
fprintf...
('alpha4_ = [%g,%g,%6.3f](rad/s^2)\n', alpha4_) 
fprintf...
('alpha5_ = [%g,%g,%6.3f](rad/s^2)\n', alpha5_) 

h = 0.02; % (m) height of the bar
d = 0.01; % (m) depth of the bar
hSlider = 0.04; % (m) height of the slider 
wSlider = 0.08; % (m) width of the slider 
dSlider = 0.02; % (m) depth of the slider 
rho = 8000; % (kg/m^3) density of the material
g = 9.807; % (m/s^2) gravitational acceleration

fprintf('\n')
Me_ = -sign(omega4_(3))*[0 0 1000];
fprintf('Me_ = [%d, %d, %6.3f] (N m)\n', Me_)
fprintf('\n')

fprintf('Inertia forces and inertia moments\n\n')

fprintf('Link 1  \n')
m1 = rho*AB*h*d;
Fin1_ = -m1*aC1_;
G1_ = [0,-m1*g,0];
IC1 = m1*AB^2/12;
alpha1_ = [0 0 0];
Min1_ = -IC1*alpha1_;
fprintf('m1 = %6.3f (kg)\n', m1)
fprintf('m1 aC1_ = [%6.3f,%6.3f,%d] (N)\n', m1*aC1_)
fprintf('Fin1_= - m1 aC1_ = [%6.3f,%6.3f,%d] (N)\n', Fin1_)
fprintf('G1_ = - m1 g = [%d,%6.3f,%d] (N)\n', G1_)
fprintf('IC1 = %g (kg m^2)\n', IC1)
fprintf('IC1 alpha1_=[%6.3f,%6.3f,%d](N m)\n',IC1*alpha1_)
fprintf('Min1_=-IC1 alpha1_=[%d, %d, %d](N m)\n',Min1_)
fprintf('\n')

fprintf('Link 2  \n')
m2 = rho*(BC+CD)*h*d;
Fin2_ = -m2*aC2_;
G2_ = [0,-m2*g,0];
IC2 = m2*(BC+CD)^2/12;
Min2_ = -IC2*alpha2_;
fprintf('m2 = %6.3f (kg)\n', m2)
fprintf('m2 aC2_ = [%6.3f,%6.3f,%d] (N)\n', m2*aC2_)
fprintf('Fin2_= - m2 aC2_ = [%6.3f,%6.3f,%d] (N)\n', Fin2_)
fprintf('G2_ = - m2 g = [%d,%6.3f,%d] (N)\n', G2_)
fprintf('IC2 = %g (kg m^2)\n', IC2)
fprintf('IC2 alpha2_=[%6.3f,%6.3f,%d](N m)\n',IC2*alpha2_)
fprintf('Min2_=-IC2 alpha2_=[%d, %d, %d](N m)\n',Min2_)
fprintf('\n')

fprintf('Link 3 \n')
m3 = rho*hSlider*wSlider*dSlider;
Fin3_ = -m3*aC3_;
G3_ = [0,-m3*g,0];
IC3 = m3*(hSlider^2+wSlider^2)/12;
Min3_ = -IC3*alpha3_;
fprintf('m3 = %6.3f (kg)\n', m3)
fprintf('m3 aC3_ = [%6.3f,%6.3f,%d] (N)\n', m3*aC3_)
fprintf('Fin3_= - m3 aC3_ =[%6.3f,%6.3f,%d] (N)\n', Fin3_)
fprintf('G3_ = - m3 g_ = [%d,%6.3f,%d] (N)\n', G3_)
fprintf('IC3 = %6.3d (kg m^2)\n', IC3)
fprintf('IC3 alpha3_=[%g,%g,%d](N m)\n',IC3*alpha3_)
fprintf('Min3_=-IC3 alpha3_=[%d,%d,%d](N m)\n',Min3_)
fprintf('\n')

fprintf('Link 4 \n')
m4 = rho*CF*h*d;
Fin4_ = -m4*aC4_;
G4_ = [0,-m4*g,0];
IC4 = m4*CF^2/12;
Min4_ = -IC4*alpha4_;
fprintf('m4 = %6.3f (kg)\n', m4)
fprintf('m4 aC4_ = [%6.3f,%6.3f,%d] (N)\n', 4*aC4_)
fprintf('Fin4_= - m4 aC4_ =[%6.3f,%6.3f,%d] (N)\n', Fin4_)
fprintf('G4_ = - m4 g_ = [%d,%6.3f,%d] (N)\n', G4_)
fprintf('IC4 = %6.3d (kg m^2)\n', IC4)
fprintf('IC4 alpha4_=[%g,%g,%6.3f](N m)\n',IC4*alpha4_)
fprintf('Min4_=-IC4 alpha4_=[%d,%d,%6.3f](N m)\n',Min4_)
fprintf('\n')

fprintf('Link 5 \n')
m5 = rho*hSlider*wSlider*dSlider;
Fin5_ = -m5*aC5_;
G5_ = [0,-m5*g,0];
IC5 = m5*(hSlider^2+wSlider^2)/12;
Min5_ = -IC5*alpha5_;
fprintf('m5 = %6.3f (kg)\n', m5)
fprintf('m5 aC5_ = [%g,%g,%g] (N)\n', m5*aC5_)
fprintf('Fin5_ = - m5 aC5 =[%g,%g,%d] (N)\n', Fin5_)
fprintf('G5_ = - m5 g = [%g, %6.3f, %g] (N)\n', G5_)
fprintf('IC5 = %6.3d (kg m^2)\n', IC5)
fprintf('IC5 alpha5_=[%g, %g, %6.3f](N m)\n',IC5*alpha5_)
fprintf('Min5_=-IC5 alpha5_=[%d, %d, %6.3f](N m)\n',Min5_)
fprintf('\n')

fprintf('\n')
fprintf('Dynamic force analysis  \n')
fprintf('Classical method  \n\n')

% Link 5
F05x=sym('F05x','real');
F05y=sym('F05y','real');
F05_=[F05x, F05y, 0]; % unknown joint force 
F45x=sym('F45x','real');
F45y=sym('F45y','real');
F45_=[F45x, F45y, 0]; % unknown joint force 
M45z=sym('M45z','real');
M45_=[0, 0, M45z]; % unknown joint moment
% F45_ perpendicular to CE_
eqF45CE = dot(F45_,rE_-rC_); % (1)
% sum F of 5 
eqF5_ = -m5*aC5_+F45_+F05_+G5_;
eqF5x = eqF5_(1); % (2) x-component
eqF5y = eqF5_(2); % (3) y-component
% sum M of 5 wrt C5=E
eqMC5_ = -IC5*alpha5_+M45_+Me_;
eqMC5z = eqMC5_(3); % (4) z-component

% Link 4
F24x=sym('F24x','real');
F24y=sym('F24y','real');
F24_=[F24x, F24y, 0]; % unknown joint force 
F54_=-F45_;
M54_=-M45_;
% sum F of 5 
eqF4_ = -m4*aC4_+F54_+F24_+G4_;
eqF4x = eqF4_(1); % (5) x-component
eqF4y = eqF4_(2); % (6) y-component 
% sum M of 4 wrt C4
eqMC4_ = -IC4*alpha4_+cross(rC_-rC4_,F24_)...
         +cross(rE_-rC4_,F54_)+M54_;
eqMC4z = eqMC4_(3); % (7) z-component

% Eqs.(1)-(7)
sol45=solve(eqF45CE,eqF5x,eqF5y,eqMC5z,eqF4x,eqF4y,eqMC4z);

F24xs=eval(sol45.F24x);
F24ys=eval(sol45.F24y);
F05xs=eval(sol45.F05x);
F05ys=eval(sol45.F05y);
F45xs=eval(sol45.F45x);
F45ys=eval(sol45.F45y);
M45zs=eval(sol45.M45z);

F24_ = [F24xs F24ys 0];
F05_ = [F05xs F05ys 0];
F45_ = [F45xs F45ys 0];
M45_ = [0 0 M45zs];

fprintf('F05_ = [%6.3f,%6.3f,%d] (N)\n',F05_)
fprintf('F24_ = [%6.3f,%6.3f,%d] (N)\n',F24_)
fprintf('F45_ = [%6.3f,%6.3f,%d] (N)\n',F45_)
fprintf('M45_ = [%d,%d,%6.3f] (N m)\n',M45_)
fprintf('\n')

% Link 2
F12x=sym('F12x','real');
F12y=sym('F12y','real');
F12_=[F12x, F12y, 0]; % unknown joint force 
F32x=sym('F32x','real');
F32y=sym('F32y','real');
F32_=[F32x, F32y, 0]; % unknown joint force 
% sum F of 2 
F42_ = -F24_;
eqF2_ = -m2*aC2_+F42_+F12_+F32_+G2_;
eqF2x = eqF2_(1); % (8) x-component
eqF2y = eqF2_(2); % (9) y-component 
% sum M of 2 wrt C2
eqMC2_ = -IC2*alpha2_+cross(rB_-rC2_,F12_)...  
         +cross(rD_-rC2_,F32_)+cross(rC_-rC2_,F42_);
eqMC2z = eqMC2_(3); % (10)

% Link 3
F03y=sym('F03y','real');
F03_=[0, F03y, 0]; % unknown joint force
% sum F of 3
F23_ = -F32_;
eqF3_ = -m3*aC3_+F23_+F03_+G3_;
eqF3x = eqF3_(1); % (11) x-component
eqF3y = eqF3_(2); % (12) y-component 

% Eqs.(8)-(12)
sol23=solve(eqF2x,eqF2y,eqMC2z,eqF3x,eqF3y);

F12xs=eval(sol23.F12x);
F12ys=eval(sol23.F12y);
F32xs=eval(sol23.F32x);
F32ys=eval(sol23.F32y);
F03ys=eval(sol23.F03y);

F12_ = [F12xs F12ys 0];
F03_ = [0 F03ys 0];
F32_ = [F32xs F32ys 0];

fprintf('F12_ = [%6.3f,%6.3f,%d] (N)\n',F12_)
fprintf('F03_ = [%d,%6.3f,%d] (N)\n',F03_)
fprintf('F32_ = [%6.3f,%6.3f,%d] (N)\n',F32_)
fprintf('\n')

% link 1 driver
F21_ = -F12_;
F01_ = m1*aC1_-G1_-F21_;
IA = IC1+m1*(AB/2)^2;
Mm_ =IA*alpha1_-cross(rC1_,G1_)-cross(rB_,F21_);
fprintf('F01_ = [%6.3f,%6.3f,%d] (N)\n',F01_)
fprintf('Mm_ = [%d,%d,%6.3f] (N m)\n',Mm_)

% eof
% 
% Results 
% 
% phi = phi1 = 45 (degrees) 
% rA_ = [0, 0, 0] (m)
% rE_ = [ 0.300, -0.250, 0] (m)
% rB_ = [ 0.141,  0.141, 0] (m)
% rD_ = [ 0.734, 0.05, 0] (m)
% phi2 = -8.764 (degrees) 
% rC_ = [ 0.349,  0.109, 0] (m)
% phi4 = phi5 = 82.242 (degrees) 
% rF_ = [ 0.288, -0.336, 0] (m)
% 
% Velocity and acceleration analysis 
% 
% omega1_ = [0, 0, -52.360] (rad/s)
% alpha1_ = [0, 0, 0] (rad/s^2)
% 
% vB_ = vB1_ = vB2_ = [ 7.405, -7.405, 0] (m/s)
% aB_ = aB1_ = aB2_ = [-387.715, -387.715, 0] (m/s^2)
% 
% vD2_ = vB_ + omega2_ x rBD_ = vD3_  => 
% x-axis: vD - 0.0914*omega2z - 7.4 = 0 
% y-axis: 7.4 - 0.593*omega2z = 0 
% =>
% omega2z = 12.487 (rad/s)
% vD =  8.546 (m/s)
% 
% omega2_ = [0,0,12.487](rad/s)
% vD_ = [ 8.546, 0.000,0] (m/s)
% 
% 
% aD2_=aB_+alpha2_xrBD_-(omega2_.omega2_)rBD_
% aD3_=aD2_ =>
% x-axis:
% aD - 0.0914*alpha2z + 480.0 = 0
% y-axis:
% 373.0 - 0.593*alpha2z = 0
% =>
% alpha2z = 629.786 (rad/s^2)
% aD = -422.604 (m/s^2)
% 
% alpha2_ = [0,0,629.786](rad/s^2)
% aD_ = [-422.604, 0.000,0] (m/s^2)
% 
% alpha3_ = [0,0,0](rad/s^2)
% vC_=vC2_=vC4_=[ 7.804,-4.813,0] (m/s)
% aC_=aC2_=aC4_=[-399.926,-252.015,0] (m/s^2)
% 
% vE4_ = vC_ + omega4_ x rCE_ = vE5_ + vE45_ => 
% x-axis: 0.359*omega4z - 0.135*vE45 + 7.8 = 0 
% y-axis: - 0.049*omega4z - 0.991*vE45 - 4.81 = 0 
% =>
% omega4z = -23.109 (rad/s)
% vE45 = -3.716 (m/s)
% 
% omega4_=omega5_ = [0,0,-23.109](rad/s)
% vE45_ = [-0.502,-3.681,0] (m/s)
% 
% aE45cor_ = [  -170,  23.2,0] (m/s^2)
% 
% aE4_=vC_+alpha4_xrCE_+omega4_x(omega4_xrCE_)=
% aE4_=aE5_+aE45_+aE45cor_=>
% x-axis: 0.359*alpha4z - 0.135*aE45 - 204.0 = 0 
% y-axis: - 0.991*aE45 - 0.049*alpha4z - 83.3 = 0 
% =>
% alpha4z = 525.219 (rad/s^2)
% aE45 = -109.981 (m/s^2)
% 
% alpha4_=alpha5_ = [0,0,525.219](rad/s^2)
% aE45_ = [-14.847,-108.974,0] (m/s^2)
% 
% vF_=[-2.500,-3.409,0] (m/s)
% aF_=[-133.299,-45.808,0] (m/s^2)
% 
% rC1_ = [ 0.071, 0.071,0] (m)
% rC2_ = [ 0.438, 0.096,0] (m)
% rC3_ = [ 0.734, 0.050,0] (m)
% rC4_ = [ 0.319,-0.114,0] (m)
% rC5_ = [ 0.300,-0.250,0] (m)
% aC1_ = [-193.857,-193.857,0] (m/s^2)
% aC2_ = [-405.159,-193.857,0] (m/s^2)
% aC3_ = [-422.604, 0.000,0] (m/s^2)
% aC4_ = [-266.612,-148.911,0] (m/s^2)
% aC5_ = [ 0.000,0,0] (m/s^2)
% alpha1_ = [0,0, 0.000](rad/s^2)
% alpha2_ = [0,0,629.786](rad/s^2)
% alpha3_ = [0,0, 0.000](rad/s^2)
% alpha4_ = [0,0,525.219](rad/s^2)
% alpha5_ = [0,0,525.219](rad/s^2)
% 
% Me_ = [0, 0, 1000.000] (N m)
% 
% Inertia forces and inertia moments
% 
% Link 1  
% m1 =  0.320 (kg)
% m1 aC1_ = [-62.034,-62.034,0] (N)
% Fin1_= - m1 aC1_ = [62.034,62.034,0] (N)
% G1_ = - m1 g = [0,-3.138,0] (N)
% IC1 = 0.00106667 (kg m^2)
% IC1 alpha1_=[ 0.000, 0.000,0](N m)
% Min1_=-IC1 alpha1_=[0, 0, 0](N m)
% 
% Link 2  
% m2 =  0.960 (kg)
% m2 aC2_ = [-388.953,-186.103,0] (N)
% Fin2_= - m2 aC2_ = [388.953,186.103,0] (N)
% G2_ = - m2 g = [0,-9.415,0] (N)
% IC2 = 0.0288 (kg m^2)
% IC2 alpha2_=[ 0.000, 0.000,1.813784e+01](N m)
% Min2_=-IC2 alpha2_=[0, 0, -1.813784e+01](N m)
% 
% Link 3 
% m3 =  0.512 (kg)
% m3 aC3_ = [-216.373, 0.000,0] (N)
% Fin3_= - m3 aC3_ =[216.373,-0.000,0] (N)
% G3_ = - m3 g_ = [0,-5.021,0] (N)
% IC3 = 3.413e-04 (kg m^2)
% IC3 alpha3_=[0,0,0](N m)
% Min3_=-IC3 alpha3_=[0,0,0](N m)
% 
% Link 4 
% m4 =  0.720 (kg)
% m4 aC4_ = [-1066.450,-595.645,0] (N)
% Fin4_= - m4 aC4_ =[191.961,107.216,0] (N)
% G4_ = - m4 g_ = [0,-7.061,0] (N)
% IC4 = 1.620e-02 (kg m^2)
% IC4 alpha4_=[0,0, 8.509](N m)
% Min4_=-IC4 alpha4_=[0,0,-8.509](N m)
% 
% Link 5 
% m5 =  0.512 (kg)
% m5 aC5_ = [0,0,0] (N)
% Fin5_ = - m5 aC5 =[-0,-0,0] (N)
% G5_ = - m5 g = [0, -5.021, 0] (N)
% IC5 = 3.413e-04 (kg m^2)
% IC5 alpha5_=[0, 0,  0.179](N m)
% Min5_=-IC5 alpha5_=[0, 0, -0.179](N m)
% 
% 
% Dynamic force analysis  
% Classical method  
% 
% F05_ = [-2822.194,389.528,0] (N)
% F24_ = [2630.233,-484.662,0] (N)
% F45_ = [2822.194,-384.507,0] (N)
% M45_ = [0,0,-999.821] (N m)
% 
% F12_ = [2024.907,-512.546,0] (N)
% F03_ = [0,-143.783,0] (N)
% F32_ = [216.373,-148.804,0] (N)
% 
% F01_ = [1962.873,-571.442,0] (N)
% Mm_ = [0,0,-358.628] (N m)
