% R-RTR
% velocity,acceleration, dynamic forces

clear; clc; close all

AB = 0.10 ;  % (m)
AC = 0.05 ;  % (m)
CD = 0.15 ;  % (m)
phi = pi/6;  % (rad)

xA = 0; yA = 0; 
rA_ = [xA yA 0]; 
xC = AC ; yC = 0; 
rC_ = [xC yC 0]; 

xB = AB*cos(phi); 
yB = AB*sin(phi); 
rB_ = [xB yB 0];  

phi2 = atan((yB-yC)/(xB-xC)); 
phi3 = phi2;

CB = norm([xB-xC, yB-yC]);
ux = (xB - xC)/CB;
uy = (yB - yC)/CB;
xD = xC + CD*ux;
yD = yC + CD*uy;
rD_ = [xD yD 0];

fprintf('Results \n\n') 
fprintf('phi = phi1 = %g (degrees) \n',phi*180/pi) 
fprintf('rA_ = [%6.3f,%6.3f,%g] (m)\n',rA_) 
fprintf('rC_ = [%6.3f,%6.3f,%g] (m)\n',rC_) 
fprintf('phi2 = phi3 = %6.3f (degrees) \n',phi2*180/pi) 
fprintf('rD_ = [%6.3f,%6.3f,%g] (m)\n',rD_) 

% Graphic of the mechanism
axis manual
axis equal
hold on
grid on
ax = 0.2;
axis([-ax ax -ax ax])
xlabel('x (m)'), ylabel('y (m)')
title('positions for phi = 30 (deg)')


textAx = xA+0.01;
textAy = yA;

textBx = xB+0.01;
textBy = yB-0.01;

textCx = xC+0.01;
textCy = yC;

textDx = xD+0.007;
textDy = yD+0.007;
 
% plotting paramters
text_size = 10;
slider_scale_factor = 0.015;
pivot_scale_factor = 0.005;        
         
text(textAx, textAy,'A','FontSize',text_size)
plot([xA xB], [yA yB], '-o')
text(textBx, textBy,'B','FontSize',text_size)
text( textCx, textCy,'C','FontSize',text_size)        
plot([xC xD], [yC yD], '-o')
text(textDx, textDy,'D','FontSize',text_size)

