% Problem 6.1 
% 
% Results 
% 
% rA =  [ 0, 0, 0 ] (m)
% rD =  [ 0.09, 0, 0 ] (m)
% rB =  [ 0.034641, 0.02, 0 ] (m)
% rC =  [ 0.103859, -0.0375222, 0 ] (m)
% phi1 = 30 (degrees) 
% phi2 = -39.7274 (degrees) 
% phi3 = -69.7274 (degrees) 
% rC1 = rB/2 = [ 0.0173205, 0.01, 0 ] (m)
% rC2 = (rB+rC)/2 = [ 0.0692503, -0.00876109, 0 ] (m)
% rC3 = (rC+rD)/2 = [ 0.0969297, -0.0187611, 0 ] (m)
% 
% Velocity and acceleration analysis 
% 
% omega1 =  [ 0, 0, 12.5664 ] (rad/s)
% alpha1 =  [ 0, 0, 0 ] (rad/s^2)
% 
% vB = vB1 = vB2 =  [ -0.251327, 0.435312, 0 ] (m/s)
% aB = aB1 = aB2 =  [ -5.47029, -3.15827, 0 ] (m/s^2)
% 
% vC = vB + omega2 x rBC = vD + omega3 x rDC  => 
% x-axis: -.251327+.575222e-1*omega2z-.375222e-1*omega3z = 0 
% y-axis: .435312+.692185e-1*omega2z-.138595e-1*omega3z = 0 
% =>
% omega2z =  -11.0095 (rad/s)
% omega3z =  -23.5759 (rad/s)
% 
% omega2 = [ 0, 0, -11.0095 ] (rad/s)
% omega3 = [ 0, 0, -23.5759 ] (rad/s)
% 
% vC =  [ -0.884619, -0.32675, 0 ] (m/s)
% 
% aC = aC2 = aB + omega2 x rBC - (omega2.omega2)rBC 
% aC = aC3 = aD + omega3 x rDC - (omega3.omega3)rDC 
% x-axis: -6.15680+.575222e-1*alpha2z-.375222e-1*alpha3z = 0 
% y-axis: -17.0417+.692185e-1*alpha2z-.138595e-1*alpha3z = 0 
% =>
% alpha2z =  307.84 (rad/s^2)
% alpha3z =  307.84 (rad/s^2)
% 
% alpha2 = [ 0, 0, 307.84 ] (rad/s^2)
% alpha3 = [ 0, 0, 307.84 ] (rad/s^2)
% 
% aC =  [ 3.84741, 25.1222, 0] (m/s^2)
% 
% aC1 = aB/2 = [ -2.73515, -1.57914, 0] (m/s^2)
% aC2 = (aB+aC)/2 = [ -0.811439, 10.982, 0] (m/s^2)
% aC3 = (aC+aD)/2 = [ 1.92371, 12.5611, 0] (m/s^2)
% 
% 
% 
%  link 1 
% m1 = 0.0032 (kg)
% IC1 = 4.53333e-07 (kg m^2)
% G1 = [ 0, -0.0313824, 0 ] (N)
% m1*aC1 = [ -0.00875246, -0.00505324, 0 ] (N)
% IC1*alpha1 = [ 0, 0, 0 ] (N m)
%  link 2 
% m2 = 0.0072 (kg)
% IC2 = 4.92e-06 (kg m^2)
% G2 = [ 0, -0.0706104, 0 ] (N)
% m1*aC2 = [ -0.00584236, 0.07907, 0 ] (N)
% IC2*alpha2 = [ 0, 0, 1.514573e-03 ] (N m)
%  link 3 
% m3 = 0.0032 (kg)
% IC3 = 4.53333e-07 (kg m^2)
% G3 = [ 0, -0.0313824, 0 ] (N)
% m3*aC3 = [ 0.00615586, 0.0401955, 0 ] (N)
% IC3*alpha3 = [ 0, 0, 1.395541e-04 ] (N m)
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Problem 6.2 
%
% Results 
% rA =  [ 0, 0, 0 ] (m)
% rD =  [ 0.3, 0.45, 0 ] (m)
% rB =  [ 0.129904, 0.075, 0 ] (m)
% rC =  [ -0.0689445, 0.422073, 0 ] (m)
% rE =  [ -0.298288, 0.404712, 0 ] (m)
% rF =  [ -0.37, 0.186177, 0 ] (m)
% phi1 = 30 (degrees) 
% phi2 = -60.1903 (degrees) 
% phi3 = 4.32878 (degrees) 
% phi4 = 71.8329 (degrees) 
% rC1 = rB/2 = [ 0.0649519, 0.0375, 0 ] (m)
% rC2 = (rB+rC)/2 = [ 0.0304796, 0.248536, 0 ] (m)
% rC3 = (rD+rE)/2 = [ 0.000855797, 0.427356, 0 ] (m)
% rC4 = (rE+rF)/2 = [ -0.334144, 0.295445, 0 ] (m)
% rC5 = rF = [ -0.37, 0.186177, 0 ] (m)
% Velocity and acceleration analysis 
% omega1 =  [ 0, 0, 6.28319 ] (rad/s)
% alpha1 =  [ 0, 0, 0 ] (rad/s^2)
% vB = vB1 = vB2 =  [ -0.471239, 0.81621, 0 ] (m/s)
% aB = aB1 = aB2 =  [ -5.1284, -2.96088, 0 ] (m/s^2)
% vC = vB + omega2 x rBC = vD + omega3 x rDC  => 
% x-axis: -.471238-.347073*omega2z-.279275e-1*omega3z = 0 
% y-axis: .816210-.198848*omega2z+.368945*omega3z = 0 
% =>
% omega2z =  -1.1307 (rad/s)
% omega3z =  -2.82169 (rad/s)
% omega2 = [ 0, 0, -1.1307 ] (rad/s)
% omega3 = [ 0, 0, -2.82169 ] (rad/s)
% vC =  [ -0.0788027, 1.04105, 0 ] (m/s)
% vE =  [ -0.127788, 1.68819, 0 ] (m/s)
% vF = vE + omega4 x rEF  => 
% x-axis: .127788-.218535*omega4z = 0 
% y-axis: vFy-1.68819+.717116e-1*omega4z = 0 
% =>
% omega4z =  0.58475 (rad/s)
% vFy =  1.64625 (m/s)
% omega4 = [ 0, 0, 0.58475 ] (rad/s)
% vF =  [ 0, 1.64625, 0 ] (m/s)
% aC = aC2 = aB + alpha2 x rBC - (omega2.omega2)rBC 
% aC = aC3 = aD + alpha3 x rDC - (omega3.omega3)rDC 
% x-axis: -7.81169-.347073*alpha2z-.279275e-1*alpha3z = 0 
% y-axis: -3.62697-.198848*alpha2z+.368945*alpha3z = 0 
% =>
% alpha2z =  -22.33 (rad/s^2)
% alpha3z =  -2.20443 (rad/s^2)
% alpha2 = [ 0, 0, -22.33 ] (rad/s^2)
% alpha3 = [ 0, 0, -2.20443 ] (rad/s^2)
% aC =  [ 2.87595, 1.03567, 0] (m/s^2)
% aE =  [ 4.66371, 1.67947, 0] (m/s^2)
% aF = aE + alpha4 x rEF - (omega4.omega4)rEF 
% x-axis: -4.68823-.218535*alpha4z = 0 
% y-axis: aFy-1.75419+.717116e-1*alpha4z = 0 
% =>
% alpha4z =  -21.453 (rad/s^2)
% aFy =  3.29262 (m/s^2)
% alpha4 = [ 0, 0, -21.453 ] (rad/s^2)
% aF =  [ 0, 3.29262, 0 ] (m/s^2)
% aC1 = aB/2 = [ -2.5642, -1.48044, 0] (m/s^2)
% aC2 = (aB+aC)/2 = [ -1.12622, -0.962605, 0] (m/s^2)
% aC3 = (aD+aE)/2 = [ 2.33185, 0.839733, 0] (m/s^2)
% aC4 = (aE+aF)/2 = [ 2.33185, 2.48604, 0] (m/s^2)
% aC5 = aF = [ 0, 3.29262, 0] (m/s^2)
% alpha1 = [ 0, 0, 0 ] (rad/s^2)
% alpha2 = [ 0, 0, -22.33 ] (rad/s^2)
% alpha3 = [ 0, 0, -2.20443 ] (rad/s^2)
% alpha4 = [ 0, 0, -21.453 ] (rad/s^2)
%
%  link 1 
% m1 = 0.012 (kg)
% IC1 = 2.26e-05 (kg m^2)
% G1 = [ 0, -0.117684, 0 ] (N)
% m1*aC1 = [ -0.0307704, -0.0177653, 0 ] (N)
% IC1*alpha1 = [ 0, 0, 0 ] (N m)
%  link 2 
% m2 = 0.032 (kg)
% IC2 = 0.000426933 (kg m^2)
% G2 = [ 0, -0.313824, 0 ] (N)
% m1*aC2 = [ -0.0360391, -0.0308034, 0 ] (N)
% IC2*alpha2 = [ 0, 0, -9.533416e-03 ] (N m)
%  link 3 
% m3 = 0.048 (kg)
% IC3 = 0.0014404 (kg m^2)
% G3 = [ 0, -0.470736, 0 ] (N)
% m3*aC3 = [ 0.111929, 0.0403072, 0 ] (N)
% IC3*alpha3 = [ 0, 0, -3.175265e-03 ] (N m)
%  link 4 
% m4 = 0.0184 (kg)
% IC4 = 8.12667e-05 (kg m^2)
% G4 = [ 0, -0.180449, 0 ] (N)
% m4*aC4 = [ 0.0429061, 0.0457432, 0 ] (N)
% IC4*alpha4 = [ 0, 0, -1.743415e-03 ] (N m)
%  link 5  
% m5 =  0.008 (kg)
% IC5 =  1.93333e-06 (kg m^2)
% G5 = - m5 g = [ 0, -0.078456, 0] (N)
% m5*aC5 = [ 0, 0.026341, 0] (N)
% IC5*alpha5 = [ 0, 0, 0] (N m)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Problem 6.3 
%
% Results 
% rA =  [ 0, 0, 0 ] (m)
% rD =  [ -1.04491, -0.154122, 0 ] (m)
% rB =  [ -0.1, 0.173205, 0 ] (m)
% rC =  [ -0.6, 0, 0 ] (m)
% rE =  [ -0.75, 0.25, 0 ] (m)
% rF =  [ -1.10369, -0.234668, 0 ] (m)
% phi1 = 120 (degrees) 
% phi2 = 19.1066 (degrees) 
% phi5 = 53.8796 (degrees) 
% rC1 = rB/2 = [ -0.05, 0.0866025, 0 ] (m)
% rC2 = (rB+rD)/2 = [ -0.572456, 0.00954166, 0 ] (m)
% rC3 = rC = [ -0.6, 0, 0 ] (m)
% rC4 = rD = [ -1.04491, -0.154122, 0 ] (m)
% rC5 = (rE+rF)/2 = [ -0.926845, 0.00766601, 0 ] (m)
% Velocity and acceleration analysis 
% omega1 =  [ 0, 0, 6.28319 ] (rad/s)
% alpha1 =  [ 0, 0, 0 ] (rad/s^2)
% vB = vB1 = vB2 =  [ -1.08828, -0.628319, 0 ] (m/s)
% aB = aB1 = aB2 =  [ 3.94784, -6.83786, 0 ] (m/s^2)
% vC2 = vB + omega2 x rBC = vC3 + vC23 (vC3=0)  => 
% x-axis: -1.08828+.173205*omega2z-.944911*vC23 = 0 
% y-axis: -.628318-.500000*omega2z-.327327*vC23 = 0 
% =>
% omega2z =  -0.448799 (rad/s)
% vC23 =  -1.23399 (m/s)
% omega2 = omega3 = [ 0, 0, -0.448799 ] (rad/s)
% vC2C3 = [ -1.16601, -0.403919, -0 ] (m/s)
% vD2 =  [ -1.23518, -0.204243, 0 ] (m/s)
% vD5 = vE + omega5 x rED = vD4 + vD54 (vD4=vD2)  => 
% x-axis: .404122*omega5z+1.23518-.589484*vD54 = 0 
% y-axis: -.294911*omega5z+.204243-.807780*vD54 = 0 
% =>
% omega5z =  -1.75371 (rad/s)
% vD23 =  0.893105 (m/s)
% omega5 = omega5 = [ 0, 0, -1.75371 ] (rad/s)
% vD2D3 = [ 0.526471, 0.721432, 0 ] (m/s)
% vF =  [ -0.849967, 0.620271, 0 ] (m/s)
% aC23cor =  [ -0.362557, 1.04661, 0 ] (m/s^2)
% aC2 = aB + alpha2 x rBC - (omega2.omega2)rBC 
% aC2 = aC3 + aC23 + aC23cor (aC3=0)  
% x-axis: 4.41111+.173205*alpha2z-.944911*aC23 = 0 
% y-axis: -7.84959-.500000*alpha2z-.327327*aC23 = 0 
% =>
% alpha2z =  -16.7458 (rad/s^2)
% aC23 =  1.59873 (m/s^2)
% alpha2 = [ 0, 0, -16.7458 ] (rad/s^2)
% aC2C3 = [ 1.51065, 0.523306, 0 ] (m/s^2)
% aD2 =  [ -1.34318, 9.05135, 0] (m/s^2)
% aD54cor =  [ 2.53037, -1.84656, 0 ] (m/s^2)
% aD5 = aE + alpha5 x rED - (omega4.omega4)rED (aE=0) 
% aD5 = aD4 + aD54 + aD54cor (aD4=aD2)  
% x-axis: .404122*alpha5z-.280188-.589484*aD54 = 0 
% y-axis: -.294911*alpha5z-5.96192-.807780*aD54 = 0 
% =>
% alpha5z =  -6.57248 (rad/s^2)
% aD54 =  -4.98108 (m/s^2)
% alpha5 = [ 0, 0, -6.57248 ] (rad/s^2)
% aD5D4 = [ -2.93627, -4.02362, -0 ] (m/s^2)
% aF =  [ -2.09769, 3.81522, 0] (m/s^2)
% alpha1 = [ 0, 0, 0 ] (rad/s^2)
% alpha2 = alpha3 = [ 0, 0, -16.7458 ] (rad/s^2)
% alpha4 = alpha5 = [ 0, 0, -6.57248 ] (rad/s^2)
% aC1 = [ 1.97392, -3.41893, 0 ] (m/s^2)
% aC2 = [ 1.30233, 1.10674, 0 ] (m/s^2)
% aC3 = [ 0, 0, 0 ] (m/s^2)
% aC4 = aD4 = aD2 = [ -1.34318, 9.05135, 0 ] (m/s^2)
% aC5 = [ -1.04885, 1.90761, 0 ] (m/s^2)
%    
%    
%  link 1 
% m1 = 0.016 (kg)
% IC1 = 5.34667e-05 (kg m^2)
% G1 = [ 0, -0.156912, 0 ] (N)
% m1*aC1 = [ 0.0315827, -0.0547029, 0 ] (N)
% IC1*alpha1 = [ 0, 0, 0 ] (N m)
%  link 2 
% m2 = 0.08 (kg)
% IC2 = 0.00666733 (kg m^2)
% G2 = [ 0, -0.78456, 0 ] (N)
% m1*aC2 = [ 0.104187, 0.0885394, 0 ] (N)
% IC2*alpha2 = [ 0, 0, -1.116497e-01 ] (N m)
%  link 3 
% m3 = 0.008 (kg)
% IC3 = 1.93333e-06 (kg m^2)
% G3 = [ 0, -0.078456, 0 ] (N)
% m3*aC3 = [ 0, 0, 0 ] (N)
% IC3*alpha3 = [ 0, 0, -3.237519e-05 ] (N m)
%  link 4 
% m4 = 0.008 (kg)
% IC4 = 1.93333e-06 (kg m^2)
% G4 = [ 0, -0.078456, 0 ] (N)
% m4*aC4 = [ -0.0107454, 0.0724108, 0 ] (N)
% IC4*alpha4 = [ 0, 0, -1.270679e-05 ] (N m)
%  link 5  
% m5 =  0.048 (kg)
% IC5 =  0.0014404 (kg m^2)
% G5 = - m5 g = [ 0, -0.470736, 0] (N)
% m5*aC5 = [ -0.0503447, 0.0915653, 0] (N)
% IC5*alpha5 = [ 0, 0, -0.009467] (N m)