(1) % MotionGenesis file:  MGBabybootDynamics.txt
   (2) % Problem: Analysis of 3D chaotic double pendulum.
   (3) % Copyright (c) 2009 Motion Genesis LLC.  All rights reserved.
   (4) %--------------------------------------------------------------------
   (5) SetDigits( 5 )               % Number of digits displayed for numbers.
   (6) %--------------------------------------------------------------------
   (7) NewtonianFrame  N            % Earth.
   (8) RigidBody       A            % Upper rod.
   (9) RigidBody       B            % Lower plate.
   (10) %--------------------------------------------------------------------
   (11) Variable   qA''              % Pendulum angle and its time-derivatives.
   (12) Variable   qB''              % Plate angle and its time-derivative.
   (13) Constant   LA = 7.5 cm       % Distance from pivot to A's mass center.
   (14) Constant   LB = 20 cm        % Distance from pivot to B's mass center.
   (15) Constant   g = 9.81 m/s^2    % Earth's gravitational acceleration.
   (16) A.SetMassInertia( mA =  10 grams,  IAx = 50 g*cm^2,  IAy,  IAz )
   (17) B.SetMassInertia( mB = 100 grams,  IBx = 2500 g*cm^2, IBy = 500 g*cm^2, IBz = 2000 g*cm^2 )
   (18) %--------------------------------------------------------------------
   (19) %   Rotational kinematics.
   (20) A.RotateX( N, qA )
-> (21) A_N = [1, 0, 0;  0, cos(qA), sin(qA);  0, -sin(qA), cos(qA)]
-> (22) w_A_N> = qA'*Ax>
-> (23) alf_A_N> = qA''*Ax>

   (24) B.RotateZ( A, qB )
-> (25) B_A = [cos(qB), sin(qB), 0;  -sin(qB), cos(qB), 0;  0, 0, 1]
-> (26) w_B_A> = qB'*Bz>
-> (27) w_B_N> = cos(qB)*qA'*Bx> - sin(qB)*qA'*By> + qB'*Bz>
-> (28) alf_B_A> = qB''*Bz>
-> (29) alf_B_N> = (cos(qB)*qA''-sin(qB)*qA'*qB')*Bx> + (-cos(qB)*qA'*qB'-sin(
        qB)*qA'')*By> + qB''*Bz>

   (30) %--------------------------------------------------------------------
   (31) %   Translational kinematics.
   (32) Acm.Translate( No, -LA*Az> )
-> (33) p_No_Acm> = -LA*Az>
-> (34) v_Acm_N> = LA*qA'*Ay>
-> (35) a_Acm_N> = LA*qA''*Ay> + LA*qA'^2*Az>

   (36) Bcm.Translate( No, -LB*Az> )
-> (37) p_No_Bcm> = -LB*Az>
-> (38) v_Bcm_N> = LB*qA'*Ay>
-> (39) a_Bcm_N> = LB*qA''*Ay> + LB*qA'^2*Az>

   (40) %--------------------------------------------------------------------
   (41) %   Add relevant contact/distance forces.
   (42) System.AddForceGravity( -g*Nz> )
-> (43) Force_Acm> = -mA*g*Nz>
-> (44) Force_Bcm> = -mB*g*Nz>

   (45) %--------------------------------------------------------------------
   (46) %   Equations of motion via free-body-diagrams (MG road-maps).
   (47) Dynamics[1] = Dot( Ax>,  System(A,B).GetDynamics(No)  )
-> (48) Dynamics[1] = g*(mA*LA+mB*LB)*sin(qA) + IAx*qA'' + mA*LA^2*qA'' + mB*LB^2*qA''
        + IBy*sin(qB)^2*qA'' + cos(qB)*(2*IBy*sin(qB)*qA'*qB'-IBx*(2*sin(qB)*
        qA'*qB'-cos(qB)*qA''))

   (49) Dynamics[2] = Dot( Bz>,            B.GetDynamics(Bcm) )
