% R_RRT_RTR
% R_RRT_RTR Mechanism
% position analysis

clear; clc; close all

AB = 0.20 ;	% m 	
BC = 0.21 ;	% m 	
CD = 0.39 ;	% m	
CF = 0.45 ; % m
a  = 0.30 ;	% m	
b  = 0.25 ;	% m
c  = 0.05 ;	% m
phi = 45*pi/180 ; % rad
n = -500 ;        % rpm

% position of joint A
xA = 0; yA = 0;
rA_ = [xA yA 0];
fprintf('Results \n\n') 
fprintf('phi = phi1 = %g (degrees) \n',phi*180/pi) 
fprintf('rA_ = [%g, %g, %g] (m)\n', rA_) 

% position of joint E
xE =  a;
yE = -b;
rE_ = [xE yE 0]; % position vector of C
fprintf('rE_ = [%6.3f, %6.3f, %g] (m)\n', rE_) 

% position of joint B 
xB = AB*cos(phi); 
yB = AB*sin(phi); 
rB_ = [xB yB 0]; % position vector of B
fprintf('rB_ = [%6.3f, %6.3f, %g] (m)\n', rB_)

% position of joint D
yD = c;
syms xDs
eqD = (xB - xDs)^2 + (yB - yD)^2 - (BC+CD)^2;
solD = solve(eqD, xDs);
xD1=eval(solD(1)); 
xD2=eval(solD(2)); 

if xD1 > xB xD = xD1; else xD = xD2; end
rD_ = [xD yD 0]; % position vector of D 
fprintf('rD_ = [%6.3f, %g, %g] (m)\n', rD_)

% angle of link 2
phi2 = atan((yB-yD)/(xB-xD));
fprintf...
('phi2 = %6.3f (degrees) \n', phi2*180/pi)

% position of joint C
syms xC yC
eqC1 = (xB-xC)^2 + (yB-yC)^2 - BC^2;
eqC2 = (yB-yD)/(xB-xD)-(yD-yC)/(xD-xC);
solC = solve(eqC1, eqC2);
xCpos = eval(solC.xC);
yCpos = eval(solC.yC);
xC1 = xCpos(1); xC2 = xCpos(2); 
yC1 = yCpos(1); yC2 = yCpos(2); 

if xC1 > xB xC=xC1;yC=yC1; else
xC=xC2; yC=yC2; end
rC_ = [xC yC 0]; % position vector of C 
fprintf('rC_ = [%6.3f, %6.3f, %g] (m)\n', rC_)

% angle of link 4
phi4 = atan2((yC-yE),(xC-xE));
phi5 = phi4;
fprintf...
('phi4 = phi5 = %6.3f (degrees) \n', phi4*180/pi)
xF = xC-CF*cos(phi4);
yF = yC-CF*sin(phi4);
rF_ = [xF yF 0];
fprintf('rF_ = [%6.3f, %6.3f, %g] (m)\n', rF_)

% graphic of the mechanism
axis manual
hold on
grid on
sx = .8;
sy = .8;
axis([-sx sx -sy sy]) 
xlabel('x (m)'),ylabel('y (m)')
pM = plot([xA,xB],[yA,yB],'r-o',...
     [xB,xD],[yB,yD],'b-o',...   
     [xC,xE],[yC,yE],'k-o',...
     [xC,xF],[yC,yF],'k-o');
plot(xC, yC, 'k.','Color', 'red')
xlabel('x (m)'),ylabel('y (m)')
pin0_scale = 0.015;  
slider_scale = 0.05;      
% plotting joint A 
plot_pin0( xA, yA, pin0_scale, 0)
% plotting slider at D
plot_slider( xD, yD, slider_scale, pi)
% plotting slider at E
plot_slider( xE, yE, slider_scale, phi4)
% plotting joint E 
plot_pivot0( xE, yE, pin0_scale, pi/2)

