% Program 4.6
% R-RTR-RTR
% Dynamic force analysis 
% Newton-Euler method

fprintf('R-RTR-RTR  \n');
fprintf('Dynamic force analysis \n');
fprintf('Dyad method \n');
fprintf('Results  \n');
fprintf('  \n'); 

clear 
clc
close

AB = 0.14; AC = 0.06; AE = 0.25; CD = 0.15; DF=0.4; EG=0.5;
phi = 30*(pi/180); 
fprintf('phi = phi1 = %g (degrees) \n', phi*180/pi); fprintf('\n');
fprintf('Position analysis \n');
fprintf('\n');
xA = 0; yA = 0; rA = [xA yA 0]; fprintf('rA = [ %g, %g, %g ] (m)\n', rA);
xC = 0; yC = AC; rC = [xC yC 0]; fprintf('rC = [ %g, %g, %g ] (m)\n', rC);
xE = 0; yE = -AE; rE = [xE yE 0]; fprintf('rE = [ %g, %g, %g ] (m)\n', rE);
xB = AB*cos(phi); yB = AB*sin(phi); rB = [xB yB 0];  
fprintf('rB = [ %g, %g, %g] (m)\n', rB);
eqnD1 = '( xDsol - xC )^2 + ( yDsol - yC )^2 = CD^2 ';
eqnD2 = '( yB - yC ) / ( xB - xC ) = ( yDsol - yC ) / ( xDsol - xC )';
solD = solve(eqnD1, eqnD2, 'xDsol, yDsol');
xDpositions = eval(solD.xDsol); yDpositions = eval(solD.yDsol);
xD1 = xDpositions(1); xD2 = xDpositions(2); 
yD1 = yDpositions(1); yD2 = yDpositions(2);
if (phi>=0 && phi<=pi/2)||(phi >= 3*pi/2 && phi<=2*pi)
   if xD1 <= xC xD = xD1; yD=yD1; else xD = xD2; yD=yD2; end
   else
   if xD1 >= xC xD = xD1; yD=yD1; else xD = xD2; yD=yD2; end
end
rD = [xD yD 0]; fprintf('rD = [ %g, %g, %g ] (m)\n', rD);
phi2 = atan((yB-yC)/(xB-xC)); phi3 = phi2;
phi4 = atan((yD-yE)/(xD-xE))+pi; phi5 = phi4;
xF = xD + DF*cos(phi3); yF = yD + DF*sin(phi3); rF = [xF yF 0]; 
fprintf('rF = [ %g, %g, %g ] (m)\n', rF);
xG = xE + EG*cos(phi5); yG = yE + EG*sin(phi5); rG = [xG yG 0]; 
fprintf('rG = [ %g, %g, %g ]  (m)\n', rG); 
fprintf('phi2 = phi3 = %g (degrees) \n', phi2*180/pi);
fprintf('phi4 = phi5 = %g (degrees) \n', phi4*180/pi);
fprintf('\n');
xC1 = xB/2; yC1 = yB/2; rC1 = [xC1 yC1 0]; 
fprintf('rC1 = [ %g, %g, %g ] (m)\n', rC1);
rC2 = rB;  fprintf('rC2 = rB = [ %g, %g, %g ] (m)\n', rC2);
xC3 = (xD+xF)/2; yC3 = (yD+yF)/2; rC3 = [xC3 yC3 0]; 
fprintf('rC3 = [ %g, %g, %g ] (m)\n', rC3);
rC4 = rD; fprintf('rC4 = rD = [ %g, %g, %g ] (m)\n', rC4);
xC5 = (xE+xG)/2; yC5 = (yE+yG)/2; rC5 = [xC5 yC5 0]; 
fprintf('rC5 = [ %g, %g, %g ] (m)\n', rC5);

% Graphic of the mechanism
plot([0,xB],[0,yB],'r-o',[xD,xC],[yD,yC],'b-o',[xC,xF],[yC,yF],'b-o'),...
hold on
plot([xE,xG],[yE,yG],'g-o'),...
xlabel('x (m)'), ylabel('y (m)'), title('positions for phi = 30 (deg)'),...
text(xA,yA,'  A'); text(xB,yB,'  B=C2'), text(xC,yC,'  C'),
text(xD,yD,'  D=C4'), text(xE,yE,'  E'), text(xF,yF,'  F'), 
text(xG,yG,'  G'), text(xC1,yC1,'  C1'), text(xC3,yC3,'  C3'),
text(xC5,yC5,'  C5'), axis([-0.3 0.3 -0.3 0.3]), grid on,

