MGAircraftTrifilarPendulumDynamicsFBD.html  (MotionGenesis input/output).
```   (1) % MotionGenesis file:  MGAircraftTrifilarPendulumDynamicsFBD.txt
(2) % Copyright (c) 2009-2018 Motion Genesis LLC.
(3) %--------------------------------------------------------------------
(4) SetAutoZ( OFF )
(5) NewtonianFrame  N                    % Earth / aircraft hanger.
(6) RigidBody       C                    % Aircraft.
(7) Point           N1(N), N2(N), N3(N)  % End-points of cable on N.
(8) Point           C1(C), C2(C), C3(C)  % End-points of cable on C.
(9) %--------------------------------------------------------------------
(10) Constant  L1 = 30 m                  % Length of cable between N1 and C1.
(11) Constant  L2 = 30 m                  % Length of cable between N2 and C2.
(12) Constant  L3 = 30 m                  % Length of cable between N3 and C3.
(13) Constant  dN = 30 m                  % Distance between No and N1.
(14) Constant  dC = 30 m                  % Distance between Co and C1.
(15) Constant  wN = 20 m                  % Distance between No and N2.
(16) Constant  wC = 20 m                  % Distance between Co and C2.
(17) Constant dcm =  8 m                  % Distance between Co and Ccm.
(18) Constant  g = 9.8 m/s^2              % Earth's gravitational acceleration.
(19) Variable  x'', y'', z''              % Locates Ccm from No.
(20) Variable  q1', q2', q3'              % BodyZYX Euler angles.
(21) Variable  wx', wy', wz'              % Cxyz> measures of C's angular velocity in N.
(22) Variable  T1, T2, T3                 % Tension in cables.
(23) Constant  bT = 8.0E3 noUnits         % Torque damping constant.
(24) Constant  bF = 6.0E3 noUnits         % Force  damping constant.
(25) Constant  epsilonV   = 1.0E-5 m/s    % Small number to avoid divide by zero errors.
(26) Constant  epsilonW   = 1.0E-5 rad/s  % Small number to avoid divide by zero errors.
(27) Constant  expDamping = 0.25 noUnits  % Exponent used with damping force/torques.
(28) %--------------------------------------------------------------------
(29) C.SetMassInertia( m = 9000 kg, Ixx = 4.0E5 kg*m^2,  Iyy = 3.0E5 kg*m^2,  Izz = Ixx + Iyy )
-> (30) Izz = Ixx + Iyy

(31) %--------------------------------------------------------------------
(32) %   Rotational kinematics.
(33) C.SetAngularVelocityAcceleration( N, wx*Cx> + wy*Cy> + wz*Cz> )
-> (34) w_C_N> = wx*Cx> + wy*Cy> + wz*Cz>
-> (35) alf_C_N> = wx'*Cx> + wy'*Cy> + wz'*Cz>

(36) C.SetRotationMatrixODE( N, BodyZYX, q1, q2, q3 )
-> (37) C_N[1,1] = cos(q1)*cos(q2)
-> (38) C_N[1,2] = sin(q1)*cos(q2)
-> (39) C_N[1,3] = -sin(q2)
-> (40) C_N[2,1] = sin(q2)*sin(q3)*cos(q1) - sin(q1)*cos(q3)
-> (41) C_N[2,2] = cos(q1)*cos(q3) + sin(q1)*sin(q2)*sin(q3)
-> (42) C_N[2,3] = sin(q3)*cos(q2)
-> (43) C_N[3,1] = sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3)
-> (44) C_N[3,2] = sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1)
-> (45) C_N[3,3] = cos(q2)*cos(q3)
-> (46) q1' = (wy*sin(q3)+wz*cos(q3))/cos(q2)
-> (47) q2' = wy*cos(q3) - wz*sin(q3)
-> (48) q3' = wx + tan(q2)*(wy*sin(q3)+wz*cos(q3))

(49) %--------------------------------------------------------------------
(50) %   Translational kinematics.
(51) CCm.Translate( No, x*Nx> + y*Ny> + z*Nz> )
-> (52) p_No_Ccm> = x*Nx> + y*Ny> + z*Nz>
-> (53) v_Ccm_N> = x'*Nx> + y'*Ny> + z'*Nz>
-> (54) a_Ccm_N> = x''*Nx> + y''*Ny> + z''*Nz>

(55) Co.Translate( CCm, -dcm*Cx> )
-> (56) p_Ccm_Co> = -dcm*Cx>
-> (57) v_Co_N> = -dcm*wz*Cy> + dcm*wy*Cz> + x'*Nx> + y'*Ny> + z'*Nz>
-> (58) a_Co_N> = dcm*(wy^2+wz^2)*Cx> - dcm*(wx*wy+wz')*Cy> - dcm*(wx*wz-wy')*Cz>
+ x''*Nx> + y''*Ny> + z''*Nz>

(59) C1.SetPositionVelocity( Co,  dC*Cx> )
-> (60) p_Co_C1> = dC*Cx>
-> (61) v_C1_N> = (dC-dcm)*wz*Cy> - (dC-dcm)*wy*Cz> + x'*Nx> + y'*Ny> + z'*Nz>

(62) C2.SetPositionVelocity( Co, -wC*Cy> )
-> (63) p_Co_C2> = -wC*Cy>
-> (64) v_C2_N> = wC*wz*Cx> - dcm*wz*Cy> + (dcm*wy-wC*wx)*Cz> + x'*Nx> + y'*Ny> + z'*Nz>

(65) C3.SetPositionVelocity( Co,  wC*Cy> )
-> (66) p_Co_C3> = wC*Cy>
-> (67) v_C3_N> = -wC*wz*Cx> - dcm*wz*Cy> + (dcm*wy+wC*wx)*Cz> + x'*Nx> + y'*Ny> + z'*Nz>

(68) N1.SetPosition( No,  dN*Nx> )
-> (69) p_No_N1> = dN*Nx>

(70) N2.SetPosition( No, -wN*Ny> )
-> (71) p_No_N2> = -wN*Ny>

(72) N3.SetPosition( No,  wN*Ny> )
-> (73) p_No_N3> = wN*Ny>

(74) %--------------------------------------------------------------------
(75) %   Configuration constraints: Length of cables.
(76) CableConstraint[1] = C1.GetDistanceSquared( N1 ) - L1^2
-> (77) CableConstraint[1] = (dC-dcm)^2 + y^2 + z^2 + (dN-x)^2 + 2*(dC-dcm)*y*
sin(q1)*cos(q2) - L1^2 - 2*(dC-dcm)*z*sin(q2) - 2*(dC-dcm)*cos(q1)*cos(
q2)*(dN-x)

(78) CableConstraint[2] = C2.GetDistanceSquared( N2 ) - L2^2
-> (79) CableConstraint[2] = dcm^2 + wC^2 + x^2 + z^2 + (wN+y)^2 + 2*dcm*z*sin(q2)
+ 2*wC*x*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)) - L2^2 - 2*dcm*x*cos
(q1)*cos(q2) - 2*wC*z*sin(q3)*cos(q2) - 2*dcm*sin(q1)*cos(q2)*(wN+y)
- 2*wC*(wN+y)*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))

(80) CableConstraint[3] = C3.GetDistanceSquared( N3 ) - L3^2
-> (81) CableConstraint[3] = dcm^2 + wC^2 + x^2 + z^2 + (wN-y)^2 + 2*dcm*z*sin(q2)
+ 2*wC*z*sin(q3)*cos(q2) + 2*dcm*sin(q1)*cos(q2)*(wN-y) - L3^2 - 2*dcm*
x*cos(q1)*cos(q2) - 2*wC*x*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))
- 2*wC*(wN-y)*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))

(82) %--------------------------------------------------------------------
(83) %   Add forces from gravity and tension.
-> (85) Force_Ccm> = m*g*Nz>

(86) unitVectorToN1FromC1> = N1.GetPosition(C1) / L1
-> (87) unitVectorToN1FromC1> = -(dC-dcm)/L1*Cx> + (dN-x)/L1*Nx> - y/L1*Ny> - z/L1*Nz>

(88) unitVectorToN2FromC2> = N2.GetPosition(C2) / L2
-> (89) unitVectorToN2FromC2> = dcm/L2*Cx> + wC/L2*Cy> - x/L2*Nx> - (wN+y)/L2*Ny> - z/L2*Nz>

(90) unitVectorToN3FromC3> = N3.GetPosition(C3) / L3
-> (91) unitVectorToN3FromC3> = dcm/L3*Cx> - wC/L3*Cy> - x/L3*Nx> + (wN-y)/L3*Ny> - z/L3*Nz>

(92) C1.AddForce( N1,  T1 * unitVectorToN1FromC1>  )
-> (93) Force_C1_N1> = -(dC-dcm)*T1/L1*Cx> + T1*(dN-x)/L1*Nx> - T1*y/L1*Ny>
- T1*z/L1*Nz>

(94) C2.AddForce( N2,  T2 * unitVectorToN2FromC2>  )
-> (95) Force_C2_N2> = dcm*T2/L2*Cx> + wC*T2/L2*Cy> - T2*x/L2*Nx> - T2*(wN+y)/L2*Ny>
- T2*z/L2*Nz>

(96) C3.AddForce( N3,  T3 * unitVectorToN3FromC3>  )
-> (97) Force_C3_N3> = dcm*T3/L3*Cx> - wC*T3/L3*Cy> - T3*x/L3*Nx> + T3*(wN-y)/L3*Ny>
- T3*z/L3*Nz>

(98) %--------------------------------------------------------------------
(99) %   Add force to damp translational motion.
(100) vMag = Ccm.GetSpeed( N )
-> (101) vMag = sqrt(x'^2+y'^2+z'^2)

(102) unitVectorVelocity> = CCm.GetVelocity( N ) / ( vMag + epsilonV )
-> (103) unitVectorVelocity> = x'/(epsilonV+vMag)*Nx> + y'/(epsilonV+vMag)*Ny>
+ z'/(epsilonV+vMag)*Nz>

(104) dampingForce> = -bF *  vMag^expDamping * unitVectorVelocity>
-> (105) dampingForce> = -bF*x'*vMag^expDamping/(epsilonV+vMag)*Nx> - bF*y'*vMag
^expDamping/(epsilonV+vMag)*Ny> - bF*z'*vMag^expDamping/(epsilonV+vMag)*Nz>

-> (107) Force_Ccm> = -bF*x'*vMag^expDamping/(epsilonV+vMag)*Nx> - bF*y'*vMag^expDamping
/(epsilonV+vMag)*Ny> + (m*g-bF*z'*vMag^expDamping/(epsilonV+vMag))*Nz>

(108) %--------------------------------------------------------------------
(109) %   Add torque to damp rotational motion.
(110) wMag = C.GetAngularSpeed( N )
-> (111) wMag = sqrt(wx^2+wy^2+wz^2)

(112) unitVectorAngularVelocity> = C.GetAngularVelocity( N ) / ( wMag + epsilonW )
-> (113) unitVectorAngularVelocity> = wx/(epsilonW+wMag)*Cx> + wy/(epsilonW+wM
ag)*Cy> + wz/(epsilonW+wMag)*Cz>

(114) dampingTorque> =  -bT *  wMag^expDamping * unitVectorAngularVelocity>
-> (115) dampingTorque> = -bT*wx*wMag^expDamping/(epsilonW+wMag)*Cx> - bT*wy*
wMag^expDamping/(epsilonW+wMag)*Cy> - bT*wz*wMag^expDamping/(epsilonW+
wMag)*Cz>

-> (117) Torque_C> = -bT*wx*wMag^expDamping/(epsilonW+wMag)*Cx> - bT*wy*wMag^expDamping
/(epsilonW+wMag)*Cy> - bT*wz*wMag^expDamping/(epsilonW+wMag)*Cz>

(118) %--------------------------------------------------------------------
(119) %   Calculate resultant force on C in terms of Fx, Fy, Fz.
(120) ResultantForce> = C.GetResultantForce()
-> (121) ResultantForce> = (dcm*T2/L2+dcm*T3/L3-(dC-dcm)*T1/L1)*Cx> + wC*(T2/L2
-T3/L3)*Cy> + (T1*(dN-x)/L1-T2*x/L2-T3*x/L3-bF*x'*vMag^expDamping/(ep
silonV+vMag))*Nx> + (T3*(wN-y)/L3-T1*y/L1-T2*(wN+y)/L2-bF*y'*vMag^expDamping
/(epsilonV+vMag))*Ny> + (m*g-T1*z/L1-T2*z/L2-T3*z/L3-bF*z'*vMag^expDamping
/(epsilonV+vMag))*Nz>

(122) Fx = Dot( ResultantForce>, Nx> )
-> (123) Fx = T1*(dN-x)/L1 + cos(q1)*cos(q2)*(dcm*T2/L2+dcm*T3/L3-(dC-dcm)*T1/L1)
- T2*x/L2 - T3*x/L3 - wC*(T2/L2-T3/L3)*(sin(q1)*cos(q3)-sin(q2)*sin(
q3)*cos(q1)) - bF*x'*vMag^expDamping/(epsilonV+vMag)

(124) Fy = Dot( ResultantForce>, Ny> )
-> (125) Fy = T3*(wN-y)/L3 + sin(q1)*cos(q2)*(dcm*T2/L2+dcm*T3/L3-(dC-dcm)*T1/L1)
+ wC*(T2/L2-T3/L3)*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)) - T1*y/L1
- T2*(wN+y)/L2 - bF*y'*vMag^expDamping/(epsilonV+vMag)

(126) Fz = Dot( ResultantForce>, Nz> )
-> (127) Fz = m*g + wC*sin(q3)*cos(q2)*(T2/L2-T3/L3) - T1*z/L1 - T2*z/L2 - T3*z
/L3 - sin(q2)*(dcm*T2/L2+dcm*T3/L3-(dC-dcm)*T1/L1) - bF*z'*vMag^expDamping
/(epsilonV+vMag)

(128) ResultantForce> := Fx*Nx> + Fy*Ny> + Fz*Nz>
-> (129) ResultantForce> = Fx*Nx> + Fy*Ny> + Fz*Nz>

(130) %--------------------------------------------------------------------
(131) %   Calculate moment of all forces on C about Ccm in terms of Mx, My, Mz.
(132) ResultantMoment> = C.GetMomentOfForces( Ccm )
-> (133) ResultantMoment> = -bT*wx*wMag^expDamping/(epsilonW+wMag)*Cx> - bT*wy*
wMag^expDamping/(epsilonW+wMag)*Cy> - bT*wz*wMag^expDamping/(epsilonW+
wMag)*Cz> + (T3*(dcm*z*sin(q1)*cos(q2)-dcm*sin(q2)*(wN-y)-wC*sin(q3)*
cos(q2)*(wN-y)-wC*z*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))/L3-(dC-
dcm)*T1*(y*sin(q2)+z*sin(q1)*cos(q2))/L1-T2*(wC*sin(q3)*cos(q2)*(wN+y)
-dcm*sin(q2)*(wN+y)-dcm*z*sin(q1)*cos(q2)-wC*z*(cos(q1)*cos(q3)+sin(
q1)*sin(q2)*sin(q3)))/L2)*Nx> + ((dC-dcm)*T1*(z*cos(q1)*cos(q2)-sin(
q2)*(dN-x))/L1-T3*(dcm*x*sin(q2)+dcm*z*cos(q1)*cos(q2)+wC*x*sin(q3)*
cos(q2)+wC*z*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))/L3-T2*(dcm*x*
sin(q2)+dcm*z*cos(q1)*cos(q2)-wC*x*sin(q3)*cos(q2)-wC*z*(sin(q1)*cos(
q3)-sin(q2)*sin(q3)*cos(q1)))/L2)*Ny> + (T2*(dcm*cos(q1)*cos(q2)*(wN+y)
-dcm*x*sin(q1)*cos(q2)-wC*x*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))-
wC*(wN+y)*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))/L2+T3*(wC*x*(cos(
q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))-dcm*x*sin(q1)*cos(q2)-dcm*cos(q1)
*cos(q2)*(wN-y)-wC*(wN-y)*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))/L3
-(dC-dcm)*T1*cos(q2)*(y*cos(q1)+sin(q1)*(dN-x))/L1)*Nz>

(134) Mx = Dot( ResultantMoment>, Cx> )
-> (135) Mx = wC*T2*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)-sin(q3)*cos(q1)*(wN+y))
/L2 + wC*sin(q2)*cos(q3)*(T2*(x*cos(q1)+sin(q1)*(wN+y))/L2-T3*(x*cos(
q1)-sin(q1)*(wN-y))/L3) - wC*T3*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+
sin(q3)*cos(q1)*(wN-y))/L3 - bT*wx*wMag^expDamping/(epsilonW+wMag)

(136) My = Dot( ResultantMoment>, Cy> )
-> (137) My = (sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*((dC-dcm)*T1*(y*sin(q2)
+z*sin(q1)*cos(q2))/L1-dcm*T2*(sin(q2)*(wN+y)+z*sin(q1)*cos(q2))/L2-
dcm*T3*(z*sin(q1)*cos(q2)-sin(q2)*(wN-y))/L3) - sin(q3)*cos(q2)^2*(dcm
*T2*(x*sin(q1)-cos(q1)*(wN+y))/L2+dcm*T3*(x*sin(q1)+cos(q1)*(wN-y))/L3
+(dC-dcm)*T1*(y*cos(q1)+sin(q1)*(dN-x))/L1) - (cos(q1)*cos(q3)+sin(q1)
*sin(q2)*sin(q3))*(dcm*T2*(x*sin(q2)+z*cos(q1)*cos(q2))/L2+dcm*T3*(x*
sin(q2)+z*cos(q1)*cos(q2))/L3-(dC-dcm)*T1*(z*cos(q1)*cos(q2)-sin(q2)*(
dN-x))/L1) - bT*wy*wMag^expDamping/(epsilonW+wMag)

(138) Mz = Dot( ResultantMoment>, Cz> )
-> (139) Mz = -(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*((dC-dcm)*T1*(z*cos(
q1)*cos(q2)-sin(q2)*(dN-x))/L1-T3*(dcm*x*sin(q2)+dcm*z*cos(q1)*cos(q2)
+wC*x*sin(q3)*cos(q2)+wC*z*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))/L3
-T2*(dcm*x*sin(q2)+dcm*z*cos(q1)*cos(q2)-wC*x*sin(q3)*cos(q2)-wC*z*(
sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))/L2) - (sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))*((dC-dcm)*T1*(y*sin(q2)+z*sin(q1)*cos(q2))/L1+T2*
(wC*sin(q3)*cos(q2)*(wN+y)-dcm*sin(q2)*(wN+y)-dcm*z*sin(q1)*cos(q2)-
wC*z*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))/L2-T3*(dcm*z*sin(q1)*
cos(q2)-dcm*sin(q2)*(wN-y)-wC*sin(q3)*cos(q2)*(wN-y)-wC*z*(cos(q1)*cos
(q3)+sin(q1)*sin(q2)*sin(q3)))/L3) - cos(q2)*cos(q3)*((dC-dcm)*T1*cos(
q2)*(y*cos(q1)+sin(q1)*(dN-x))/L1-T2*(dcm*cos(q1)*cos(q2)*(wN+y)-dcm*x
*sin(q1)*cos(q2)-wC*x*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))-wC*(
wN+y)*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))/L2-T3*(wC*x*(cos(q1)*
cos(q3)+sin(q1)*sin(q2)*sin(q3))-dcm*x*sin(q1)*cos(q2)-dcm*cos(q1)*cos
(q2)*(wN-y)-wC*(wN-y)*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))/L3)
- bT*wz*wMag^expDamping/(epsilonW+wMag)

(140) ResultantMoment> := Mx*Cx> + My*Cy> + Mz*Cz>
-> (141) ResultantMoment> = Mx*Cx> + My*Cy> + Mz*Cz>

(142) %--------------------------------------------------------------------
(143) %   F = m*a   (Translational equations of motion).
(144) ma> = m * Ccm.GetAcceleration( N )
-> (145) ma> = m*x''*Nx> + m*y''*Ny> + m*z''*Nz>

(146) Dynamics[1] = Dot(  Nx>,  ma> - ResultantForce>  )
-> (147) Dynamics[1] = m*x'' - Fx

(148) Dynamics[2] = Dot(  Ny>,  ma> - ResultantForce>  )
-> (149) Dynamics[2] = m*y'' - Fy

(150) Dynamics[3] = Dot(  Nz>,  ma> - ResultantForce>  )
-> (151) Dynamics[3] = m*z'' - Fz

(152) %--------------------------------------------------------------------
(153) %   M = DH/Dt (Rotational equations of motion).
(154) dHDt> = Dt( C.GetAngularMomentum(Ccm), N )
-> (155) dHDt> = (Izz*wy*wz+Ixx*wx'-Iyy*wy*wz)*Cx> + (Ixx*wx*wz+Iyy*wy'-Izz*wx*
wz)*Cy> + (Iyy*wx*wy+Izz*wz'-Ixx*wx*wy)*Cz>

(156) Dynamics[4] = Dot(  Cx>,  dHDt> - ResultantMoment>  )
-> (157) Dynamics[4] = Izz*wy*wz + Ixx*wx' - Iyy*wy*wz - Mx

(158) Dynamics[5] = Dot(  Cy>,  dHDt> - ResultantMoment>  )
-> (159) Dynamics[5] = Ixx*wx*wz + Iyy*wy' - Izz*wx*wz - My

(160) Dynamics[6] = Dot(  Cz>,  dHDt> - ResultantMoment>  )
-> (161) Dynamics[6] = Iyy*wx*wy + Izz*wz' - Ixx*wx*wy - Mz

(162) %--------------------------------------------------------------------
(163) %   Append time-derivatives of constraints with constraint stabilization.
(164) %   For each yi = CableConstraint[i], use critical damping zetaStabilize=1 and natural frequency wStabilize=1 for:
(165) %   yi''  +  2 * zeta * wn * yi'  +  wn^2 * yi  =  0.
(166) Constant  zetaConstraintStabilize = 1 noUnits,   wnConstraintStabilize = 1 rad/sec
(167) kd =  2 * zetaConstraintStabilize * wn;    kp = wnConstraintStabilize^2;
-> (168) kd = 2*wN*zetaConstraintStabilize
-> (169) kp = wnConstraintStabilize^2

(170) Dynamics[7] = DtDt( CableConstraint[1] )  +  kd * Dt( CableConstraint[1] )  +  kp * CableConstraint[1]
-> (171) Dynamics[7] = kp*((dC-dcm)^2+y^2+z^2+(dN-x)^2+2*(dC-dcm)*y*sin(q1)*cos
(q2)-L1^2-2*(dC-dcm)*z*sin(q2)-2*(dC-dcm)*cos(q1)*cos(q2)*(dN-x))
+ 2*x'^2 + 2*y'^2 + 2*z'^2 + 4*(dC-dcm)*cos(q1)*cos(q2)*q1'*y' + 2*(
dC-dcm)*z*(sin(q2)*q2'^2-cos(q2)*(cos(q3)*wy'-sin(q3)*wz'-wy*sin(q3)*
q3'-wz*cos(q3)*q3')) + 2*(dC-dcm)*cos(q1)*(dN-x)*(cos(q2)*q2'^2+sin(
q2)*(cos(q3)*wy'-sin(q3)*wz'-wy*sin(q3)*q3'-wz*cos(q3)*q3')) + 2*(dC-
dcm)*cos(q2)*(dN-x)*(cos(q1)*q1'^2+sin(q1)*(sin(q2)*(wy*sin(q3)+wz*cos
(q3))*q2'+cos(q2)*(sin(q3)*wy'+cos(q3)*wz'+wy*cos(q3)*q3'-wz*sin(q3)*
q3'))/cos(q2)^2) + 2*y*y'' + 2*z*z'' + 2*(dC-dcm)*sin(q1)*cos(q2)*y''
+ 2*(dC-dcm)*cos(q1)*cos(q2)*x'' - 4*(dC-dcm)*cos(q2)*q2'*z' - 4*(dC-
dcm)*sin(q1)*sin(q2)*q2'*y' - 4*(dC-dcm)*sin(q1)*cos(q2)*q1'*x' - 4*(
dC-dcm)*sin(q2)*cos(q1)*q2'*x' - 4*(dC-dcm)*y*sin(q2)*cos(q1)*q1'*q2'
- 4*(dC-dcm)*sin(q1)*sin(q2)*(dN-x)*q1'*q2' - 2*(dC-dcm)*y*sin(q1)*(
cos(q2)*q2'^2+sin(q2)*(cos(q3)*wy'-sin(q3)*wz'-wy*sin(q3)*q3'-wz*cos(
q3)*q3')) - 2*(dC-dcm)*y*cos(q2)*(sin(q1)*q1'^2-cos(q1)*(sin(q2)*(wy*
sin(q3)+wz*cos(q3))*q2'+cos(q2)*(sin(q3)*wy'+cos(q3)*wz'+wy*cos(q3)*
q3'-wz*sin(q3)*q3'))/cos(q2)^2) - 2*kd*((dN-x)*x'+(dC-dcm)*sin(q2)*z'+
(dC-dcm)*z*cos(q2)*q2'+(dC-dcm)*y*sin(q1)*sin(q2)*q2'-y*y'-z*z'-(dC-
dcm)*sin(q1)*cos(q2)*y'-(dC-dcm)*cos(q1)*cos(q2)*x'-(dC-dcm)*y*cos(q1)
*cos(q2)*q1'-(dC-dcm)*sin(q1)*cos(q2)*(dN-x)*q1'-(dC-dcm)*sin(q2)*cos(
q1)*(dN-x)*q2') - 2*(dN-x)*x'' - 2*(dC-dcm)*sin(q2)*z''

(172) Dynamics[8] = DtDt( CableConstraint[2] )  +  kd * Dt( CableConstraint[2] )  +  kp * CableConstraint[2]
-> (173) Dynamics[8] = kp*(dcm^2+wC^2+x^2+z^2+(wN+y)^2+2*dcm*z*sin(q2)+2*wC*x*(
sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))-L2^2-2*dcm*x*cos(q1)*cos(q2)-
2*wC*z*sin(q3)*cos(q2)-2*dcm*sin(q1)*cos(q2)*(wN+y)-2*wC*(wN+y)*(cos(
q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))) + 2*x'^2 + 2*y'^2 + 2*z'^2
+ 4*dcm*cos(q2)*q2'*z' + 4*dcm*sin(q1)*sin(q2)*q2'*y' + 4*dcm*sin(q1)*
cos(q2)*q1'*x' + 4*dcm*sin(q2)*cos(q1)*q2'*x' + 4*wC*sin(q2)*sin(q3)*
q2'*z' + 4*wC*z*sin(q2)*cos(q3)*q2'*q3' + 4*dcm*sin(q2)*cos(q1)*(wN+y)
*q1'*q2' + 2*dcm*x*cos(q1)*(cos(q2)*q2'^2+sin(q2)*(cos(q3)*wy'-sin(q3)
*wz'-wy*sin(q3)*q3'-wz*cos(q3)*q3')) + 2*wC*z*sin(q3)*(cos(q2)*q2'^2+
sin(q2)*(cos(q3)*wy'-sin(q3)*wz'-wy*sin(q3)*q3'-wz*cos(q3)*q3'))
+ 2*dcm*sin(q1)*(wN+y)*(cos(q2)*q2'^2+sin(q2)*(cos(q3)*wy'-sin(q3)*wz'
-wy*sin(q3)*q3'-wz*cos(q3)*q3')) + 4*wC*x'*(cos(q1)*cos(q3)*q1'+sin(
q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*
q3'-sin(q3)*cos(q1)*cos(q2)*q2') + 4*wC*y'*(sin(q1)*cos(q3)*q1'+sin(
q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*
q2'-sin(q2)*sin(q3)*cos(q1)*q1') + 2*wC*z*cos(q2)*(sin(q3)*q3'^2-cos(
q3)*(wx'+(wy*sin(q3)+wz*cos(q3))*q2'/cos(q2)^2+tan(q2)*(sin(q3)*wy'+
cos(q3)*wz'+wy*cos(q3)*q3'-wz*sin(q3)*q3'))) + 2*dcm*x*cos(q2)*(cos(
q1)*q1'^2+sin(q1)*(sin(q2)*(wy*sin(q3)+wz*cos(q3))*q2'+cos(q2)*(sin(
q3)*wy'+cos(q3)*wz'+wy*cos(q3)*q3'-wz*sin(q3)*q3'))/cos(q2)^2) + 2*dcm
*cos(q2)*(wN+y)*(sin(q1)*q1'^2-cos(q1)*(sin(q2)*(wy*sin(q3)+wz*cos(q3))
*q2'+cos(q2)*(sin(q3)*wy'+cos(q3)*wz'+wy*cos(q3)*q3'-wz*sin(q3)*q3'))/
cos(q2)^2) + 2*x*x'' + 2*z*z'' + 2*(wN+y)*y'' + 2*dcm*sin(q2)*z''
+ 2*wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x'' - 4*dcm*cos(q1)*
cos(q2)*q1'*y' - 4*wC*cos(q2)*cos(q3)*q3'*z' - 4*dcm*x*sin(q1)*sin(q2)
*q1'*q2' - 2*dcm*z*(sin(q2)*q2'^2-cos(q2)*(cos(q3)*wy'-sin(q3)*wz'-wy*
sin(q3)*q3'-wz*cos(q3)*q3')) - 2*kd*(dcm*sin(q1)*cos(q2)*y'+dcm*cos(
q1)*cos(q2)*x'+wC*sin(q3)*cos(q2)*z'+wC*z*cos(q2)*cos(q3)*q3'+dcm*cos(
q1)*cos(q2)*(wN+y)*q1'+wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))*
y'-x*x'-z*z'-(wN+y)*y'-dcm*sin(q2)*z'-dcm*z*cos(q2)*q2'-dcm*x*sin(q1)*
cos(q2)*q1'-dcm*x*sin(q2)*cos(q1)*q2'-wC*z*sin(q2)*sin(q3)*q2'-dcm*sin
(q1)*sin(q2)*(wN+y)*q2'-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*
x'-wC*x*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(
q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*q2')-wC*(
wN+y)*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)
*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')) - 2*wC*
x*(2*sin(q3)*cos(q1)*q1'*q3'+2*cos(q1)*cos(q2)*cos(q3)*q2'*q3'+sin(q1)
*(cos(q3)*q3'^2+sin(q3)*(wx'+(wy*sin(q3)+wz*cos(q3))*q2'/cos(q2)^2+tan
(q2)*(sin(q3)*wy'+cos(q3)*wz'+wy*cos(q3)*q3'-wz*sin(q3)*q3')))+cos(q3)
*(sin(q1)*q1'^2-cos(q1)*(sin(q2)*(wy*sin(q3)+wz*cos(q3))*q2'+cos(q2)*(
sin(q3)*wy'+cos(q3)*wz'+wy*cos(q3)*q3'-wz*sin(q3)*q3'))/cos(q2)^2)-2*
sin(q1)*sin(q2)*cos(q3)*q1'*q3'-2*sin(q1)*sin(q3)*cos(q2)*q1'*q2'-sin(
q3)*cos(q1)*(sin(q2)*q2'^2-cos(q2)*(cos(q3)*wy'-sin(q3)*wz'-wy*sin(q3)
*q3'-wz*cos(q3)*q3'))-sin(q2)*cos(q1)*(sin(q3)*q3'^2-cos(q3)*(wx'+(wy*
sin(q3)+wz*cos(q3))*q2'/cos(q2)^2+tan(q2)*(sin(q3)*wy'+cos(q3)*wz'+wy*
cos(q3)*q3'-wz*sin(q3)*q3')))-sin(q2)*sin(q3)*(cos(q1)*q1'^2+sin(q1)*(
sin(q2)*(wy*sin(q3)+wz*cos(q3))*q2'+cos(q2)*(sin(q3)*wy'+cos(q3)*wz'+
wy*cos(q3)*q3'-wz*sin(q3)*q3'))/cos(q2)^2)) - 2*wC*(wN+y)*(2*sin(q1)*
sin(q3)*q1'*q3'+2*sin(q1)*cos(q2)*cos(q3)*q2'*q3'+2*sin(q2)*cos(q1)*
cos(q3)*q1'*q3'+2*sin(q3)*cos(q1)*cos(q2)*q1'*q2'-sin(q1)*sin(q3)*(sin
(q2)*q2'^2-cos(q2)*(cos(q3)*wy'-sin(q3)*wz'-wy*sin(q3)*q3'-wz*cos(q3)*
q3'))-cos(q1)*(cos(q3)*q3'^2+sin(q3)*(wx'+(wy*sin(q3)+wz*cos(q3))*q2'/
cos(q2)^2+tan(q2)*(sin(q3)*wy'+cos(q3)*wz'+wy*cos(q3)*q3'-wz*sin(q3)*
q3')))-cos(q3)*(cos(q1)*q1'^2+sin(q1)*(sin(q2)*(wy*sin(q3)+wz*cos(q3))
*q2'+cos(q2)*(sin(q3)*wy'+cos(q3)*wz'+wy*cos(q3)*q3'-wz*sin(q3)*q3'))/
cos(q2)^2)-sin(q1)*sin(q2)*(sin(q3)*q3'^2-cos(q3)*(wx'+(wy*sin(q3)+wz*
cos(q3))*q2'/cos(q2)^2+tan(q2)*(sin(q3)*wy'+cos(q3)*wz'+wy*cos(q3)*q3'
-wz*sin(q3)*q3')))-sin(q2)*sin(q3)*(sin(q1)*q1'^2-cos(q1)*(sin(q2)*(
wy*sin(q3)+wz*cos(q3))*q2'+cos(q2)*(sin(q3)*wy'+cos(q3)*wz'+wy*cos(q3)
*q3'-wz*sin(q3)*q3'))/cos(q2)^2)) - 2*dcm*sin(q1)*cos(q2)*y'' - 2*dcm*
cos(q1)*cos(q2)*x'' - 2*wC*sin(q3)*cos(q2)*z'' - 2*wC*(cos(q1)*cos(q3)
+sin(q1)*sin(q2)*sin(q3))*y''

(174) Dynamics[9] = DtDt( CableConstraint[3] )  +  kd * Dt( CableConstraint[3] )  +  kp * CableConstraint[3]
-> (175) Dynamics[9] = kp*(dcm^2+wC^2+x^2+z^2+(wN-y)^2+2*dcm*z*sin(q2)+2*wC*z*
sin(q3)*cos(q2)+2*dcm*sin(q1)*cos(q2)*(wN-y)-L3^2-2*dcm*x*cos(q1)*cos(
q2)-2*wC*x*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))-2*wC*(wN-y)*(cos(
q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))) + 2*x'^2 + 2*y'^2 + 2*z'^2
+ 4*dcm*cos(q2)*q2'*z' + 4*dcm*sin(q1)*sin(q2)*q2'*y' + 4*dcm*sin(q1)*
cos(q2)*q1'*x' + 4*dcm*sin(q2)*cos(q1)*q2'*x' + 4*wC*cos(q2)*cos(q3)*
q3'*z' + 2*dcm*x*cos(q1)*(cos(q2)*q2'^2+sin(q2)*(cos(q3)*wy'-sin(q3)*
wz'-wy*sin(q3)*q3'-wz*cos(q3)*q3')) + 2*dcm*x*cos(q2)*(cos(q1)*q1'^2+
sin(q1)*(sin(q2)*(wy*sin(q3)+wz*cos(q3))*q2'+cos(q2)*(sin(q3)*wy'+cos(
q3)*wz'+wy*cos(q3)*q3'-wz*sin(q3)*q3'))/cos(q2)^2) + 2*wC*x*(2*sin(q3)
*cos(q1)*q1'*q3'+2*cos(q1)*cos(q2)*cos(q3)*q2'*q3'+sin(q1)*(cos(q3)*q3'^2
+sin(q3)*(wx'+(wy*sin(q3)+wz*cos(q3))*q2'/cos(q2)^2+tan(q2)*(sin(q3)*
wy'+cos(q3)*wz'+wy*cos(q3)*q3'-wz*sin(q3)*q3')))+cos(q3)*(sin(q1)*q1'^2
-cos(q1)*(sin(q2)*(wy*sin(q3)+wz*cos(q3))*q2'+cos(q2)*(sin(q3)*wy'+cos
(q3)*wz'+wy*cos(q3)*q3'-wz*sin(q3)*q3'))/cos(q2)^2)-2*sin(q1)*sin(q2)*
cos(q3)*q1'*q3'-2*sin(q1)*sin(q3)*cos(q2)*q1'*q2'-sin(q3)*cos(q1)*(sin
(q2)*q2'^2-cos(q2)*(cos(q3)*wy'-sin(q3)*wz'-wy*sin(q3)*q3'-wz*cos(q3)*
q3'))-sin(q2)*cos(q1)*(sin(q3)*q3'^2-cos(q3)*(wx'+(wy*sin(q3)+wz*cos(
q3))*q2'/cos(q2)^2+tan(q2)*(sin(q3)*wy'+cos(q3)*wz'+wy*cos(q3)*q3'-wz*
sin(q3)*q3')))-sin(q2)*sin(q3)*(cos(q1)*q1'^2+sin(q1)*(sin(q2)*(wy*sin
(q3)+wz*cos(q3))*q2'+cos(q2)*(sin(q3)*wy'+cos(q3)*wz'+wy*cos(q3)*q3'-
wz*sin(q3)*q3'))/cos(q2)^2)) + 2*x*x'' + 2*z*z'' + 2*dcm*sin(q2)*z''
+ 2*wC*sin(q3)*cos(q2)*z'' + 2*wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin
(q3))*y'' - 4*dcm*cos(q1)*cos(q2)*q1'*y' - 4*wC*sin(q2)*sin(q3)*q2'*z'
- 4*dcm*x*sin(q1)*sin(q2)*q1'*q2' - 4*wC*z*sin(q2)*cos(q3)*q2'*q3'
- 4*dcm*sin(q2)*cos(q1)*(wN-y)*q1'*q2' - 2*dcm*z*(sin(q2)*q2'^2-cos(
q2)*(cos(q3)*wy'-sin(q3)*wz'-wy*sin(q3)*q3'-wz*cos(q3)*q3')) - 2*wC*z*
sin(q3)*(cos(q2)*q2'^2+sin(q2)*(cos(q3)*wy'-sin(q3)*wz'-wy*sin(q3)*q3'
-wz*cos(q3)*q3')) - 2*dcm*sin(q1)*(wN-y)*(cos(q2)*q2'^2+sin(q2)*(cos(
q3)*wy'-sin(q3)*wz'-wy*sin(q3)*q3'-wz*cos(q3)*q3')) - 4*wC*x'*(cos(q1)
*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*
cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*q2') - 4*wC*y'*(sin(q1)*
cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*
sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1') - 2*wC*z*cos(q2)*(sin
(q3)*q3'^2-cos(q3)*(wx'+(wy*sin(q3)+wz*cos(q3))*q2'/cos(q2)^2+tan(q2)*
(sin(q3)*wy'+cos(q3)*wz'+wy*cos(q3)*q3'-wz*sin(q3)*q3'))) - 2*dcm*cos(
q2)*(wN-y)*(sin(q1)*q1'^2-cos(q1)*(sin(q2)*(wy*sin(q3)+wz*cos(q3))*q2'
+cos(q2)*(sin(q3)*wy'+cos(q3)*wz'+wy*cos(q3)*q3'-wz*sin(q3)*q3'))/cos(
q2)^2) - 2*kd*((wN-y)*y'+dcm*sin(q1)*cos(q2)*y'+dcm*cos(q1)*cos(q2)*
x'+wC*z*sin(q2)*sin(q3)*q2'+dcm*sin(q1)*sin(q2)*(wN-y)*q2'+wC*(sin(q1)
*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x'+wC*x*(cos(q1)*cos(q3)*q1'+sin(q1)
*sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-
sin(q3)*cos(q1)*cos(q2)*q2')-x*x'-z*z'-dcm*sin(q2)*z'-dcm*z*cos(q2)*
q2'-wC*sin(q3)*cos(q2)*z'-dcm*x*sin(q1)*cos(q2)*q1'-dcm*x*sin(q2)*cos(
q1)*q2'-wC*z*cos(q2)*cos(q3)*q3'-dcm*cos(q1)*cos(q2)*(wN-y)*q1'-wC*(
cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))*y'-wC*(wN-y)*(sin(q1)*cos(q3)
*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*
cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')) - 2*wC*(wN-y)*(2*sin(q1)*sin
(q3)*q1'*q3'+2*sin(q1)*cos(q2)*cos(q3)*q2'*q3'+2*sin(q2)*cos(q1)*cos(
q3)*q1'*q3'+2*sin(q3)*cos(q1)*cos(q2)*q1'*q2'-sin(q1)*sin(q3)*(sin(q2)
*q2'^2-cos(q2)*(cos(q3)*wy'-sin(q3)*wz'-wy*sin(q3)*q3'-wz*cos(q3)*q3'))
-cos(q1)*(cos(q3)*q3'^2+sin(q3)*(wx'+(wy*sin(q3)+wz*cos(q3))*q2'/cos(
q2)^2+tan(q2)*(sin(q3)*wy'+cos(q3)*wz'+wy*cos(q3)*q3'-wz*sin(q3)*q3')))
-cos(q3)*(cos(q1)*q1'^2+sin(q1)*(sin(q2)*(wy*sin(q3)+wz*cos(q3))*q2'+
cos(q2)*(sin(q3)*wy'+cos(q3)*wz'+wy*cos(q3)*q3'-wz*sin(q3)*q3'))/cos(
q2)^2)-sin(q1)*sin(q2)*(sin(q3)*q3'^2-cos(q3)*(wx'+(wy*sin(q3)+wz*cos(
q3))*q2'/cos(q2)^2+tan(q2)*(sin(q3)*wy'+cos(q3)*wz'+wy*cos(q3)*q3'-wz*
sin(q3)*q3')))-sin(q2)*sin(q3)*(sin(q1)*q1'^2-cos(q1)*(sin(q2)*(wy*sin
(q3)+wz*cos(q3))*q2'+cos(q2)*(sin(q3)*wy'+cos(q3)*wz'+wy*cos(q3)*q3'-
wz*sin(q3)*q3'))/cos(q2)^2)) - 2*(wN-y)*y'' - 2*dcm*sin(q1)*cos(q2)*y''
- 2*dcm*cos(q1)*cos(q2)*x'' - 2*wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*
cos(q1))*x''

(176) %--------------------------------------------------------------------
(177) %   Energy calculations.
(178) PowerDamping = Dot( dampingTorque>, C.GetAngularVelocity(N) ) + Dot( dampingForce>, CCm.GetVelocity(N) )
-> (179) PowerDamping = -bT*(wx^2*wMag^expDamping+wy^2*wMag^expDamping+wz^2*wMag
^expDamping)/(epsilonW+wMag) - bF*(x'^2*vMag^expDamping+y'^2*vMag^expDamping
+z'^2*vMag^expDamping)/(epsilonV+vMag)

(180) PowerTension = Dot( C1.GetResultantForce(N1), C1.GetVelocity(N) )   &
+ Dot( C2.GetResultantForce(N2), C2.GetVelocity(N) )   &
+ Dot( C3.GetResultantForce(N3), C3.GetVelocity(N) )
-> (181) PowerTension = T1*((dC-dcm)*wy*z*cos(q2)*cos(q3)+(dN-x)*x'+(dC-dcm)*
sin(q2)*z'-(dC-dcm)*wz*z*sin(q3)*cos(q2)-(dC-dcm)*wz*y*(cos(q1)*cos(
q3)+sin(q1)*sin(q2)*sin(q3))-(dC-dcm)*wy*y*(sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3))-(dC-dcm)*wy*(dN-x)*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
cos(q3))-(dC-dcm)*wz*(dN-x)*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))-
y*y'-z*z'-(dC-dcm)*sin(q1)*cos(q2)*y'-(dC-dcm)*cos(q1)*cos(q2)*x')/L1
+ T2*(wC*wz*z*sin(q2)+dcm*wz*z*sin(q3)*cos(q2)+dcm*wz*(wN+y)*(cos(q1)*
cos(q3)+sin(q1)*sin(q2)*sin(q3))+(wN+y)*(dcm*wy-wC*wx)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3))+dcm*sin(q1)*cos(q2)*y'+dcm*cos(q1)*cos(
q2)*x'+wC*sin(q3)*cos(q2)*z'+wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(
q3))*y'-wC*wz*x*cos(q1)*cos(q2)-wC*wz*sin(q1)*cos(q2)*(wN+y)-z*cos(q2)
*cos(q3)*(dcm*wy-wC*wx)-dcm*wz*x*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(
q1))-x*(dcm*wy-wC*wx)*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-x*x'-z
*z'-(wN+y)*y'-dcm*sin(q2)*z'-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(
q1))*x')/L2 - T3*(wC*wz*z*sin(q2)+wC*wz*sin(q1)*cos(q2)*(wN-y)+z*cos(
q2)*cos(q3)*(dcm*wy+wC*wx)+dcm*wz*x*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*
cos(q1))+dcm*wz*(wN-y)*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))+x*(
dcm*wy+wC*wx)*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(dcm*
wy+wC*wx)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))+x*x'+z*z'+dcm*sin(
q2)*z'+wC*sin(q3)*cos(q2)*z'+wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(
q3))*y'-dcm*wz*z*sin(q3)*cos(q2)-wC*wz*x*cos(q1)*cos(q2)-(wN-y)*y'-dcm
*sin(q1)*cos(q2)*y'-dcm*cos(q1)*cos(q2)*x'-wC*(sin(q1)*cos(q3)-sin(q2)
*sin(q3)*cos(q1))*x')/L3

(182) Variable  WorkDamping' = PowerDamping + PowerTension
-> (183) WorkDamping' = PowerDamping + PowerTension

(184) KineticEnergy = System.GetKineticEnergy()
-> (185) KineticEnergy = 0.5*Ixx*wx^2 + 0.5*Iyy*wy^2 + 0.5*Izz*wz^2 + 0.5*m*(x'^2
+y'^2+z'^2)

(186) GravityPotentialEnergy = System.GetForceGravityPotentialEnergy( g*Nz>, No )
-> (187) GravityPotentialEnergy = -m*g*z

(188) KePe = KineticEnergy + GravityPotentialEnergy
-> (189) KePe = GravityPotentialEnergy + KineticEnergy

(190) MechanicalEnergy =  KePe - WorkDamping
-> (191) MechanicalEnergy = KePe - WorkDamping

(192) Input  WorkDamping = 0 Joules
(193) %--------------------------------------------------------------------
(194) %   Integration parameters and initial values for variables.
(195) Input  tFinal = 120 sec,  tStep = 0.05 sec,  absError = 1.0E-07
(196) Input  x' = 0 m/s,  y' = 0 m/s,  z' = 0 m/s
(198) %--------------------------------------------------------------------
(199) %   Calculate yaw, pitch and roll angles in terms of q1, q2, q3.
(200) Yaw   = GetAngleBetweenUnitVectors( Nx>, Cy> )  -  pi/2
-> (201) Yaw = -1.570796 + acos(sin(q2)*sin(q3)*cos(q1)-sin(q1)*cos(q3))

(202) Pitch = GetAngleBetweenUnitVectors( Nz>, Cx> )  -  pi/2
-> (203) Pitch = 1.570796 - acos(sin(q2))

(204) Roll  = pi/2  -  GetAngleBetweenUnitVectors( Nz>, Cy> )
-> (205) Roll = 1.570796 - acos(sin(q3)*cos(q2))

(206) %--------------------------------------------------------------------
(207) %   Solve for initial values of q1, q2, q3 via initial yaw, pitch, roll angles.
(208) Constant  Yaw0 = 60 deg,  Pitch0 = 0 deg,  Roll0 = 0 deg
(209) SolveSetInput( [Yaw; Pitch; Roll] = [Yaw0; Pitch0; Roll0],  q1 = 60 deg, q2 = 0 deg, q3 = 0 deg )

->    %  INPUT has been assigned as follows:
->    %   q1                        60                      deg
->    %   q2                        0                       deg
->    %   q3                        0                       deg

(210) %--------------------------------------------------------------------
(211) %   Solve for initial values of x, y, z using the CableConstraint (and initial values of q1, q2, q3).
(212) SolveSetInput( CableConstraint = 0,  x = Input(dN)/2 m,  y=0 m,  z = 0.8*Input(L1) m )

->    %  INPUT has been assigned as follows:
->    %   x                         8.166666666666668       m
->    %   y                        -0.2886751345948148      m
->    %   z                         20.74983266331456       m

(213) %--------------------------------------------------------------------
(214) %   List output quantities and solve ODEs.
(215) Distance = Co.GetDistance( No )
-> (216) Distance = sqrt(dcm^2+x^2+y^2+z^2+2*dcm*z*sin(q2)-2*dcm*x*cos(q1)*cos(
q2)-2*dcm*y*sin(q1)*cos(q2))

(217) OutputPlot  t sec,  Distance m,  Yaw deg,  Pitch deg,  Roll deg,  T1 kN,  T2 kN,  T3 kN,  &
vMag m/s,  wMag rad/sec, MechanicalEnergy Joules,                          &
CableConstraint[1] m, CableConstraint[2] m, CableConstraint[3] m
(218) Output   t sec,  x m,  y m,  z m,  x' m/s,  y' m/s,  z' m/s,               &