% R_RRT_RTR Mechanism
% force analysis
% contour method


clear; clc; close all

PV_1;


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(omega5_(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,%6.3d](N m)\n',IC3*alpha3_)
fprintf('Min3_=-IC3 alpha3_=[%d,%d,%6.3d](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.3d](N m)\n',IC4*alpha4_)
fprintf('Min4_=-IC4 alpha4_=[%d,%d,%6.3d](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_ = [%6.3f,%6.3f,%g] (N)\n', m5*aC5_)
fprintf('Fin5_ = - m5 aC5 =[%6.3f,%6.3f,%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.3d](N m)\n',IC5*alpha5_)
fprintf('Min5_=-IC5 alpha5_=[%d, %d, %6.3d](N m)\n',Min5_)
fprintf('\n')

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

%%%%%%%%%%%
% C_R: F24 
%%%%%%%%%%%
F24x=sym('F24x','real');
F24y=sym('F24y','real');
F24_=[F24x, F24y, 0]; % unknown joint force 
% E_P: (sumF4).CE = 0
% (Fin4_+G4_+F24_).(rE_-rC_)=0
SF4CE = dot(Fin4_+G4_+F24_,rE_-rC_);
% E_R: sumM45E_ = 0_
% EC_xF24_+EC4x(Fin4_+G4_)+Min4_+Min5_+Me_=0
sumM45E_ = cross(rC_-rE_,F24_)+cross(rC4_-rE_,Fin4_+G4_)...
           +Min4_+Min5_+Me_;
sumM45Ez = sumM45E_(3);
solF24=solve(SF4CE,sumM45Ez);
F24xs=eval(solF24.F24x);
F24ys=eval(solF24.F24y);
F24_ = [F24xs F24ys 0];
F42_ = -F24_;
fprintf('F24_ = [%6.3f,%6.3f,%d] (N)\n',F24_)
% F24_ = [2624.423,-483.870,0] (N)

%%%%%%%%%%%%%%%%%%
% D_R: F23_=-F32_
%%%%%%%%%%%%%%%%%%
F23x=sym('F23x','real');
F23y=sym('F23y','real');
F23_=[F23x, F23y, 0]; % unknown joint force 
% D_P: sumF3_(1) = 0
% (F23_+Fin3_+G3_)(1) = 0
SF3_ = F23_+Fin3_+G3_;
SF3x = SF3_(1);
% B_R: sumM2B_ = 0_
% BDx(-F23_)+BCx(-F24_)+BC2x(Fin2_+G2_)+Min2_ = 0_
sumM2B_ = cross(rD_-rB_,-F23_)+cross(rC_-rB_,-F24_)...
          +cross(rC2_-rB_,Fin2_+G2_)+Min2_;
sumM2Bz = sumM2B_(3);
solF23=solve(SF3x,sumM2Bz);
F23xs=eval(solF23.F23x);
F23ys=eval(solF23.F23y);
F23_ = [F23xs F23ys 0];
fprintf('F23_ = [%6.3f,%6.3f,%d] (N)\n',F23_)
% F23_ = [-216.373,148.841,0] (N)

%%%%%%%%%%%%
% B_R: F12_
%%%%%%%%%%%%
F12x=sym('F12x','real');
F12y=sym('F12y','real');
F12_=[F12x, F12y, 0]; % unknown joint force 
% D_R: sumM2D_ = 0_
% DB_xF12_+DC_x(-F24_)+DC2_x(Fin2_+G2_)+Min2_ = 0_
sumM2D_ = cross(rB_-rD_,F12_)+cross(rC_-rD_,-F24_)...
          +cross(rC2_-rD_,Fin2_+G2_)+Min2_;
sumM2Dz = sumM2D_(3);
% D_P: sumF23_(1) = 0
% (F12_+(-F24)+Fin2_+G2_+Fin3_+G3_)(1) = 0
SF23_ = F12_+(-F24_)+Fin2_+G2_+Fin3_+G3_;
SF23x = SF23_(1);
solF12=solve(sumM2Dz,SF23x);
F12xs=eval(solF12.F12x);
F12ys=eval(solF12.F12y);
F12_ = [F12xs F12ys 0];
F21_ = -F12_;
fprintf('F12_ = [%6.3f,%6.3f,%d] (N)\n',F12_)
% F12_ = [2019.097,-511.718,0] (N)

%%%%%%%%%%%%
% D_P: F03_
%%%%%%%%%%%%
F03y=sym('F03y','real');
F03_=[0, F03y, 0]; % unknown joint force 
% B_R: sumM32B_ = 0_
% BD_x(F03_+Fin3_+G3_)+BC_x(-F24_)+BC2_x(Fin2_+G2_)+Min2_ = 0_
sumM32B_ = cross(rD_-rB_,F03_+Fin3_+G3_)+cross(rC_-rB_,-F24_)...
          +cross(rC2_-rB_,Fin2_+G2_)+Min2_;
sumM32Bz = sumM32B_(3);
F03ys=eval(solve(sumM32Bz));
F03_ = [0 F03ys 0];
fprintf('F03_ = [%d,%6.3f,%d] (N)\n',F03_)     
% F03_ = [0,-143.819,0] (N)      

%%%%%%%%%%%%
% E_R: F05_
%%%%%%%%%%%%
F05x=sym('F05x','real');
F05y=sym('F05y','real');
F05_=[F05x, F05y, 0]; % unknown joint force 
% E_P: sumF5_.CE_ = 0
% (F05_+Fin5_+G5_).CE_ = 0
SF5CE = dot(F05_+Fin5_+G5_,rE_-rC_);
% C_R: sumM54C = 0
% CE_x(F05_+Fin5_+G5_)+CC4_x(Fin4_+G4_)+Me_+Min5_+Min4_ = 0_
sumM54C_ = cross(rE_-rC_,F05_+Fin5_+G5_)+...
          cross(rC4_-rC_,Fin4_+G4_)+...
          Me_+Min5_+Min4_;
sumM54Cz = sumM54C_(3);
solF05=solve(SF5CE,sumM54Cz);
F05xs=eval(solF05.F05x);
F05ys=eval(solF05.F05y);
F05_ = [F05xs F05ys 0];
fprintf('F05_ = [%6.3f,%6.3f,%d] (N)\n',F05_)
% F05_ = [-2816.384,388.736,0] (N)


%%%%%%%%%%%%%%%%%%%%
% E_P: M54_ and F54_
%%%%%%%%%%%%%%%%%%%%
% E_R: sumM5E_ = 0_
% M45_+Min5_+Me_=0_
M45_ = - (Min5_+Me_);
M54_ = -M45_;
fprintf('M54_ = [%d,%d,%6.3f] (N m)\n',M54_)
F54x=sym('F54x','real');
F54y=sym('F54y','real');
F54_=[F54x, F54y, 0]; % unknown joint force 
% F54_ perpendicular to CE_: F54_.CE_=0
PF54 = dot(F54_,rE_-rC_);
% C_R: sumM4C_ = 0_
% CE_x(F54_)+CC4_x(Fin4_+G4_)+Min4_+M54_ = 0_
sumM4C_ = cross(rE_-rC_,F54_)+...
          cross(rC4_-rC_,Fin4_+G4_)+...
          Min4_+M54_;
sumM4Cz = sumM4C_(3);
solF54=solve(PF54,sumM4Cz);
F54xs=eval(solF54.F54x);
F54ys=eval(solF54.F54y);
F54_ = [F54xs F54ys 0];
fprintf('F54_ = [%6.3f,%6.3f,%d] (N)\n',F54_)
% M54_ = [0,0,999.821] (N m)
% F54_ = [-2816.384,383.715,0] (N)
 
%%%%%%%%%%%%%%%%%%%%
% A_R: F01 and Mm_
%%%%%%%%%%%%%%%%%%%%
F01x=sym('F01x','real');
F01y=sym('F01y','real');
F01_=[F01x, F01y, 0]; % unknown joint force 
Mm=sym('Mm','real');
Mm_=[0, 0, Mm]; % unknown motor torque 

% B_R: sumM1B_ = 0_
% BA_x(F01_)+BC1_x(Fin1_+G1_)+Min1_+Mm_ = 0_
sumM1B_ = cross(-rB_,F01_)+...
          cross(rC1_-rB_,Fin1_+G1_)+...
          Min1_+Mm_;
sumM1Bz = sumM1B_(3);
% D_R: sumM12D_ = 0_
% DA_x(F01_)+DC1_x(Fin1_+G1_)+Min1_+Mm_+
% DC_xF42_+DC2_x(Fin2_+G2_)+Min2_= 0_
sumM12D_ = cross(-rD_,F01_)+...
           cross(rC1_-rD_,Fin1_+G1_)+...
           cross(rC_-rD_,F42_)+...
           Min1_+Mm_+...
           cross(rC2_-rD_,Fin2_+G2_)+...
           Min2_;
sumM12Dz = sumM12D_(3);
% D_P: sumF123x = 0
% (F01_+Fin1_+G1_+F42_+Fin2_+G2_+Fin3_+G3_)(1)= 0
sumF123_ = (F01_+Fin1_+G1_+F42_+Fin2_+G2_+Fin3_+G3_);
sumF123x = sumF123_(1);

solF01=solve(sumM1Bz,sumM12Dz,sumF123x);
F01xs=eval(solF01.F01x);
F01ys=eval(solF01.F01y);
Ms=eval(solF01.Mm);
F01_ = [F01xs F01ys 0];
Mm_ = [0 0 Ms];
fprintf('F01_ = [%6.3f,%6.3f,%d] (N)\n',F01_)
fprintf('Mm_ = [%d,%d,%6.3f] (N m)\n',Mm_)
% F01_ = [1957.063,-570.614,0] (N)
% Mm_ = [0,0,-357.689] (N m)

% eof


% Dynamic force analysis  
% Contour method  
% 
% F24_ = [2630.233,-484.662,0] (N)
% F23_ = [-216.373,148.804,0] (N)
% F12_ = [2024.907,-512.546,0] (N)
% F03_ = [0,-143.783,0] (N)
% F05_ = [-2822.194,389.528,0] (N)
% M54_ = [0,0,999.821] (N m)
% F54_ = [-2822.194,384.507,0] (N)
% F01_ = [1962.873,-571.442,0] (N)
% Mm_ = [0,0,-358.628] (N m)