fprintf('\n'); 
fprintf('Velocity and acceleration analysis \n'); fprintf('\n');
n = 50.; 
omega1 = [ 0 0 pi*n/30 ]; alpha1 = [0 0 0 ]; 
vA = [0 0 0 ]; aA = [0 0 0 ]; 
vB1 = vA + cross(omega1,rB); vB2 = vB1; 
aB1 = aA + cross(alpha1,rB) - dot(omega1,omega1)*rB; aB2 = aB1;
fprintf('aB1 = aB2 = [ %g, %g, %g ] (m/s^2)\n', aB1); 
omega3z=sym('omega3z','real'); alpha3z=sym('alpha3z','real');
vB32=sym('vB32','real'); aB32=sym('aB32','real');
omega3 = [ 0 0 omega3z ]; 
vC = [0 0 0 ]; 
vB3 = vC + cross(omega3,rB-rC); 
vB3B2 = vB32*[ cos(phi2) sin(phi2) 0]; 
eqvB = vB3 - vB2 - vB3B2; eqvBx = eqvB(1); eqvBy = eqvB(2); 
solvB = solve(eqvBx,eqvBy);
omega3zs=eval(solvB.omega3z); vB32s=eval(solvB.vB32);
Omega3 = [0 0 omega3zs]; Omega2 = Omega3;
v32 = vB32s*[cos(phi2) sin(phi2) 0];
vD3 = vC + cross(Omega3,rD-rC); vD4 = vD3;
aB3B2cor = 2*cross(Omega3,v32); 
alpha3 = [ 0 0 alpha3z ]; 
aC = [0 0 0 ]; 
aB3 = aC + cross(alpha3,rB-rC) - dot(Omega3,Omega3)*(rB-rC); 
aB3B2 = aB32*[ cos(phi2) sin(phi2) 0]; 
eqaB = aB3 - aB2 - aB3B2 - aB3B2cor; eqaBx = eqaB(1); eqaBy = eqaB(2); 
solaB = solve(eqaBx,eqaBy);
alpha3zs=eval(solaB.alpha3z); aB32s=eval(solaB.aB32);
Alpha3 = [0 0 alpha3zs]; Alpha2 = Alpha3;
aD3 = aC + cross(Alpha3,rD-rC) - dot(Omega3,Omega3)*(rD-rC); aD4 = aD3;
fprintf('aD3 = aD4 = [ %g, %g, %g ] (m/s^2)\n', aD3 );
omega5z=sym('omega5z','real'); alpha5z=sym('alpha5z','real');
vD54=sym('vD54','real'); aD54=sym('aD54','real');
omega5 = [ 0 0 omega5z ];
vE = [0 0 0 ]; 
vD5 = vE + cross(omega5,rD-rE); 
vD5D4 = vD54*[ cos(phi5) sin(phi5) 0]; 
eqvD = vD5 - vD4 - vD5D4; eqvDx = eqvD(1);  eqvDy = eqvD(2);
solvD = solve(eqvDx,eqvDy);
omega5zs=eval(solvD.omega5z); vD54s=eval(solvD.vD54);
Omega5 = [0 0 omega5zs];
v54 = vD54s*[cos(phi5) sin(phi5) 0];
Omega4 = Omega5;
aD5D4cor = 2*cross(Omega5,v54); 
alpha5 = [ 0 0 alpha5z ]; 
aE = [0 0 0 ]; 
aD5 = aE + cross(alpha5,rD-rE) - dot(Omega5,Omega5)*(rD-rE); 
aD5D4 = aD54*[ cos(phi5) sin(phi5) 0];
eqaD = aD5 - aD4 - aD5D4 - aD5D4cor; eqaDx = eqaD(1); eqaDy = eqaD(2); 
solaD = solve(eqaDx,eqaDy);
alpha5zs=eval(solaD.alpha5z); aD54s=eval(solaD.aD54);
Alpha5 = [0 0 alpha5zs]; Alpha4 = Alpha5;
aF = aC + cross(Alpha3,rF-rC) - dot(Omega3,Omega3)*(rF-rC); 
aG = aE + cross(Alpha5,rG-rE) - dot(Omega5,Omega5)*(rG-rE); 
fprintf('aF = [ %g, %g, %g ] (m/s^2)\n', aF );
fprintf('aG = [ %g, %g, %g ] (m/s^2)\n', aG );

