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.
(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-07
(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) % 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, &
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
(219) ODE( Dynamics, x'', y'', z'', wx', wy', wz', T1, T2, T3 ) MGAircraftTrifilarPendulumDynamicsFBD.m
(220) %--------------------------------------------------------------------
Saved by Motion Genesis LLC. Command names and syntax: Copyright (c) 2009-2021 Motion Genesis LLC. All rights reserved.