text(xA+0.03,yA,'A'), 
text(xB+0.02,yB+0.03,'B'),
text(xC,yC+0.035,'C'), 
text(xD+0.03,yD,'D'),
text(xE-0.05,yE,'E'),
text(xF-0.04,yF,'F')

print -depsc f.eps 


fprintf('\n')
fprintf('Velocity and acceleration analysis \n\n') 

n = -500; % rpm
omega1_ = [0 0 pi*n/30]; alpha1_ = [0 0 0]; 
fprintf...
('omega1_ = [%g, %g, %6.3f] (rad/s)\n', omega1_) 
fprintf...
('alpha1_ = [%g, %g, %g] (rad/s^2)\n', alpha1_)  
fprintf('\n') 

vA_ = [0 0 0 ]; aA_ = [0 0 0 ];

vB1_ = vA_ + cross(omega1_,rB_); 
vB2_ = vB1_; 
aB1_ = aA_ + cross(alpha1_,rB_) - ...
    dot(omega1_,omega1_)*rB_;
vB2_ = vB1_; vB_ = vB1_; 
aB2_ = aB1_; aB_ = aB1_;
fprintf...
('vB_ = vB1_ = vB2_ = [%6.3f, %6.3f, %g] (m/s)\n',vB1_)  
fprintf...
('aB_ = aB1_ = aB2_ = [%6.3f, %6.3f, %g] (m/s^2)\n',aB1_)  
fprintf('\n')

syms omega2z vD
omega2_ = [ 0 0 omega2z ]; 
% omega2z unknown (to be calculated)
% vD unknown (to be calculated)
% vD2_ = vB_ + omegaB_ x rBD_   
% (D2 & B are points on link 2)
vD2_ = vB1_ + cross(omega2_,rD_-rB_); 
% point D2 is on link 2 and point D3 is on link 3
% vD3_ = vD2_
% vD3 is parallel to the sliding x-direction 
vD3_ = [vD 0 0]; 
eqvD_ = vD3_ - vD2_; 
% vectorial equation
% the component of the vectorial equation on x-axis
eqvDx = eqvD_(1); 
% the component of the vectorial equation on y-axis
eqvDy = eqvD_(2); 
% two equations eqvBx and eqvBy with two unknowns 
% solve for omega2z and vD 
solvD = solve(eqvDx,eqvDy); 
omega2=eval(solvD.omega2z); 
vDs=eval(solvD.vD);

omega2_ = [0 0 omega2]; 
vD_ = [vDs 0 0];;
% print the equations for calculating 
% omega2 and vD
fprintf...
('vD2_ = vB_ + omega2_ x rBD_ = vD3_  => \n')
digits(3)
qvDx=vpa(eqvDx);
fprintf('x-axis: %s = 0 \n', char(qvDx)) 
qvDy=vpa(eqvDy);
fprintf('y-axis: %s = 0 \n', char(qvDy)) 
fprintf('=>\n') 
fprintf('omega2z = %6.3f (rad/s)\n', omega2) 
fprintf('vD = %6.3f (m/s)\n', vDs) 
fprintf('\n') 
fprintf...
('omega2_ = [%g,%g,%6.3f](rad/s)\n', omega2_) 
fprintf('vD_ = [%6.3f,%6.3f,%d] (m/s)\n\n', vD_)  

syms alpha2z aD
alpha2_ = [ 0 0 alpha2z ]; % alpha3z unknown
% aD2 acceleration of D
aD2_ = aB_ + cross(alpha2_, rD_-rB_) -...
    dot(omega2_, omega2_)*(rD_-rB_); 
% aD3 parallel to the sliding direcion x
aD3_ = [aD 0 0]; 
% aD3_ = aD2 
eqaD_ = aD3_ - aD2_; 
% vectorial equation
eqaDx = eqaD_(1); % equation component on x-axis
eqaDy = eqaD_(2); % equation component on y-axis
solaD = solve(eqaDx,eqaDy);
alpha2zs=eval(solaD.alpha2z);
aDs=eval(solaD.aD);
alpha2_ = [0 0 alpha2zs];
aD_ = [aDs 0 0];

