BabybootWithDAlembertMethod.html  (MotionGenesis input/output).
   (1) % MotionGenesis file:  BabybootWithDAlembertMethod.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
   (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) A.SetMassInertia( mA =  10 grams,  IAx = 50 g*cm^2,  IAy,  IAz )
   (16) B.SetMassInertia( mB = 100 grams,  IBx = 2500 g*cm^2, IBy = 500 g*cm^2, IBz = 2000 g*cm^2 )
   (17) %----------------------------------------------------------------
   (18) %       Rotational and translational kinematics.
   (19) A.RotateX( N, qA )
-> (20) A_N = [1, 0, 0;  0, cos(qA), sin(qA);  0, -sin(qA), cos(qA)]
-> (21) w_A_N> = qA'*Ax>
-> (22) alf_A_N> = qA''*Ax>

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

   (29) Acm.Translate( No, -LA*Az> )
-> (30) p_No_Acm> = -LA*Az>
-> (31) v_Acm_N> = LA*qA'*Ay>
-> (32) a_Acm_N> = LA*qA''*Ay> + LA*qA'^2*Az>

   (33) Bcm.Translate( No, -LB*Az> )
-> (34) p_No_Bcm> = -LB*Az>
-> (35) v_Bcm_N> = LB*qA'*Ay>
-> (36) a_Bcm_N> = LB*qA''*Ay> + LB*qA'^2*Az>

   (37) %----------------------------------------------------------------
   (38) %       Add relevant forces.
   (39) g> = -9.81*Nz>
-> (40) g> = -9.81*Nz>

   (41) System.AddForceGravity( g> )
-> (42) Force_Acm> = -9.81*mA*Nz>
-> (43) Force_Bcm> = -9.81*mB*Nz>

   (44) %----------------------------------------------------------------
   (45) %       Form equations of motion (angular momentum principle).
   (46) Dynamics[1] = Dot( Bz>,  B.GetDynamics(Bcm)  )
-> (47) Dynamics[1] = (IBx-IBy)*sin(qB)*cos(qB)*qA'^2 + IBz*qB''

   (48) Dynamics[2] = Dot( Ax>,  System(A,B).GetDynamics(No)  )
-> (49) Dynamics[2] = 9.81*(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''))

   (50) Solve( Dynamics = 0,  qA'',  qB'' )
-> (51) qA'' = -2*(4.905*(mA*LA+mB*LB)*sin(qA)-(IBx-IBy)*sin(qB)*cos(qB)*qA'*
        qB')/(IAx+mA*LA^2+mB*LB^2+IBx*cos(qB)^2+IBy*sin(qB)^2)

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

   (53) %----------------------------------------------------------------
   (54) %       Kinetic and potential energy.
   (55) KE = System.GetKineticEnergy()
-> (56) 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

   (57) PE = System.GetForceGravityPotentialEnergy( g>, No )
-> (58) PE = -9.81*(mA*LA+mB*LB)*cos(qA)

   (59) Energy = KE + PE
-> (60) Energy = PE + KE

   (61) %----------------------------------------------------------------
   (62) %       Integration parameters and initial values.
   (63) Input  tFinal = 10 sec,  tStep = 0.02 sec,  absError = 1.0E-07,  relError = 1.0E-07    
   (64) Input  qA = 90 deg,  qA' = 0.0 rad/sec,  qB = 1.0 deg,  qB' = 0.0 rad/sec 
   (65) %----------------------------------------------------------------
   (66) %       List output quantities and solve ODEs.
   (67) OutputPlot  t sec,  qA deg,  qB deg,  Energy N*m
   (68) ODE()  BabybootDAlembert

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