MGAircraftTrifilarPendulumStaticsKaneAugmented.html  (MotionGenesis input/output).
   (1) % MotionGenesis file:  MGAircraftTrifilarPendulumStaticsKaneAugmented.txt
   (2) % Copyright (c) 2016 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) C.SetMass( m = 9000 kg )
   (20) %--------------------------------------------------------------------
   (21) Variable  T1, T2, T3                 % Tension in cables.
   (22) Variable  x',   y',   z'             % Locates Ccm from No.
   (23) Variable  q1',  q2',  q3'            % BodyZYX Euler angles.
   (24) SetGeneralizedSpeeds(  x',  y',  z',  q1', q2', q3'  )
   (25) %--------------------------------------------------------------------
   (26) %   Rotational and translational kinematics.
   (27) C.Rotate( N, BodyZYX, q1, q2, q3 )
-> (28) C_N[1,1] = cos(q1)*cos(q2)
-> (29) C_N[1,2] = sin(q1)*cos(q2)
-> (30) C_N[1,3] = -sin(q2)
-> (31) C_N[2,1] = sin(q2)*sin(q3)*cos(q1) - sin(q1)*cos(q3)
-> (32) C_N[2,2] = cos(q1)*cos(q3) + sin(q1)*sin(q2)*sin(q3)
-> (33) C_N[2,3] = sin(q3)*cos(q2)
-> (34) C_N[3,1] = sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3)
-> (35) C_N[3,2] = sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1)
-> (36) C_N[3,3] = cos(q2)*cos(q3)
-> (37) w_C_N> = (q3'-sin(q2)*q1')*Cx> + (cos(q3)*q2'+sin(q3)*cos(q2)*q1')*Cy>
        + (cos(q2)*cos(q3)*q1'-sin(q3)*q2')*Cz>

   (38) CCm.SetPositionVelocity( No, x*Nx> + y*Ny> + z*Nz> )
-> (39) p_No_Ccm> = x*Nx> + y*Ny> + z*Nz>
-> (40) v_Ccm_N> = x'*Nx> + y'*Ny> + z'*Nz>

   (41) Co.SetPosition( CCm, -dcm*Cx> )
-> (42) p_Ccm_Co> = -dcm*Cx>

   (43) C1.SetPosition( Co,    dC*Cx> )
-> (44) p_Co_C1> = dC*Cx>

   (45) C2.SetPosition( Co,   -wC*Cy> )
-> (46) p_Co_C2> = -wC*Cy>

   (47) C3.SetPosition( Co,    wC*Cy> )
-> (48) p_Co_C3> = wC*Cy>

   (49) N1.SetPosition( No,    dN*Nx> )
-> (50) p_No_N1> = dN*Nx>

   (51) N2.SetPosition( No,   -wN*Ny> )
-> (52) p_No_N2> = -wN*Ny>

   (53) N3.SetPosition( No,    wN*Ny> )
-> (54) p_No_N3> = wN*Ny>

   (55) %--------------------------------------------------------------------
   (56) %   Configuration constraints: Length of cables.
   (57) CableConstraint[1] = C1.GetDistanceSquared( N1 ) - L1^2
-> (58) 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)

   (59) CableConstraint[2] = C2.GetDistanceSquared( N2 ) - L2^2
-> (60) 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))

   (61) CableConstraint[3] = C3.GetDistanceSquared( N3 ) - L3^2
-> (62) 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))

   (63) %--------------------------------------------------------------------
   (64) %   Replace all forces on C (gravity and tension) with equivalent set,
   (65) %   consisting of the set's resultant at Ccm along with a torque equal
   (66) %   to the moment of the cable tensions about Ccm.
   (67) %--------------------------------------------------------------------
   (68) %   Resultant force on C in terms of Fx, Fy, Fz.
   (69) unitVectorToN1FromC1> = N1.GetPosition(C1) / L1
-> (70) unitVectorToN1FromC1> = -(dC-dcm)/L1*Cx> + (dN-x)/L1*Nx> - y/L1*Ny> - z/L1*Nz>

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

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

   (75) Tension1> = T1 * unitVectorToN1FromC1>
-> (76) Tension1> = -(dC-dcm)*T1/L1*Cx> + T1*(dN-x)/L1*Nx> - T1*y/L1*Ny> - T1*z
        /L1*Nz>

   (77) Tension2> = T2 * unitVectorToN2FromC2>
-> (78) Tension2> = dcm*T2/L2*Cx> + wC*T2/L2*Cy> - T2*x/L2*Nx> - T2*(wN+y)/L2*Ny>
        - T2*z/L2*Nz>

   (79) Tension3> = T3 * unitVectorToN3FromC3>
-> (80) Tension3> = dcm*T3/L3*Cx> - wC*T3/L3*Cy> - T3*x/L3*Nx> + T3*(wN-y)/L3*Ny>
        - T3*z/L3*Nz>

   (81) ResultantForce> = Tension1> + Tension2> + Tension3> + m*g*Nz>
-> (82) 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>

   (83) Fx = Dot(  ResultantForce>,  Nx>  )
-> (84) 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))

   (85) Fy = Dot(  ResultantForce>,  Ny>  )
-> (86) 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

   (87) Fz = Dot(  ResultantForce>,  Nz>  )
-> (88) 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)

   (89) Ccm.AddForce( Fx*Nx> + Fy*Ny> + Fz*Nz> )