-> (50) Dynamics[2] = (IBx-IBy)*sin(qB)*cos(qB)*qA'^2 + IBz*qB''

   (51) %--------------------------------------------------------------------
   (52) %   Kinetic and potential energy.
   (53) KE = System.GetKineticEnergy()
-> (54) KE = 0.5*IAx*qA'^2 + 0.5*IBx*qA'^2 + 0.5*IBz*qB'^2 + 0.5*mA*LA^2*qA'^2
        + 0.5*mB*LB^2*qA'^2 - 0.5*(IBx-IBy)*sin(qB)^2*qA'^2

   (55) PE = System.GetForceGravityPotentialEnergy( -g*Nz>, No )
-> (56) PE = -g*(mA*LA+mB*LB)*cos(qA)

   (57) MechanicalEnergy = KE + PE
-> (58) MechanicalEnergy = PE + KE

   (59) %--------------------------------------------------------------------
   (60) %   Optional: Equations of motion with Kane's method.
   (61) SetGeneralizedSpeed( qA', qB' )
   (62) KaneDynamics = System.GetDynamicsKane()
-> (63) KaneDynamics[1] = g*(mA*LA+mB*LB)*sin(qA) + (IAx+IBx+mA*LA^2+mB*LB^2-(
        IBx-IBy)*sin(qB)^2)*qA'' - 2*(IBx-IBy)*sin(qB)*cos(qB)*qA'*qB'

-> (64) KaneDynamics[2] = (IBx-IBy)*sin(qB)*cos(qB)*qA'^2 + IBz*qB''

   (65) isSameDynamics = IsSimplifyEqual( Dynamics, KaneDynamics )
-> (66) isSameDynamics = true

   (67) %--------------------------------------------------------------------
   (68) %   Optional: Equations of motion with Lagranges's method.
   (69) SetGeneralizedCoordinates( qA, qB )
   (70) LagrangeDynamics = System.GetDynamicsLagrange( SystemPotential = PE )
-> (71) LagrangeDynamics[1] = g*(mA*LA+mB*LB)*sin(qA) + (IAx+IBx+mA*LA^2+mB*LB^2
        -(IBx-IBy)*sin(qB)^2)*qA'' - 2*(IBx-IBy)*sin(qB)*cos(qB)*qA'*qB'

-> (72) LagrangeDynamics[2] = (IBx-IBy)*sin(qB)*cos(qB)*qA'^2 + IBz*qB''

   (73) isSameDynamics := IsSimplifyEqual( Dynamics, LagrangeDynamics )
-> (74) isSameDynamics = true

   (75) %--------------------------------------------------------------------
   (76) %   Solve dynamics equations for qA'', qB''.
   (77) Solve( Dynamics = 0,   qA'',  qB'')
-> (78) qA'' = -(g*(mA*LA+mB*LB)*sin(qA)-2*(IBx-IBy)*sin(qB)*cos(qB)*qA'*qB')/(
        IAx+mA*LA^2+mB*LB^2+IBx*cos(qB)^2+IBy*sin(qB)^2)

-> (79) qB'' = -(IBx-IBy)*sin(qB)*cos(qB)*qA'^2/IBz

   (80) %--------------------------------------------------------------------
   (81) %   Integration parameters and initial values.
   (82) Input  tFinal = 10 sec,  tStep = 0.02 sec,  absError = 1.0E-07,  relError = 1.0E-07
   (83) Input  qA = 90 deg,  qA' = 0.0 rad/sec,  qB = 1.0 deg,  qB' = 0.0 rad/sec
   (84) %--------------------------------------------------------------------
   (85) %   List output quantities and solve ODEs.
   (86) Output  t sec,  qA deg,  qB deg,  MechanicalEnergy Joules
   (87) ODE()  MGBabybootDynamics

   (88) %--------------------------------------------------------------------
   (89) %   Record input together with responses