% print the equations for calculating 
% alpha2 and aD
fprintf('\n') 
fprintf('aD2_=aB_+alpha2_xrBD_-(omega2_.omega2_)rBD_\n') 
fprintf('aD3_=aD2_ =>\n') 
qaDx=vpa(eqaDx);
fprintf('x-axis:\n') 
fprintf('%s = 0\n',char(qaDx)) 
qaDy=vpa(eqaDy);
fprintf('y-axis:\n') 
fprintf('%s = 0\n',char(qaDy))  
fprintf('=>\n');
fprintf('alpha2z = %6.3f (rad/s^2)\n', alpha2zs) 
fprintf('aD = %6.3f (m/s^2)\n', aDs) 
fprintf('\n') 
fprintf...
('alpha2_ = [%g,%g,%6.3f](rad/s^2)\n', alpha2_) 
fprintf('aD_ = [%6.3f,%6.3f,%d] (m/s^2)\n\n', aD_) 

alpha3_ = [0 0 0];
fprintf...
('alpha3_ = [%g,%g,%g](rad/s^2)\n', alpha3_) 

% vC velocity of C
% C & B points on link 2
vC_ = vB_ + cross(omega2_,rC_-rB_);
fprintf('vC_=vC2_=vC4_=[%6.3f,%6.3f,%g] (m/s)\n',vC_) 
% aC acceleration of C
aC_ = aB_ + cross(alpha2_, rC_-rB_) + ...
    cross(omega2_,cross(omega2_, rC_-rB_));
fprintf('aC_=aC2_=aC4_=[%6.3f,%6.3f,%g] (m/s^2)\n',aC_) 
fprintf('\n')

syms omega4z vE45
omega4_ = [ 0 0 omega4z ]; 
% omega4z unknown (to be calculated)
% vE45 unknown (to be calculated)
vE5_ = [0 0 0 ]; % E is fixed
% vE4_ = vC_ + omega4_ x rCE_   
% (E4 & C are points on link 4)
vE4_ = vC_ + cross(omega4_,rE_-rC_); 
% point E4 is on link 4 and point E5 is on link 5
% vE4_ = vE5_ + vE4E5_
% between the links 4 and 5 there is a  
% translational joint E_T
% vE4E5 is the relative velocity of B4 wrt 5
% vE4E5 is parallel to the sliding direcion EC
% vE4E5 is written as a vector
vE4E5_ = vE45*[ cos(phi4) sin(phi4) 0]; 
% vE4_ = vE5_ + aE45_
eqvE_ = vE4_ - vE5_ - vE4E5_; 
% vectorial equation
% the component of the vectorial equation on x-axis
eqvEx = eqvE_(1); 
% the component of the vectorial equation on y-axis
eqvEy = eqvE_(2); 
% two equations eqvBx and eqvEy with two unknowns 
% solve for omega4z and vE45 
solvE = solve(eqvEx,eqvEy); 
omega4zs=eval(solvE.omega4z); 
vE45s=eval(solvE.vE45);

omega4_ = [0 0 omega4zs]; 
omega5_ = omega4_;
vE45_ = vE45s*[cos(phi4) sin(phi4) 0];

% print the equations for calculating 
% omega4 and vE45
fprintf...
('vE4_ = vC_ + omega4_ x rCE_ = vE5_ + vE45_ => \n')
digits(3)
qvEx=vpa(eqvEx);
fprintf('x-axis: %s = 0 \n', char(qvEx)) 
qvEy=vpa(eqvEy);
fprintf('y-axis: %s = 0 \n', char(qvEy)) 
fprintf('=>\n') 
fprintf('omega4z = %6.3f (rad/s)\n', omega4zs) 
fprintf('vE45 = %6.3f (m/s)\n', vE45s) 
fprintf('\n') 
fprintf...
('omega4_=omega5_ = [%g,%g,%6.3f](rad/s)\n', omega4_) 
fprintf('vE45_ = [%6.3f,%6.3f,%d] (m/s)\n\n', vE45_)  