-> (90) Force_Ccm> = Fx*Nx> + Fy*Ny> + Fz*Nz>

   (91) %--------------------------------------------------------------------
   (92) %   Calculate moment of all forces on C about Ccm in terms of Mx, My, Mz.
   (93) ResultantMoment> = Cross( C1.GetPosition(Ccm), Tension1> )   &
                 + Cross( C2.GetPosition(Ccm), Tension2> )   &
                 + Cross( C3.GetPosition(Ccm), Tension3> )
-> (94) 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>

   (95) Mx = Dot(  ResultantMoment>,  Cx>  )
-> (96) 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))

   (97) My = Dot(  ResultantMoment>,  Cy>  )
-> (98) 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)

   (99) Mz = Dot(  ResultantMoment>,  Cz>  )
-> (100) 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)

   (101) C.AddTorque( Mx*Cx> + My*Cy> + Mz*Cz> )
-> (102) Torque_C> = Mx*Cx> + My*Cy> + Mz*Cz>

   (103) %--------------------------------------------------------------------
   (104) %   For statics: Set generalized forces to zero (Kane and Lagrange are identical).
   (105) Statics = System.GetStaticsKane()
-> (106) Statics[1] = Fx
-> (107) Statics[2] = Fy
-> (108) Statics[3] = Fz
-> (109) Statics[4] = sin(q3)*cos(q2)*My + cos(q2)*cos(q3)*Mz - sin(q2)*Mx
-> (110) Statics[5] = cos(q3)*My - sin(q3)*Mz
-> (111) Statics[6] = Mx

   (112) %--------------------------------------------------------------------
   (113) %   Calculate yaw, pitch and roll angles in terms of q1, q2, q3.
   (114) Yaw   = GetAngleBetweenUnitVectors( Nx>, Cy> )  -  pi/2
-> (115) Yaw = -1.570796 + acos(sin(q2)*sin(q3)*cos(q1)-sin(q1)*cos(q3))

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

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

   (120) %--------------------------------------------------------------------
   (121) %   Static analysis.  Since equations are nonlinear, solution requires a guess.
   (122) StaticsWithConstraints = [ Statics;  CableConstraint ]
-> (123) StaticsWithConstraints[1] = Fx
-> (124) StaticsWithConstraints[2] = Fy
-> (125) StaticsWithConstraints[3] = Fz
-> (126) StaticsWithConstraints[4] = sin(q3)*cos(q2)*My + cos(q2)*cos(q3)*Mz -
         sin(q2)*Mx
-> (127) StaticsWithConstraints[5] = cos(q3)*My - sin(q3)*Mz
-> (128) StaticsWithConstraints[6] = Mx
-> (129) StaticsWithConstraints[7] = (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)

-> (130) StaticsWithConstraints[8] = 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))

-> (131) StaticsWithConstraints[9] = 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))

   (132) %--------------------------------------------------------------------
   (133) %   Statics 1:  Equal-length cables  L1 = L2 = L3 = 30 m.
   (134) ans1 = Solve( StaticsWithConstraints,   q1=0 deg,  q2=0 deg,  q3=0 deg,  x = Input(dN)/2 m,  y=0 m,  z = Input(L1) m,  &
                                        T1 = Input(m*g)/2 N,  T2 = Input(m*g)/4 N,  T3 = Input(m*g)/4 N )
->    %  Note: q1 = -1.091977E-19  was converted from  q1 = -6.256565E-18 deg.
->    %  Note: q2 = 4.49892E-17  was converted from  q2 = 2.577691E-15 deg.
->    %  Note: q3 = 2.871825E-18  was converted from  q3 = 1.645434E-16 deg.
-> (135) ans1 = [-1.091977E-19;  4.49892E-17;  2.871825E-18;  8;  7.857764E-18;  
         30;  23520;  32340;  32340]

   (136) %--------------------------------------------------------------------
   (137) %   Statics 2:  Unequal-length cables  L1 = L2 = 22 m,   L3 = 30 m.
   (138) Input  L1 := 22 m,  L2 := 22 m,  L3 := 30 m
   (139) SolveSetInput( StaticsWithConstraints,  q1=0 deg,  q2=0 deg,  q3=0 deg,  x = Input(dN)/2 m,  y=0 m,  z = Input(L1) m,  &
                                        T1 = Input(m*g)/2 N,  T2 = Input(m*g)/4 N,  T3 = Input(m*g)/4 N )

->    %  INPUT has been assigned as follows:
->    %   q1                        0.9593486506940117      deg
->    %   q2                        7.659043192403285       deg
->    %   q3                        11.63295584237929       deg
->    %   x                         8.031358951235374       m
->    %   y                        -0.05976274847707876     m
->    %   z                         24.92935115026747       m
->    %   T1                        23615.15761570216       N
->    %   T2                        32427.5589998023        N
->    %   T3                        32170.25182512401       N

   (140) DistanceStatic     = EvaluateToNumber( Co.GetDistance(No) )
-> (141) DistanceStatic = 25.99649

   (142) YawStaticDegrees   = EvaluateToNumber( Yaw )   *  180/pi
-> (143) YawStaticDegrees = -0.5999802

   (144) PitchStaticDegrees = EvaluateToNumber( Pitch ) *  180/pi
-> (145) PitchStaticDegrees = 7.659043

   (146) RollStaticDegrees  = EvaluateToNumber( Roll )  *  180/pi
-> (147) RollStaticDegrees = 11.52774

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