MGAircraftTrifilarPendulumDynamicsFBD.html  (MotionGenesis input/output).
   (1) % MotionGenesis file:  MGAircraftTrifilarPendulumDynamicsFBD.txt
   (2) % Copyright (c) 1988-2015 Motion Genesis LLC.  For use only with MotionGenesis.
   (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.
   (84) Ccm.AddForce( m*g*Nz> )
-> (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>

   (106) CCm.AddForce( dampingForce> )
-> (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>

   (116) C.AddTorque( dampingTorque> )
-> (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-09  
   (196) Input  x' = 0 m/s,  y' = 0 m/s,  z' = 0 m/s
   (197) Input  wx = 0 rad/sec,  wy = 0 rad/sec,  wz = 0 rad/sec
   (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) %   Use configuration constraints and initial yaw, pitch, roll angles to solve for initial values.
   (208) Constant  Yaw0 = 60 deg,  Pitch0 = 0 deg,  Roll0 = 0 deg
   (209) SolveSetInput( [ CableConstraint;  Yaw - Yaw0;  Pitch - Pitch0;  Roll - Roll0 ],  &
               x = Input(dN)/2 m, y=0 m, z = 0.8*Input(L1) m,  q1=60 deg, q2=0 deg, q3=0 deg )

->    %  INPUT has been assigned as follows:
->    %   x                         8.16666666666667        m
->    %   y                        -0.2886751345948158      m
->    %   z                         20.74983266331457       m
->    %   q1                        60                      deg
->    %   q2                        0                       deg
->    %   q3                        0                       deg

   (210) %--------------------------------------------------------------------
   (211) %   List output quantities and solve ODEs.
   (212) Distance = Co.GetDistance( No )
-> (213) 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))

   (214) 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
   (215) Output   t sec,  x m,  y m,  z m,  x' m/s,  y' m/s,  z' m/s,               &
         q1 deg,  q2 deg,  q3 deg,  wx rad/sec,  wy rad/sec,  wz rad/sec,  &
         KineticEnergy Joules, GravityPotentialEnergy Joules, KePe Joules, MechanicalEnergy Joules, WorkDamping Joules
   (216) ODE( Dynamics, x'', y'', z'', wx', wy', wz', T1, T2, T3 )  MGAircraftTrifilarPendulumDynamicsFBD.m

   (217) %--------------------------------------------------------------------
Saved by Motion Genesis LLC.   Portions copyright (c) 2009-2017 Motion Genesis LLC. Rights reserved. Only for use with MotionGenesis.