fprintf('omega4 = omega5 = [ %g, %g, %g ] (rad/s)\n',Omega5);fprintf('\n');
fprintf('alpha1 = [ %g, %g, %g ] (rad/s^2)\n', alpha1); 
fprintf('alpha2 = alpha3 = [ %g, %g, %g ] (rad/s^2)\n', Alpha3);
fprintf('alpha4 = alpha5 = [ %g, %g, %g ] (rad/s^2)\n', Alpha5 );
fprintf('\n');
aC1 = aB1/2; fprintf('aC1 = [ %g, %g, %g ] (m/s^2)\n', aC1 );
aC2 = aB2; fprintf('aC2 = aB2 = [ %g, %g, %g ] (m/s^2)\n', aC2 );
aC3 = (aD3+aF)/2; fprintf('aC3 = [ %g, %g, %g ] (m/s^2)\n', aC3 );
aC4 = aD3; fprintf('aC4 = aD4 = [ %g, %g, %g ] (m/s^2)\n', aC4 );
aC5 = (aE+aG)/2; fprintf('aC5 = [ %g, %g, %g ] (m/s^2)\n', aC5 );

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('\n');
fprintf('Dynamic force analysis  \n');
fprintf('Dyad method  \n');
fprintf('\n');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

h = 0.01; % height of the bar
d = 0.001; % depth of the bar
hSlider = 0.02; % height of the slider 
wSlider = 0.05; % depth of the slider 
rho = 8000; % density of the material
g = 9.807; % gravitational acceleration

Me = -sign(Omega5(3))*[0,0,100];
fprintf('Me = [ %d, %d, %g] (N m)\n', Me);
fprintf('\n');
fprintf('Inertia forces and inertia moments  \n');
fprintf('\n');

