MGAircraftTrifilarPendulumDynamicsKaneAugmented.html (MotionGenesis input/output).
(1) % MotionGenesis file: MGAircraftTrifilarPendulumDynamicsKaneAugmented.txt
(2) % Copyright (c) 2016-18 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) C.SetMassInertia( m = 9000 kg, Ixx = 4.0E5 kg*m^2, Iyy = 3.0E5 kg*m^2, Izz = Ixx + Iyy )
-> (20) Izz = Ixx + Iyy
(21) %--------------------------------------------------------------------
(22) Constant bT = 8.0E3 noUnits % Torque damping constant.
(23) Constant bF = 6.0E3 noUnits % Force damping constant.
(24) Constant epsilonV = 1.0E-5 m/s % Small number to avoid divide by zero errors.
(25) Constant epsilonW = 1.0E-5 rad/s % Small number to avoid divide by zero errors.
(26) Constant expDamping = 0.25 noUnits % Exponent used with damping force/torques.
(27) %--------------------------------------------------------------------
(28) Variable T1, T2, T3 % Tension in cables.
(29) Variable x'', y'', z'' % Locates Ccm from No.
(30) Variable q1', q2', q3' % BodyZYX Euler angles.
(31) Variable wx', wy', wz' % Cxyz> measures of C's angular velocity in N.
(32) SetGeneralizedSpeeds( x', y', z', wx, wy, wz )
(33) %--------------------------------------------------------------------
(34) % Rotational kinematics.
(35) C.SetAngularVelocityAcceleration( N, wx*Cx> + wy*Cy> + wz*Cz> )
-> (36) w_C_N> = wx*Cx> + wy*Cy> + wz*Cz>
-> (37) alf_C_N> = wx'*Cx> + wy'*Cy> + wz'*Cz>
(38) C.SetRotationMatrixODE( N, BodyZYX, q1, q2, q3 )
-> (39) C_N[1,1] = cos(q1)*cos(q2)
-> (40) C_N[1,2] = sin(q1)*cos(q2)
-> (41) C_N[1,3] = -sin(q2)
-> (42) C_N[2,1] = sin(q2)*sin(q3)*cos(q1) - sin(q1)*cos(q3)
-> (43) C_N[2,2] = cos(q1)*cos(q3) + sin(q1)*sin(q2)*sin(q3)
-> (44) C_N[2,3] = sin(q3)*cos(q2)
-> (45) C_N[3,1] = sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3)
-> (46) C_N[3,2] = sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1)
-> (47) C_N[3,3] = cos(q2)*cos(q3)
-> (48) q1' = (sin(q3)*wy+cos(q3)*wz)/cos(q2)
-> (49) q2' = cos(q3)*wy - sin(q3)*wz
-> (50) q3' = wx + tan(q2)*(sin(q3)*wy+cos(q3)*wz)
(51) %--------------------------------------------------------------------
(52) % Translational kinematics.
(53) CCm.Translate( No, x*Nx> + y*Ny> + z*Nz> )
-> (54) p_No_Ccm> = x*Nx> + y*Ny> + z*Nz>
-> (55) v_Ccm_N> = x'*Nx> + y'*Ny> + z'*Nz>
-> (56) a_Ccm_N> = x''*Nx> + y''*Ny> + z''*Nz>
(57) Co.SetPosition( CCm, -dcm*Cx> )
-> (58) p_Ccm_Co> = -dcm*Cx>
(59) C1.SetPosition( Co, dC*Cx> )
-> (60) p_Co_C1> = dC*Cx>
(61) C2.SetPosition( Co, -wC*Cy> )
-> (62) p_Co_C2> = -wC*Cy>
(63) C3.SetPosition( Co, wC*Cy> )
-> (64) p_Co_C3> = wC*Cy>
(65) N1.SetPosition( No, dN*Nx> )
-> (66) p_No_N1> = dN*Nx>
(67) N2.SetPosition( No, -wN*Ny> )
-> (68) p_No_N2> = -wN*Ny>
(69) N3.SetPosition( No, wN*Ny> )
-> (70) p_No_N3> = wN*Ny>
(71) %--------------------------------------------------------------------
(72) % Configuration constraints: Length of cables.
(73) CableConstraint[1] = C1.GetDistanceSquared( N1 ) - L1^2
-> (74) 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)
(75) CableConstraint[2] = C2.GetDistanceSquared( N2 ) - L2^2
-> (76) 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))
(77) CableConstraint[3] = C3.GetDistanceSquared( N3 ) - L3^2
-> (78) 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))
(79) %--------------------------------------------------------------------
(80) % Replace all forces on C (gravity and tension) with equivalent set,
(81) % consisting of the set's resultant at Ccm along with a torque equal
(82) % to the moment of the cable tensions about Ccm.
(83) %--------------------------------------------------------------------
(84) % Resultant force on C in terms of Fx, Fy, Fz.
(85) unitVectorToN1FromC1> = N1.GetPosition(C1) / L1
-> (86) unitVectorToN1FromC1> = -(dC-dcm)/L1*Cx> + (dN-x)/L1*Nx> - y/L1*Ny> - z/L1*Nz>
(87) unitVectorToN2FromC2> = N2.GetPosition(C2) / L2
-> (88) unitVectorToN2FromC2> = dcm/L2*Cx> + wC/L2*Cy> - x/L2*Nx> - (wN+y)/L2*Ny> - z/L2*Nz>
(89) unitVectorToN3FromC3> = N3.GetPosition(C3) / L3
-> (90) unitVectorToN3FromC3> = dcm/L3*Cx> - wC/L3*Cy> - x/L3*Nx> + (wN-y)/L3*Ny> - z/L3*Nz>
(91) Tension1> = T1 * unitVectorToN1FromC1>
-> (92) Tension1> = -(dC-dcm)*T1/L1*Cx> + T1*(dN-x)/L1*Nx> - T1*y/L1*Ny> - T1*z
/L1*Nz>
(93) Tension2> = T2 * unitVectorToN2FromC2>
-> (94) Tension2> = dcm*T2/L2*Cx> + wC*T2/L2*Cy> - T2*x/L2*Nx> - T2*(wN+y)/L2*Ny>
- T2*z/L2*Nz>
(95) Tension3> = T3 * unitVectorToN3FromC3>
-> (96) Tension3> = dcm*T3/L3*Cx> - wC*T3/L3*Cy> - T3*x/L3*Nx> + T3*(wN-y)/L3*Ny>
- T3*z/L3*Nz>
(97) ResultantForce> = Tension1> + Tension2> + Tension3> + m*g*Nz>
-> (98) 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)*Nx> + (T3*(wN-y)/L3-T1*y/L1
-T2*(wN+y)/L2)*Ny> + (m*g-T1*z/L1-T2*z/L2-T3*z/L3)*Nz>
(99) Fx = Dot( ResultantForce>, Nx> )
-> (100) 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))
(101) Fy = Dot( ResultantForce>, Ny> )
-> (102) 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
(103) Fz = Dot( ResultantForce>, Nz> )
-> (104) 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)
(105) Ccm.AddForce( Fx*Nx> + Fy*Ny> + Fz*Nz> )
-> (106) Force_Ccm> = Fx*Nx> + Fy*Ny> + Fz*Nz>
(107) %--------------------------------------------------------------------
(108) % Calculate moment of all forces on C about Ccm in terms of Mx, My, Mz.
(109) ResultantMoment> = Cross( C1.GetPosition(Ccm), Tension1> ) &
+ Cross( C2.GetPosition(Ccm), Tension2> ) &
+ Cross( C3.GetPosition(Ccm), Tension3> )
-> (110) ResultantMoment> = (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>
(111) Mx = Dot( ResultantMoment>, Cx> )
-> (112) Mx = -wC*(T3*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+sin(q3)*cos(q1)*(wN-
y))/L3-T2*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)-sin(q3)*cos(q1)*(wN+y))
/L2-sin(q2)*cos(q3)*(T2*(x*cos(q1)+sin(q1)*(wN+y))/L2-T3*(x*cos(q1)-
sin(q1)*(wN-y))/L3))
(113) My = Dot( ResultantMoment>, Cy> )
-> (114) 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)
(115) Mz = Dot( ResultantMoment>, Cz> )
-> (116) 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)
(117) C.AddTorque( Mx*Cx> + My*Cy> + Mz*Cz> )
-> (118) Torque_C> = Mx*Cx> + My*Cy> + Mz*Cz>
(119) %--------------------------------------------------------------------
(120) % Add force to damp translational motion.
(121) vMag = Ccm.GetSpeed( N )
-> (122) vMag = sqrt(x'^2+y'^2+z'^2)
(123) unitVectorVelocity> = CCm.GetVelocity( N ) / ( vMag + epsilonV )
-> (124) unitVectorVelocity> = x'/(epsilonV+vMag)*Nx> + y'/(epsilonV+vMag)*Ny>
+ z'/(epsilonV+vMag)*Nz>
(125) dampingForce> = -bF * vMag^expDamping * unitVectorVelocity>
-> (126) dampingForce> = -bF*x'*vMag^expDamping/(epsilonV+vMag)*Nx> - bF*y'*vMag
^expDamping/(epsilonV+vMag)*Ny> - bF*z'*vMag^expDamping/(epsilonV+vMag)*Nz>
(127) CCm.AddForce( dampingForce> )
-> (128) Force_Ccm> = (Fx-bF*x'*vMag^expDamping/(epsilonV+vMag))*Nx> + (Fy-bF*
y'*vMag^expDamping/(epsilonV+vMag))*Ny> + (Fz-bF*z'*vMag^expDamping/(
epsilonV+vMag))*Nz>
(129) %--------------------------------------------------------------------
(130) % Add torque to damp rotational motion.
(131) wMag = C.GetAngularSpeed( N )
-> (132) wMag = sqrt(wx^2+wy^2+wz^2)
(133) unitVectorAngularVelocity> = C.GetAngularVelocity( N ) / ( wMag + epsilonW )
-> (134) unitVectorAngularVelocity> = wx/(epsilonW+wMag)*Cx> + wy/(epsilonW+wM
ag)*Cy> + wz/(epsilonW+wMag)*Cz>
(135) dampingTorque> = -bT * wMag^expDamping * unitVectorAngularVelocity>
-> (136) dampingTorque> = -bT*wx*wMag^expDamping/(epsilonW+wMag)*Cx> - bT*wy*
wMag^expDamping/(epsilonW+wMag)*Cy> - bT*wz*wMag^expDamping/(epsilonW+
wMag)*Cz>
(137) C.AddTorque( dampingTorque> )
-> (138) Torque_C> = (Mx-bT*wx*wMag^expDamping/(epsilonW+wMag))*Cx> + (My-bT*
wy*wMag^expDamping/(epsilonW+wMag))*Cy> + (Mz-bT*wz*wMag^expDamping/(
epsilonW+wMag))*Cz>
(139) %--------------------------------------------------------------------
(140) % Dynamics with Kane's augmented method.
(141) Dynamics = System.GetDynamicsKane()
-> (142) Dynamics[1] = bF*x'*vMag^expDamping/(epsilonV+vMag) + m*x'' - Fx
-> (143) Dynamics[2] = bF*y'*vMag^expDamping/(epsilonV+vMag) + m*y'' - Fy
-> (144) Dynamics[3] = bF*z'*vMag^expDamping/(epsilonV+vMag) + m*z'' - Fz
-> (145) Dynamics[4] = bT*wx*wMag^expDamping/(epsilonW+wMag) + Ixx*wx' - Mx
- (Iyy-Izz)*wy*wz
-> (146) Dynamics[5] = (Ixx-Izz)*wx*wz + bT*wy*wMag^expDamping/(epsilonW+wMag)
+ Iyy*wy' - My
-> (147) Dynamics[6] = bT*wz*wMag^expDamping/(epsilonW+wMag) + Izz*wz' - Mz
- (Ixx-Iyy)*wx*wy
(148) %--------------------------------------------------------------------
(149) % Integration parameters and initial values for variables.
(150) Input tFinal = 120 sec, tStep = 0.1 sec, absError = 1.0E-07
(151) Input x' = 0 m/s, y' = 0 m/s, z' = 0 m/s, wx = 0 rad/sec, wy = 0 rad/sec, wz = 0 rad/sec
(152) %--------------------------------------------------------------------
(153) % Calculate yaw, pitch and roll angles in terms of q1, q2, q3.
(154) Yaw = GetAngleBetweenUnitVectors( Nx>, Cy> ) - pi/2
-> (155) Yaw = -1.570796 + acos(sin(q2)*sin(q3)*cos(q1)-sin(q1)*cos(q3))
(156) Pitch = GetAngleBetweenUnitVectors( Nz>, Cx> ) - pi/2
-> (157) Pitch = 1.570796 - acos(sin(q2))
(158) Roll = pi/2 - GetAngleBetweenUnitVectors( Nz>, Cy> )
-> (159) Roll = 1.570796 - acos(sin(q3)*cos(q2))
(160) %--------------------------------------------------------------------
(161) % Solve for initial values of q1, q2, q3 via initial yaw, pitch, roll angles.
(162) Constant Yaw0 = 60 deg, Pitch0 = 0 deg, Roll0 = 0 deg
(163) 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
(164) %--------------------------------------------------------------------
(165) % Solve for initial values of x, y, z using the CableConstraint (and initial values of q1, q2, q3).
(166) 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
(167) %--------------------------------------------------------------------
(168) % List output quantities and solve ODEs.
(169) Distance = Co.GetDistance( No )
-> (170) 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))
(171) Output t sec, Distance m, Yaw deg, Pitch deg, Roll deg, vMag m/s, wMag rad/sec, CableConstraint[1] m, CableConstraint[2] m, CableConstraint[3] m
(172) 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
(173) ODE( [ Dynamics; DtDt(CableConstraint) ], x'', y'', z'', wx', wy', wz', T1, T2, T3 ) MGAircraftTrifilarPendulumDynamicsKaneAugmented.m
(174) %--------------------------------------------------------------------
Saved by Motion Genesis LLC. Command names and syntax: Copyright (c) 2009-2021 Motion Genesis LLC. All rights reserved.