% R-RRT-RTR Mechanism
% Dyad method

clear; clc; close all

AB = 0.20 ;	% (m) link 1
BC = 0.21 ;	% (m) link 2 	
CD = 0.39 ;	% (m) link 2
% link 3 is the slider at D
CF = 0.45 ; % (m) link 4
% link 5 is the slider at E

a  = 0.30 ;	% (m) distance from E to x-axis	
b  = 0.25 ;	% (m) distance from E to y-axis
c  = 0.05 ;	% (m) distance from D to x-axis	

% Joint A (origin)
xA = 0; yA = 0;
rA_ = [xA yA 0]; % position vector of A
vA_ = [0 0 0];   % velocity vector of A 
aA_ = [0 0 0];   % acceleration vector of A  

% Joint E (fixed point)
xE =  a;
yE = -b;
rE_ = [xE yE 0]; % position vector of E
vE_ = [0 0 0];   % velocity vector of E 
aE_ = [0 0 0];   % acceleration vector of E  

% y-coordinate of D 
yD = c;

phi = 45*pi/180 ; % (rad) angle of link 1 with x-axis
n = -500 ;        % (rpm) angular speed of link 1 (constant)

omega = pi*n/30; % (rad/s) angular velocity of 1
alpha = 0; % (rad/s^2) angular acceleration of 1

omega1_ = [0 0 omega]; % angular velocity vector of 1
alpha1_ = [0 0 alpha]; % angular acceleration vector of 1



% Joint B 
% call pvaR function 
% pvaR(rAi_, vAi_, aAi_, li, phi, omega, alpha)
% rAi_ = rA_
% vAi_ = vA_
% aAi_ = aA_
% li = AB
% phi = phi
% omega = omega
% alpha = alpha
pvaB = pvaR(rA_, vA_, aA_, AB, phi, omega, alpha);

xB = pvaB(1); yB = pvaB(2); 
rB_ = [xB yB 0]; % position of B

vBx = pvaB(3); vBy = pvaB(4); 
vB_ = [vBx vBy 0];  % velocity of B 

aBx = pvaB(5); aBy = pvaB(6); 
aB_ = [aBx aBy 0]; % acceleration of B

% rB_ = [ 0.087, 0.050,0] (m)
% vB_ = [ 0.262, -0.453, 0] (m/s)
% aB_ = [-2.374, -1.371, 0] (m/s^2)

fprintf('Results \n\n') 
fprintf('phi = phi1 = %g (degrees) \n',phi*180/pi) 
fprintf('rB_ = [%6.3f,%6.3f,%g] (m)\n',rB_) 
fprintf('vB_ = [%6.3f,%6.3f,%g] (m/s)\n',vB_) 
fprintf('aB_ = [%6.3f,%6.3f,%g] (m/s^2)\n',aB_) 
fprintf('\n') 

% phi = phi1 = 45 (degrees) 
% rB_ = [ 0.141, 0.141,0] (m)
% vB_ = [ 7.405,-7.405,0] (m/s)
% aB_ = [-387.715,-387.715,0] (m/s^2)

  
% position of joint D
% angle of the sliding direction
phi3 = pi;
% As point on the sliding direction
xAs = 0; yAs = c;
rAs_ = [xAs yAs 0]; 
% call function posRRT
% posRRT(rAi_, rAs_, li, Theta)
% rAi_ = rB_
% rAs_ = rAs_ = [0 c 0]  As on the x-sliding direction
% li = BC+CD
% Theta = phi3 angle of the sliding direction with x-axis
posD = posRRT(rB_, rAs_, BC+CD, phi3);

xD1 = posD(1);
xD2 = posD(3);

% select the correct position of D
% 0 < phi < pi/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 with x-axis
phi2 = atan2((yB-yD),(xB-xD));
fprintf...
('phi2 = %6.3f (degrees) \n', phi2*180/pi)
% phi2 = 171.236 (degrees) 

%
% position of joint C
% call function pos3P (3 points on a line)
% pos3P(rP_, rQ_ , PQ)
% rP_ = rB_
% rQ_ = rD_
% PQ = BC: B->C->D
posC = pos3P(rB_, rD_, BC);

xC = posC(1);
yC = posC(2);
rC_ = [xC yC 0]; % position vector of C 
fprintf('rC_ = [%6.3f, %6.3f, %g] (m)\n', rC_)

% angle of link 4 (link 5) with x-axis
phi4 = atan2((yC-yE),(xC-xE));
phi5 = phi4;
fprintf...
('phi4 = phi5 = %6.3f (degrees) \n', phi4*180/pi)
% phi4 = phi5 = 82.242 (degrees) 