% Coriolis acceleration
aE45cor_ = 2*cross(omega4_,vE45_);
fprintf('aE45cor_ = [%6.3g,%6.3g,%d] (m/s^2)\n\n',aE45cor_)   

syms alpha4z aE45
alpha4_ = [ 0 0 alpha4z ]; 
% alpha4z unknown (to be calculated)
% aE45 unknown (to be calculated)
aE5_ = [0 0 0 ]; % E is fixed
% aE4_=aC_+alpha4_xrCE_+omega4_x(omega4_xrCE_)
% (E4 & C are points on link 4)
aE4_=aC_+cross(alpha4_,rE_-rC_)+...
cross(omega4_,cross(omega4_,rE_-rC_)); 
% point E4 is on link 4 and point E5 is on link 5
% aE4_ = aE5_ + aE45_ + aE45cor_;
% between the links 4 and 5 there is a  
% translational joint E_T
% aE4E5 is the relative velocity of E4 wrt 5
% aE4E5 is parallel to the sliding direcion EC
% aE4E5 is written as a vector
aE4E5_ = aE45*[ cos(phi4) sin(phi4) 0]; 
% aE4_ = aE5_ + aE45_ + aE45cor_
eqaE_ = aE4_ - aE5_ - aE4E5_ - aE45cor_; 
% vectorial equation
% the component of the vectorial equation on x-axis
eqaEx = eqaE_(1); 
% the component of the vectorial equation on y-axis
eqaEy = eqaE_(2); 
% two equations eqvBx and eqvEy with two unknowns 
% solve for omega4z and aE45 
solaE = solve(eqaEx,eqaEy); 
alpha4zs=eval(solaE.alpha4z); 
aE45s=eval(solaE.aE45);

alpha4_ = [0 0 alpha4zs]; 
aE45_ = aE45s*[ cos(phi4) sin(phi4) 0]; 

% print the equations for calculating 
% alpha4 and aE45
fprintf...
('aE4_=vC_+alpha4_xrCE_+omega4_x(omega4_xrCE_)=\n')
fprintf...
('aE4_=aE5_+aE45_+aE45cor_=>\n')
digits(3)
qaEx=vpa(eqaEx);
fprintf('x-axis: %s = 0 \n', char(qaEx)) 
qaEy=vpa(eqaEy);
fprintf('y-axis: %s = 0 \n', char(qaEy)) 
fprintf('=>\n') 
fprintf('alpha4z = %6.3f (rad/s^2)\n', alpha4zs) 
fprintf('aE45 = %6.3f (m/s^2)\n', aE45s) 
fprintf('\n') 

alpha5_=alpha4_;
fprintf...
('alpha4_=alpha5_ = [%g,%g,%6.3f](rad/s^2)\n', alpha4_) 
fprintf('aE45_ = [%6.3f,%6.3f,%d] (m/s^2)\n\n', aE45_) 

vF_ = vC_ + cross(omega4_,rF_-rC_); 
aF_=aC_+cross(alpha4_,rF_-rC_)+...
cross(omega4_,cross(omega4_,rF_-rC_)); 
fprintf('vF_=[%6.3f,%6.3f,%g] (m/s)\n',vF_) 
fprintf('aF_=[%6.3f,%6.3f,%g] (m/s^2)\n',aF_) 
fprintf('\n') 



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_)



% 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.215e-02 (kg m^2)
% IC4 alpha4_=[0,0, 6.381](N m)
% Min4_=-IC4 alpha4_=[0,0,-6.381](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)