fprintf('\n') 
fprintf('Velocity and acceleration analysis \n\n') 
n = -50.; %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,%6.3f] (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_;
aB2_ = aB1_;
fprintf...
('vB1_ = vB2_ =[%6.3f,%6.3f,%g] (m/s)\n',vB1_)  
fprintf...
('aB1_ = aB2_ =[%6.3f,%6.3f,%g] (m/s^2)\n',aB1_)  
fprintf('\n')

omega3z=sym('omega3z','real'); 
vB32=sym('vB32','real');
omega3u_ = [ 0 0 omega3z]; 
% omega3z unknown (to be calculated)
% vB32 unknown (to be calculated)
vC_ = [0 0 0]; % C is fixed
% vB3_ = vC_ + omega3_ x rCB_   
% (B3 & C are points on link 3)
vB3_ = vC_ + cross(omega3u_,rB_-rC_); 
% point B2 is on link 2 and point B3 is on link 3
% vB3_ = vB2_ + vB3B2_
% between the links 2 and 3 there is a translational 
% joint B_T
% vB3B2_ is the relative velocity of B3 wrt 2
% vB3B2_ is parallel to the sliding direcion BC
% vB3B2_ is written as a vector
vB3B2u_ = vB32*[cos(phi2) sin(phi2) 0]; 
% vB3 = vB2 + vB3B2
eqvB_ = vB3_ - vB2_ - vB3B2u_; 
% vectorial equation
% the component of the vectorial equation on x-axis
eqvBx = eqvB_(1); 
% the component of the vectorial equation on y-axis
eqvBy = eqvB_(2); 
% two equations eqvBx and eqvBy with two unknowns 

digits(3)
% print the equations for calculating omega3 and vB32
fprintf...
('vB3_ = vC_ + omega3_ x rCB_ = vB2_ + vB3B2_ => \n') 
qvBx=vpa(eqvBx);
fprintf('x-axis: %s = 0 \n',char(qvBx)) 
qvBy=vpa(eqvBy);
fprintf('y-axis: %s = 0 \n',char(qvBy)) 
fprintf('=>\n') 

% solve for omega3z and vB32 
solvB = solve(eqvBx,eqvBy); 
omega3zs=eval(solvB.omega3z); 
vB32s=eval(solvB.vB32);

omega3_ = [0 0 omega3zs]; 
omega2_ = omega3_;
vB3B2_ = vB32s*[cos(phi2) sin(phi2) 0];

fprintf('omega3z = %6.3f (rad/s)\n',omega3zs) 
fprintf('vB32 = %6.3f (m/s)\n',vB32s) 
fprintf('\n') 
fprintf('omega2_ = omega3_ = [%g,%g,%6.3f](rad/s)\n',omega3_) 
fprintf('vB32_ = [%6.3f,%6.3f,%g] (m/s)\n',vB3B2_)  
fprintf('\n') 

% Coriolis acceleration
aB3B2cor_ = 2*cross(omega3_,vB3B2_); 

alpha3z=sym('alpha3z','real'); 
aB32=sym('aB32','real'); % aB32 unknown

alpha3u_ = [0 0 alpha3z]; % alpha3z unknown
aC_ = [0 0 0]; % C is fixed
% aB3 acceleration of B3
aB3_=...
    aC_+cross(alpha3u_,rB_-rC_)...
    -dot(omega3_,omega3_)*(rB_-rC_); 
% aB3B2_ relative velocity of B3 wrt 2
% aB3B2_ parallel to the sliding direcion BC

aB3B2u_ = aB32*[cos(phi2) sin(phi2) 0]; 
% aB3_ = aB2_ + aB3B2_ + aB3B2cor_
eqaB_ = aB3_ - aB2_ - aB3B2u_ - aB3B2cor_; 
% vectorial equation
eqaBx = eqaB_(1); % equation component on x-axis
eqaBy = eqaB_(2); % equation component on y-axis

% print the equations for calculating alpha3 and aB32
fprintf('aB32cor_ = [%6.3f,%6.3f,%g] (m/s^2)\n',aB3B2cor_)  
fprintf('\n') 
fprintf('aB3_=aC_+alpha3_ x rCB_-(omega3_.omega3_)rCB_=') 
fprintf('aB2_+aB3B2_+aB3B2cor_ =>\n') 
qaBx=vpa(eqaBx);
fprintf('x-axis: %s = 0 \n',char(qaBx)) 
qaBy=vpa(eqaBy);
fprintf('y-axis: %s = 0 \n',char(qaBy)) 
fprintf('=>\n');

solaB = solve(eqaBx,eqaBy);
alpha3zs=eval(solaB.alpha3z);
aB32s=eval(solaB.aB32);
alpha3_ = [0 0 alpha3zs];
alpha2_ = alpha3_;
aB32_ = aB32s*[cos(phi2) sin(phi2) 0];

fprintf('alpha3z = %6.3f (rad/s)\n', alpha3zs) 
fprintf('aB32 = %6.3f (m/s)\n', aB32s) 
fprintf('\n') 
fprintf('alpha2_=alpha3_=[%g,%g,%6.3f](rad/s^2)\n',alpha3_) 
fprintf('aB3B2_=[%6.3f,%6.3f,%g] (m/s^2)\n',aB32_) 
fprintf('\n') 

% vD_ velocity of D
% D & C points on link 3
vD_ = vC_ + cross(omega3_,rD_-rC_);
fprintf('vD_ = [%6.3f,%6.3f,%g] (m/s)\n', vD_) 
% aD3_ acceleration of D3
aD_=...
   aC_+cross(alpha3_,rD_-rC_)...
   -dot(omega3_,omega3_)*(rD_-rC_);  
fprintf('aD_ = [%6.3f,%6.3f,%g] (m/s^2)\n', aD_) 
fprintf('\n')

%%%%%%%%%%%%%%%%%%%
rC1_ = rB_/2; 
rC2_ = rB_; 
rC3_ = (rD_+rC_)/2;
aC1_ = aB1_/2; 
aC2_ = aB1_; 
aC3_ = (aD_+aC_)/2; 

fprintf('rC1_ = [%6.3f,%6.3f,%d] (m)\n',rC1_)
fprintf('rC2_ = [%6.3f,%6.3f,%d] (m)\n',rC2_)
fprintf('rC3_ = [%6.3f,%6.3f,%d] (m)\n',rC3_)
fprintf('aC1_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC1_)
fprintf('aC2_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC2_)
fprintf('aC3_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC3_)
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('\n')


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

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

fprintf('Link 1 \n')
m1 = rho*AB*h*d; % mass
G1_ = [0,-m1*g,0]; % force of gravity
Fin1_ = -m1*aC1_; % force of inertia
IC1 = m1*(AB^2+h^2)/12; % mass moment of inertia
IA = IC1 + m1*(AB/2)^2; % A fixed point
alpha1_ = [0 0 0]; % angular acceleration
Min1_ = -IC1*alpha1_; % moment of inertia
fprintf('m1 = %6.3f (kg)\n', m1)
fprintf('G1_ = - m1 g_ = [%d,%6.3f,%d] (N)\n', G1_)
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('IC1 = %g (kg m^2)\n', IC1)
fprintf('IA = %g (kg m^2)\n', IA)
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*hSlider*wSlider*d;
G2_ = [0,-m2*g,0];
Fin2_ = -m2*aC2_;
IC2 = m2*(hSlider^2+wSlider^2)/12;
Min2_ = -IC2*alpha2_;
fprintf('m2 = %6.3f (kg)\n', m2)
fprintf('G2_ = - m2 g_ = [%d,%6.3f,%d] (N)\n', G2_)
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('IC2 = %6.3d (kg m^2)\n', IC2)
fprintf('IC2 alpha2_=[%g,%g,%6.3d](N m)\n',IC2*alpha2_)
fprintf('Min2_=-IC2 alpha2=[%d,%d,%6.3d](N m)\n',Min2_)
fprintf('\n')

fprintf('Link 3 \n')
m3 = rho*CD*h*d;
G3_ = [0,-m3*g,0];
Fin3_ = -m3*aC3_;
IC3 = m3*(CD^2+h^2)/12;
IC  = IC3+m3*(CD/2)^2; % C fixed point
Min3_ = -IC3*alpha3_;
fprintf('m3 = %6.3f (kg)\n', m3)
fprintf('G3_ = - m3 g_ = [%g, %6.3f, %g] (N)\n', G3_)
fprintf('m3 aC3_ = [%6.3f,%6.3f,%g] (N)\n', m3*aC3_)
fprintf('Fin3_ = - m3 aC3 =[%6.3f,%6.3f,%d] (N)\n',Fin3_)
fprintf('IC3 = %6.3d (kg m^2)\n', IC3)
fprintf('IC3 alpha3_=[%d,%d,%6.3d](N m)\n',IC3*alpha3_)
fprintf('Min3_=-IC3 alpha3=[%d,%d,%6.3d](N m)\n',Min3_)
fprintf('\n')

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

% Newton-Euler eom for link 3
F23x=sym('F23x','real'); % force of 2 on 3 at B x-comp
F23y=sym('F23y','real'); % force of 2 on 3 at B y-comp
M23z=sym('M23z','real'); % moment of 2 on 3 z-component
F23_=[F23x, F23y, 0]; % unknown joint force 
M23_=[0, 0, M23z]; % unknown joint moment 

F23_=[F23x, F23y, 0]; % unknown joint force 
M23_=[0, 0, M23z]; % unknown joint moment 

% F23_ perpendicular to BC: F32.BC = 0 => 
eqF23 = (rB_-rC_)*F23_.'; % (1)

% sum M of 3 wrt C (C 'fixed' point)
% CB_xF23_+CC3xG3+M23_+Me_=IC*alpha3_ 
eqM3C_ = ...
  cross(rB_-rC_,F23_)+cross(rC3_-rC_,G3_)...
  +M23_+Me_-IC*alpha3_;
eqM3z = eqM3C_(3); % (2)
  
% Newton-Euler eom for link 2
F12x=sym('F12x','real'); % force of 1 on 2 at B x-comp
F12y=sym('F12y','real'); % force of 1 on 2 at B y-comp
F12_=[F12x, F12y, 0]; % unknown joint force 

% sum F of 2 
% F12_+G2_+(-F23_) = m2*aC2_
eqF2_ = F12_+G2_+(-F23_)-m2*aC2_;
eqF2x = eqF2_(1); % (3)
eqF2y = eqF2_(2); % (4)

% sum M of 2 wrt B=C2 
% -M23_=IC2*alpha2_ 
eqM2_ = -M23_ - IC2*alpha2_;
eqM2z = eqM2_(3); % (5)

% Eqs.(1)-(5) +>
sol23=solve(eqF23,eqM3z,eqF2x,eqF2y,eqM2z);
F23xs=eval(sol23.F23x);
F23ys=eval(sol23.F23y);
M23zs=eval(sol23.M23z);
F12xs=eval(sol23.F12x);
F12ys=eval(sol23.F12y);

F12s_=[F12xs, F12ys, 0];
F23s_=[F23xs, F23ys, 0];
M23s_=[0,0,M23zs];

fprintf('F23_ = [%6.3f,%6.3f,%g] (N)\n',F23s_)
fprintf('F12_ = [%6.3f,%6.3f,%g] (N)\n',F12s_)
fprintf('M23_ = [%g,%g,%6.6f] (N m)\n',M23s_)

% F03_+G3_+F23_ = m3*aC3_ =>
F03_ = m3*aC3_-(G3_+F23s_);
fprintf('F03_ = [%6.3f,%6.3f,%g] (N)\n',F03_)

% sum M of 1 wrt A (A 'fixed' point)
% IA*alpha1_=rC1_xG1_)+rB_x(-F12s_)+Mm_ =>
Mm_=IA*alpha1_-(cross(rC1_,G1_)+cross(rB_,-F12s_));
fprintf('Mm_ = [%g,%g,%6.3f] (N m)\n',Mm_)

% eof

% Results 
% 
% phi = phi1 = 30 (degrees) 
% rA_ = [ 0.000, 0.000,0] (m)
% rC_ = [ 0.050, 0.000,0] (m)
% phi2 = phi3 = 53.794 (degrees) 
% rD_ = [ 0.139, 0.121,0] (m)
% 
% Velocity and acceleration analysis 
% 
% omega1_ = [0,0,-5.236] (rad/s)
% alpha1_ = [0,0, 0.000] (rad/s^2)
% 
% vB1_ = vB2_ =[ 0.262,-0.453,0] (m/s)
% aB1_ = aB2_ =[-2.374,-1.371,0] (m/s^2)
% 
% vB3_ = vC_ + omega3_ x rCB_ = vB2_ + vB3B2_ => 
% x-axis: - 0.05*omega3z - 0.591*vB32 - 0.262 = 0 
% y-axis: 0.0366*omega3z - 0.807*vB32 + 0.453 = 0 
% =>
% omega3z = -7.732 (rad/s)
% vB32 =  0.211 (m/s)
% 
% omega2_ = omega3_ = [0,0,-7.732](rad/s)
% vB32_ = [ 0.125, 0.170,0] (m/s)
% 
% aB32cor_ = [ 2.636,-1.930,0] (m/s^2)
% 
% aB3_=aC_+alpha3_ x rCB_-(omega3_.omega3_)rCB_=aB2_+aB3B2_+aB3B2cor_ =>
% x-axis: - 0.591*aB32 - 0.05*alpha3z - 2.45 = 0 
% y-axis: 0.0366*alpha3z - 0.807*aB32 + 0.311 = 0 
% =>
% alpha3z = -34.865 (rad/s)
% aB32 = -1.196 (m/s)
% 
% alpha2_=alpha3_=[0,0,-34.865](rad/s^2)
% aB3B2_=[-0.706,-0.965,-0] (m/s^2)
% 
% vD_ = [ 0.936,-0.685,0] (m/s)
% aD_ = [-1.077,-10.324,0] (m/s^2)
% 
% rC1_ = [ 0.043, 0.025,0] (m)
% rC2_ = [ 0.087, 0.050,0] (m)
% rC3_ = [ 0.094, 0.061,0] (m)
% aC1_ = [-1.187,-0.685,0] (m/s^2)
% aC2_ = [-2.374,-1.371,0] (m/s^2)
% aC3_ = [-0.538,-5.162,0] (m/s^2)
% alpha1_ = [0,0, 0.000](rad/s^2)
% alpha2_ = [0,0,-34.865](rad/s^2)
% alpha3_ = [0,0,-34.865](rad/s^2)
% 
% 
% Dynamic force analysis 
% Link 1 
% m1 =  0.008 (kg)
% G1_ = - m1 g_ = [0,-0.078,0] (N)
% m1 aC1_ = [-0.009,-0.005,0] (N)
% Fin1_= - m1 aC1_ = [ 0.009, 0.005,0] (N)
% IC1 = 6.73333e-06 (kg m^2)
% IA = 2.69333e-05 (kg m^2)
% IC1 alpha1_=[ 0.000, 0.000,0](N m)
% Min1_=-IC1 alpha1_=[0, 0, 0](N m)
% 
% Link 2 
% m2 =  0.006 (kg)
% G2_ = - m2 g_ = [0,-0.063,0] (N)
% m2 aC2_ = [-0.015,-0.009,0] (N)
% Fin2_= - m2 aC2 =[ 0.015, 0.009,0] (N)
% IC2 = 1.067e-06 (kg m^2)
% IC2 alpha2_=[0,0,-3.719e-05](N m)
% Min2_=-IC2 alpha2=[0,0,3.719e-05](N m)
% 
% Link 3 
% m3 =  0.012 (kg)
% G3_ = - m3 g_ = [0, -0.118, 0] (N)
% m3 aC3_ = [-0.006,-0.062,0] (N)
% Fin3_ = - m3 aC3 =[ 0.006, 0.062,0] (N)
% IC3 = 2.260e-05 (kg m^2)
% IC3 alpha3_=[0,0,-7.880e-04](N m)
% Min3_=-IC3 alpha3=[0,0,7.880e-04](N m)
% 
% Me_ = [0, 0, 100.000] (N m)
% 
% F23_ = [1302.143,-953.235,0] (N)
% F12_ = [1302.128,-953.181,0] (N)
% M23_ = [0,0,0.000037] (N m)
% F03_ = [-1302.149,953.291,0] (N)
% Mm_ = [0,0,-147.651] (N m)