fprintf('Link 1  \n');
m1 = rho*AB*h*d;
Fin1 = -m1*aC1;
G1 = [0,-m1*g,0];
IC1 = m1*(AB^2+h^2)/12;
Min1 = -IC1*alpha1;
fprintf('m1 =  %g (kg)\n', m1);
fprintf('m1 aC1 = [ %g, %g, %g] (N)\n', m1*aC1 );
fprintf('Fin1 = - m1 aC1 = [ %g, %g, %d] (N)\n', Fin1 );
fprintf('G1 = - m1 g = [ %g, %g, %g] (N)\n', G1 );
fprintf('IC1 =  %g (kg m^2)\n', IC1);
fprintf('IC1 alpha1 = [ %g, %g, %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*hSlider*wSlider*d;
Fin2 = -m2*aC2;
G2 = [0,-m2*g,0];
IC2 = m2*(hSlider^2+wSlider^2)/12;
Min2 = -IC2*Alpha2;
fprintf('m2 =  %g (kg)\n', m2);
fprintf('m2 aC2 = [ %g, %g, %g] (N)\n', m2*aC2 );
fprintf('Fin2 = - m2 aC2 = [ %g, %g, %d] (N)\n', Fin2 );
fprintf('G2 = - m2 g = [ %g, %g, %g] (N)\n', G2 );
fprintf('IC2 =  %g (kg m^2)\n', IC2);
fprintf('IC2 alpha2 = [ %g, %g, %g] (N m)\n', IC2*Alpha2 );
fprintf('Min2 = - IC2 alpha2 = [ %d, %d, %g] (N m)\n', Min2 );
fprintf('\n');

fprintf('Link 3  \n');
m3 = rho*DF*h*d;
Fin3 = -m3*aC3;
G3 = [0,-m3*g,0];
IC3 = m3*(DF^2+h^2)/12;
Min3 = -IC3*Alpha3;
fprintf('m3 =  %g (kg)\n', m3);
fprintf('m3 aC3 = [ %g, %g, %g] (N)\n', m3*aC3 );
fprintf('Fin3 = - m3 aC3 = [ %g, %g, %d] (N)\n', Fin3 );
fprintf('G3 = - m3 g = [ %g, %g, %g] (N)\n', G3 );
fprintf('IC3 =  %g (kg m^2)\n', IC3);
fprintf('IC3 alpha3 = [ %g, %g, %g] (N m)\n', IC3*Alpha3 );
fprintf('Min3 = - IC3 alpha3 = [ %d, %d, %g] (N m)\n', Min3 );
fprintf('\n');

fprintf('Link 4  \n');
m4 = rho*hSlider*wSlider*d;
Fin4 = -m4*aC4;
G4 = [0,-m4*g,0];
IC4 = m4*(hSlider^2+wSlider^2)/12;
Min4 = -IC4*Alpha4;
fprintf('m4 =  %g (kg)\n', m4);
fprintf('m4 aC4 = [ %g, %g, %g] (N)\n', m4*aC4 );
fprintf('Fin4 = - m4 aC4 = [ %g, %g, %d] (N)\n', Fin4 );
fprintf('G4 = - m4 g = [ %g, %g, %g] (N)\n', G4 );
fprintf('IC4 =  %g (kg m^2)\n', IC4);
fprintf('IC4 alpha4 = [ %g, %g, %g] (N m)\n', IC4*Alpha4 );
fprintf('Min4 = - IC4 alpha4 = [ %d, %d, %g] (N m)\n', Min4 );
fprintf('\n');

fprintf('Link 5  \n');
m5 = rho*EG*h*d;
Fin5 = -m5*aC5;
G5 = [0,-m5*g,0];
IC5 = m5*(EG^2+h^2)/12;
Min5 = -IC5*Alpha5;
fprintf('m5 =  %g (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, %g, %g] (N)\n', G5 );
fprintf('IC5 =  %g (kg m^2)\n', IC5);
fprintf('IC5 alpha5 = [ %g, %g, %g] (N m)\n', IC5*Alpha5 );
fprintf('Min5 = - IC5 alpha5 = [ %d, %d, %g] (N m)\n', Min5 );

fprintf('\n');
fprintf('Joint reactions and equilibrium moment \n');
fprintf('\n');

% Links 5 and 4 - dyad 4 & 5 (RTR)
fprintf('Dyad 4 & 5 \n'); fprintf('\n');

F05x=sym('F05x','real');
F05y=sym('F05y','real');
F34x=sym('F34x','real');
F34y=sym('F34y','real');
F05=[ F05x, F05y, 0 ] ; % unknown joint force of 0 on 5
F34=[ F34x, F34y, 0 ] ; % unknown joint force of 3 on 4

% Sum of the forces for 5 and 4
eqF45=F05+G5+G4+F34-m5*aC5-m4*aC4; 
eqF45x=eqF45(1); % projection on x-axis  eq(1)
eqF45y=eqF45(2); % projection on y-axis  eq(2)

SF45x=vpa(eqF45x,6);
fprintf('%s = 0 (1)\n', char(SF45x));
SF45y=vpa(eqF45y,6);
fprintf('%s = 0 (2)\n', char(SF45y));

% Sum of the moments for 5 and 4 wrt D  
eqMD45=cross(rE-rD,F05)+cross(rC5-rD,G5-m5*aC5)-IC5*Alpha5-IC4*Alpha4+Me; 
eqMD45z=eqMD45(3); % projection on z-axis  eq(3)

SMD45z=vpa(eqMD45z,6);
fprintf('%s = 0 (3)\n', char(SMD45z));

% Sum of the forces for 4 projected on ED
eqF4DE=dot(F34+G4-m4*aC4,rD-rE); % eq(4)
F4DE=vpa(eqF4DE,6);
fprintf('%s = 0 (4)\n', char(F4DE));

% eqs(1)-(4) => F05x, F05y, F34x, F34y
fprintf('Eqs(1)-(4) => F05x, F05y, F34x, F34y \n');
solDI=solve(eqF45x, eqF45y , eqMD45z, eqF4DE);
F05xs=eval(solDI.F05x);
F05ys=eval(solDI.F05y);
F34xs=eval(solDI.F34x);
F34ys=eval(solDI.F34y);
F05s=[ F05xs, F05ys, 0 ];
F34s=[ F34xs, F34ys, 0 ];
fprintf('F05 = [ %g, %g, %g] (N)\n', F05s );
fprintf('F34 = [ %g, %g, %g] (N)\n', F34s ); fprintf('\n');

% Sum of the forces for 5: F45+F05+G5=m5*aC5 => F45
F45=m5*aC5-G5-F05s;
fprintf('F45 = [ %g, %g, %g] (N)\n', F45 );
fprintf('verify: F45 perp to DE   F45.ED = %g \n',dot(F45,rD-rE));
fprintf('\n');

xP=sym('xP','real');
yP=sym('yP','real');
rP=[xP, yP, 0]; % application point of force F45
eqP=cross(rD-rE,rP-rE); % P is on the line ED
eqPz=eqP(3); % eq(5)
Pz=vpa(eqPz,6);
fprintf('%s = 0 (5)\n', char(Pz));
% Sum of the moments for 4 wrt C4=D 
eqM4=cross(rP-rC4,-F45)-IC4*Alpha4;
eqM4z=eqM4(3); % eq(6)
M4z=vpa(eqPz,6);
fprintf('%s = 0 (6)\n', char(M4z));
fprintf('Eqs(5)-(6) => xP, yP \n');
% eqs(5)-(6) => xP, yP
solP=solve(eqPz,eqM4z);
xPs=eval(solP.xP);
yPs=eval(solP.yP);
rPs=[xPs, yPs, 0];
fprintf('rP = [ %g, %g, %g] (m)\n', rPs );
fprintf('rP - rD = [ %g, %g, %g] (m)\n', rPs-rD );
fprintf('because \n');
fprintf('-IC4*Alpha4(3) =  %g is a very small number\n',-IC4*Alpha4(3));
fprintf('\n');

% Links 2 and 3 - dyad 2 & 3 (RTR)
fprintf('Dyad 2 & 3 \n');

F03x=sym('F03x','real');
F03y=sym('F03y','real');
F12x=sym('F12x','real');
F12y=sym('F12y','real'); 
F03=[ F03x, F03y, 0 ] ; % unknown joint force of 0 on 3
F12=[ F12x, F12y, 0 ] ; % unknown joint force of 1 on 2
F43=-F34s;
% Sum of the forces for 2 and 3
eqF23=F43+F03+G3-m3*aC3+G2-m2*aC2+F12;
eqF23x=eqF23(1); % projection on x-axis  eq(7)
SF23x=vpa(eqF23x,6);
fprintf('%s = 0 (7)\n', char(SF23x));
eqF23y=eqF23(2); % projection on y-axis  eq(8)
SF23y=vpa(eqF23y,6);
fprintf('%s = 0 (8)\n', char(SF23y));
% Sum of the moments for 2 and 3 wrt B  
eqMB3=cross(rD-rB,F43)+cross(rC-rB,F03)+cross(rC3-rB,G3-m3*aC3);
eqMB2=-IC3*Alpha3-IC2*Alpha2; 
eqMB23=eqMB3+eqMB2;
eqMB23z=eqMB23(3); % eq(9)
SMB23z=vpa(eqMB23z,6);
fprintf('%s = 0 (9)\n', char(SMB23z));
% Sum of the forces for 2 projected on BC
eqF2BC=dot(F12+G2-m2*aC2, rC-rB); % eq(10)
F2BC=vpa(eqF2BC,6);
fprintf('%s = 0 (10)\n', char(F2BC));
% eqs(7)-(10) => F03x, F03y, F12x, F12y
fprintf('Eqs(7)-(10) => F03x, F03y, F12x, F12y \n');
solDII = solve(eqF23x, eqF23y , eqMB23z, eqF2BC);
F03xs=eval(solDII.F03x);
F03ys=eval(solDII.F03y);
F12xs=eval(solDII.F12x);
F12ys=eval(solDII.F12y);
F03s=[ F03xs, F03ys, 0 ];
F12s=[ F12xs, F12ys, 0 ];
fprintf('F03 = [ %g, %g, %g] (N)\n', F03s );
fprintf('F12 = [ %g, %g, %g] (N)\n', F12s );

fprintf('\n');
F32=m2*aC2-G2-F12s;
fprintf('F32 = [ %g, %g, %g] (N)\n', F32 );
fprintf('verify: F32 perp to BC   F32.BC = %g \n',dot(F32,rC-rB));
fprintf('\n');

xQ=sym('xQ','real');
yQ=sym('yQ','real');
rQ=[xQ, yQ, 0]; % application point of force F32
eqQ=cross(rC-rB,rQ-rC); % Q is on the line BC
eqQz=eqQ(3); % eq(11)
Qz=vpa(eqQz,6);
fprintf('%s = 0 (11)\n', char(Qz));
% Sum of the moments for 2 wrt C2=B 
eqM2=cross(rQ-rC2,F32)-IC2*Alpha2;
eqM2z=eqM2(3); % eq(12)
SM2z=vpa(eqM2z,6);
fprintf('%s = 0 (12)\n', char(SM2z));
fprintf('Eqs(11)-(12) => xQ, yQ \n');
% eqs(11)-(12) => xQ, yQ
solQ=solve(eqQz,eqM2z);
xQs=eval(solQ.xQ);
yQs=eval(solQ.yQ);
rQs=[xQs, yQs, 0];
fprintf('rQ = [ %g, %g, %g] (m)\n', rQs );
fprintf('rQ - rB = [ %g, %g, %g] (m)\n', rQs-rB );
fprintf('because\n');
fprintf('-IC2*Alpha2(3) = %g is a very small number \n',-IC2*Alpha2(3));
fprintf('\n');

% Link 1
fprintf('Link 1 \n');
fprintf('\n');
% Sum of the forces for 1 
F01=m1*aC1-G1+F12s;
fprintf('F01 = [ %g, %g, %g] (N)\n', F01 );
% Sum of the moments for 1 wrt A 
Mm=-cross(rB,-F12s)-cross(rC1,G1-m1*aC1)-IC1*alpha1;
fprintf('Mm = [ %d, %d, %g] (N m)\n', Mm );