xF = xC-CF*cos(phi4);
yF = yC-CF*sin(phi4);
rF_ = [xF yF 0];

fprintf('rE_ = [%6.3f, %6.3f, %g] (m)\n', rE_) 
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([-.4 sx -.4 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','LineWidth',2);
plot(xC, yC, 'k.','Color', 'red')
xlabel('x (m)'),ylabel('y (m)')
pivot0_scale = 0.015;  
slider_scale = 0.05;      
% plotting joint A 
plot_pivot0( xA, yA, pivot0_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, pivot0_scale, pi/2)

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

% eof

% Results 
% 
% phi = phi1 = 45 (degrees) 
% rB_ = [ 0.141, 0.141,0] (m)
% vB_ = [ 7.405,-7.405,0] (m/s)
% aB_ = [-387.715,-387.715,0] (m/s^2)
% 
% rE_ = [ 0.300, -0.250, 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)

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

% velocity and acceleration of D
% As a point on the sliding direction
% xAs = 0; yAs = c; => rAs_ = [xAs yAs 0]; 
vAs_ = [0 0 0]; % velocity of As
aAs_ = [0 0 0]; % acceleration of As 

% velaccRRT(rAi_,rAs_,rAj_,vAi_,vAs_,aAi_,aAs_,dtheta,ddtheta)
% rAi_ = rB_
% rAs_ = rAs_
% rAj_ = rD_
% vAi_ = vB_
% vAs_ = vAs_
% aAi_ = aB_
% aAs_ = aAs_
% dtheta = 0 slider direction is fixed
% ddtheta = 0
vaD=velaccRRT(rB_,rAs_,rD_,vB_,vAs_,aB_,aAs_,0,0);
vDx = vaD(1);
vDy = vaD(2);
vD_ = [vDx vDy 0]; % velocity vector of D 
aDx = vaD(3);
aDy = vaD(4);
aD_ = [aDx aDy 0]; % acceleration vector of D

fprintf('vD_ = [%6.3f, %d, %d] (m/s)\n', vD_)
fprintf('aD_ = [%6.3f, %d, %d] (m/s^2)\n', aD_)
fprintf('\n')

% vD_ = [ 8.546, 0, 0] (m/s)
% aD_ = [-422.604, 0, 0] (m/s^2)


% angular velocity and acceleration of 2
% call  angvelacc
% angvelacc(rP_,rQ_, vP_,vQ_, aP_,aQ_)
% rP_ = rB_
% rQ_ = rD_
% vP_ = vB_
% vQ_ = vD_
% aP_ = aB_
% aQ_ = aD_
ang2=angvelacc(rB_,rD_,vB_,vD_,aB_,aD_);
omega2z = ang2(1);
alpha2z = ang2(2);

omega2_ = [0 0 omega2z]; % angular velocity of 2
alpha2_ = [0 0 alpha2z]; % angular acceleration of 2

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

omega3_ = [0 0 0]; % angular velocity of 3 
alpha3_ = [0 0 0]; % angular acceleration of 3

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

% velocity and acceleration of C
% linvelacc(rP_,rQ_,vP_,aP_,omega,alpha)
% rP_ = rB_
% rQ_ = rC_
% vP_ = vB_
% aP_ = aB_
% omega = omega2z
% alpha = alpha2z
vaC = linvelacc(rB_,rC_,vB_,aB_,omega2z,alpha2z);
vCx = vaC(1);
vCy = vaC(2);
vC_ = [vCx vCy 0]; % velocity vector of C 
aCx = vaC(3);
aCy = vaC(4);
aC_ = [aCx aCy 0]; % acceleration vector of C


fprintf('vC_ = [%6.3f, %6.3f, %d] (m/s)\n', vC_)
fprintf('aC_ = [%6.3f, %6.3f, %d] (m/s^2)\n', aC_)
fprintf('\n') 
% vC_ = [ 7.804, -4.813, 0] (m/s)
% aC_ = [-399.926, -252.015, 0] (m/s^2)

% angular velocity and acceleration of 4
% call  angvelacc
% angvelacc(rP_,rQ_, vP_,vQ_, aP_,aQ_)
% rP_ = rC_
% rQ_ = rE_
% vP_ = vC_
% vQ_ = vE_
% aP_ = aC_
% aQ_ = aE_
ang4=angvelacc(rC_,rE_,vC_,vE_,aC_,aE_);
omega4z = ang4(1);
alpha4z = ang4(2);

omega4_ = [0 0 omega4z];  % angular velocity of 4 
alpha4_ = [0 0 alpha4z];  % angular acceleration of 4

omega5_ = omega4_;  % angular velocity of 5
alpha5_ = alpha4_;  % angular acceleration of 5

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

% omega4_ = omega5_ = [0,0,-23.109](rad/s)
% alpha4_ = alpha5_ = [0,0,525.219](rad/s^2)

% velocity and acceleration of F
% linvelacc(rP_,rQ_,vP_,aP_,omega,alpha)
% rP_ = rC_
% rQ_ = rF_
% vP_ = vC_
% aP_ = aC_
% omega = omega4z
% alpha = alpha4z
vaF=linvelacc(rC_,rF_,vC_,aC_,omega4z,alpha4z);
vFx = vaF(1);
vFy = vaF(2);
vF_ = [vFx vFy 0]; % velocity vector of F 
aFx = vaF(3);
aFy = vaF(4);
aF_ = [aFx aFy 0]; % acceleration vector of F

fprintf('vF_ = [%6.3f, %6.3f, %d] (m/s)\n', vF_)
fprintf('aF_ = [%6.3f, %6.3f, %d] (m/s^2)\n', aF_)
fprintf('\n') 

% vF_ = [-2.500, -3.409, 0] (m/s)
% aF_ = [-133.299, -45.808, 0] (m/s^2)

rC1_ = rB_/2;       % position vector of CM of 1
rC2_ = (rB_+rD_)/2; % position vector of CM of 2
rC3_ = rD_;         % position vector of CM of 3
rC4_ = (rC_+rF_)/2; % position vector of CM of 4
rC5_ = rE_;         % position vector of CM of 5

aC1_ = aB_/2;       % acceleration of C1
aC2_ = (aB_+aD_)/2; % acceleration of C2
aC3_ = aD_;         % acceleration of C3
aC4_ = (aC_+aF_)/2; % acceleration of C4
aC5_ = [0 0 0];     % acceleration of C5

% external moment on link 5
Me_ = -sign(omega5_(3))*[0 0 1000];
fprintf('Me_ = [%d, %d, %6.3f] (N m)\n', Me_)
fprintf('\n')
% Me_ = [0, 0, 1000.000] (N m)



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

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;
Min1_ = -IC1*alpha1_;
fprintf('m1 = %6.3f (kg)\n', m1)
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('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('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('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('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('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('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('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('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('Min5_=-IC5 alpha5_=[%d, %d, %6.3d](N m)\n',Min5_)
fprintf('\n')

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


% Dyad RTR (4 & 5)
F5_ = Fin5_ + G5_; % total force on 5
M5_ = Min5_ + Me_; % total moment on 5
F4_ = Fin4_ + G4_; % total force on 4
M4_ = Min4_;       % total moment on 4

% RTR -> CEE (4 & 5) 
% dforceRTR(rAi_,rAj_,rCi_,Fi_,Mi_,Fj_,Mj_)
% out = [FAixs,FAiys,FAjxs,FAjys,Fijx,Fijy,Mijz];
% rAi_ = rC_ 
% rAj_ = rE_  
% rCi_ = rC4_
% Fi_ = F4_
% Mi_ = M4_
% Fj_ = F5_ 
% Mj_ = M5_ 

FD45=dforceRTR(rC_,rE_,rC4_,F4_,M4_,F5_,M5_);
F24x = FD45(1);
F24y = FD45(2);
F05x = FD45(3);
F05y = FD45(4);
F45x = FD45(5);
F45y = FD45(6);
M45z = FD45(7);

F24_ = [F24x F24y 0]; % reaction force of 2 on 4 at C
F05_ = [F05x F05y 0]; % reaction force of 0 on 5 at E
F45_ = [F45x F45y 0]; % reaction force of 4 on 5 at E
M45_ = [0 0 M45z];    % reaction moment of 4 on 5 at E

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

% Dyad RRT (2 & 3)
F2_ = Fin2_ + G2_ - F24_;  % total force on 2
M2_ = Min2_+cross(rC_-rC2_,-F24_); % total moment on 2
F3_ = Fin3_ + G3_; % total force on 3
M3_ = Min3_; % total moment on 3

% dforceRRT(rAi_,rAs_,rAj_,rCi_,Fi_,Mi_,Fj_,Mj_)
% out = [FAixs,FAiys,FAjxs,FAjys,Fijx,Fijy,Mijz];
% rAi_ = rB_
% rAs_ = rAs_ 
% rAj_ = rD_
% rCi_ = rC2_
% Fi_  = F2_
% Mi_  = M2_
% Fj_  = F3_
% Mj_  = M4_
FD23=dforceRRT(rB_,rAs_,rD_,rC2_,F2_,M2_,F3_,M3_);
F12xs = FD23(1); 
F12ys = FD23(2);
F03xs = FD23(3);
F03ys = FD23(4);
F23xs = FD23(5);
F23ys = FD23(6);
M03zs = FD23(7);

F12_ = [F12xs F12ys 0]; % reaction force of 1 on 2 at B
F03_ = [F03xs F03ys 0]; % reaction force of 0 on 3 at D
F23_ = [F23xs F23ys 0]; % reaction force of 2 on 3 at D
M03_ = [0 0 M03zs];     % reaction moment of 0 on 3

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

% forceDR(rA_,rB_,rC1_,F1_,M1_,F21_);
% out = [F01_(1), F01_(2), Mm_(3)];

F21_ = -F12_;     % joint force on 2 on 1 at B
F1_ = Fin1_+G1_;  % total force on 1
M1_ = Min1_;      % total moment on 1
FDL = dforceR(rA_,rB_,rC1_,F1_,M1_,F21_);

F01xs = FDL(1);
F01ys = FDL(2);
Mms = FDL(3);

F01_ = [F01xs F01ys 0]; % reaction force of 0 on 1 at A
Mm_ = [0 0 Mms]; % equilibrium (motor) moment

fprintf('F01_ = [%6.3f,%6.3f,%d] (N)\n',F01_)
fprintf('Mm_ = [%d,%d,%6.3f] (N m)\n',Mm_)
fprintf('\n')



% 
% Results 
% 
% phi = phi1 = 45 (degrees) 
% rB_ = [ 0.141, 0.141,0] (m)
% vB_ = [ 7.405,-7.405,0] (m/s)
% aB_ = [-387.715,-387.715,0] (m/s^2)
% 
% rE_ = [ 0.300, -0.250, 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)
% 
% vD_ = [ 8.546, 0, 0] (m/s)
% aD_ = [-422.604, 0, 0] (m/s^2)
% 
% omega2_ = [0,0,12.487](rad/s)
% alpha2_ = [0,0,629.786](rad/s^2)
% 
% omega3_ = [0,0,0](rad/s)
% alpha3_ = [0,0,0](rad/s^2)
% 
% vC_ = [ 7.804, -4.813, 0] (m/s)
% aC_ = [-399.926, -252.015, 0] (m/s^2)
% 
% omega4_ = omega5_ = [0,0,-23.109](rad/s)
% alpha4_ = alpha5_ = [0,0,525.219](rad/s^2)
% 
% vF_ = [-2.500, -3.409, 0] (m/s)
% aF_ = [-133.299, -45.808, 0] (m/s^2)
% 
% 
% Me_ = [0, 0, 1000.000] (N m)
% 
% Inertia forces and inertia moments
% 
% Link 1 
% m1 =  0.320 (kg)
% Fin1_= - m1 aC1_ = [62.034,62.034,0] (N)
% G1_ = - m1 g = [0,-3.138,0] (N)
% IC1 = 0.00106667 (kg m^2)
% Min1_=-IC1 alpha1_=[0, 0, 0](N m)
% 
% Link 2 
% m2 =  0.960 (kg)
% Fin2_= - m2 aC2_ = [388.953,186.103,0] (N)
% G2_ = - m2 g = [0,-9.415,0] (N)
% IC2 = 0.0288 (kg m^2)
% Min2_=-IC2 alpha2_=[0, 0, -1.813784e+01](N m)
% 
% Link 3 
% m3 =  0.512 (kg)
% Fin3_= - m3 aC3_ =[216.373,-0.000,0] (N)
% G3_ = - m3 g_ = [0,-5.021,0] (N)
% IC3 = 3.413e-04 (kg m^2)
% Min3_=-IC3 alpha3_=[0,0,   000](N m)
% 
% Link 4 
% m4 =  0.720 (kg)
% Fin4_= - m4 aC4_ =[191.961,107.216,0] (N)
% G4_ = - m4 g_ = [0,-7.061,0] (N)
% IC4 = 1.620e-02 (kg m^2)
% Min4_=-IC4 alpha4_=[0,0,-8.509e+00](N m)
% 
% Link 5 
% m5 =  0.512 (kg)
% Fin5_ = - m5 aC5 =[-0.000,-0.000,0] (N)
% G5_ = - m5 g = [0, -5.021, 0] (N)
% IC5 = 3.413e-04 (kg m^2)
% Min5_=-IC5 alpha5_=[0, 0, -1.793e-01](N m)
% 
% 
% Dynamic force analysis  
% Dyad 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)
% 
% F03_ = [ 0.000,-143.783,0] (N)
% F12_ = [2024.907,-512.546,0] (N)
% F23_ = [-216.373,148.804,0] (N)
% M03_ = [0,0,0] (N m)
% 
% 
% F01_ = [1962.873,-571.442,0] (N)
% Mm_ = [0,0,-358.628] (N m)
% 
