(1) % MotionGenesis file:  MGSpringRestrainedDoublePendulumDynamicsFBD.txt
   (2) % Copyright (c) 2015 Motion Genesis LLC.  All rights reserved.
   (3) %---------------------------------------------------------
   (4) NewtonianFrame  N
   (5) RigidFrame      A, B                % Rods
   (6) Particle        P, Q                % Particles at end of A, B
   (7) %---------------------------------------------------------
   (8) Variable   qA'',  qB''              % Angles for A and B
   (9) Constant   LA = 1 m,  LB = 2 m      % Lengths of A, B
   (10) Constant   k = 200 N/m,   Ln = 1 m  % Spring constant, natural length
   (11) Constant   b = 100 N*s/m            % Damping constant
   (12) Constant   c = 100 N*m*s/rad        % Damping constant
   (13) Constant   g = 9.8 m/s^2            % Earth's gravitational acceleration
   (14) P.SetMass( mP = 10 kg )
   (15) Q.SetMass( mQ = 20 kg )
   (16) Variable   Fx, Fy                   % Contact forces on Ao from No
   (17) Specified  Stretch'                 % Elongation of spring between O and Q
   (18) %--------------------------------------------------------------------
   (19) %       Rotational and translational kinematics
   (20) A.RotateZ( N, qA )
-> (21) A_N = [cos(qA), sin(qA), 0;  -sin(qA), cos(qA), 0;  0, 0, 1]
-> (22) w_A_N> = qA'*Az>
-> (23) alf_A_N> = qA''*Az>

   (24) B.RotateZ( N, qB )
-> (25) B_N = [cos(qB), sin(qB), 0;  -sin(qB), cos(qB), 0;  0, 0, 1]
-> (26) w_B_N> = qB'*Bz>
-> (27) alf_B_N> = qB''*Bz>

   (28) Ao.SetVelocityAcceleration( N, 0> )
-> (29) v_Ao_N> = 0>
-> (30) a_Ao_N> = 0>

   (31) P.Translate( Ao, LA*Ax> )
-> (32) p_Ao_P> = LA*Ax>
-> (33) v_P_N> = LA*qA'*Ay>
-> (34) a_P_N> = -LA*qA'^2*Ax> + LA*qA''*Ay>

   (35) Q.Translate(  P, LB*Bx> )
-> (36) p_P_Q> = LB*Bx>
-> (37) v_Q_N> = LA*qA'*Ay> + LB*qB'*By>
-> (38) a_Q_N> = -LA*qA'^2*Ax> + LA*qA''*Ay> - LB*qB'^2*Bx> + LB*qB''*By>

   (39) %--------------------------------------------------------------------
   (40) %       Relevant forces for statics (gravity, spring, contact forces).
   (41) System.AddForceGravity( g*Nx> )
-> (42) Force_P> = mP*g*Nx>
-> (43) Force_Q> = mQ*g*Nx>

   (44) LSpring = Q.GetDistance( Ao )                    % Distance between Q and Ao
-> (45) LSpring = sqrt(LA^2+LB^2+2*LA*LB*cos(qA-qB))

   (46) SetNoDt( Stretch = LSpring - Ln )                % Calculate spring stretch 
-> (47) Stretch = LSpring - Ln

   (48) UnitVectorFromAoToQ> = Q.GetPosition( Ao ) / LSpring 
-> (49) UnitVectorFromAoToQ> = LA/LSpring*Ax> + LB/LSpring*Bx>

   (50) Q.AddForce( Ao,  -k * Stretch * UnitVectorFromAoToQ> ) 
-> (51) Force_Q_Ao> = -k*LA*Stretch/LSpring*Ax> - k*LB*Stretch/LSpring*Bx>

   (52) Ao.AddForce( No,  Fx*Nx> + Fy*Ny> )              % Contact force on A from N 
-> (53) Force_Ao_No> = Fx*Nx> + Fy*Ny>

   (54) %--------------------------------------------------------------------
   (55) %       Damping force and torques.
   (56) Stretch' = Dot( Q.GetVelocity(N), UnitVectorFromAoToQ> ) 
-> (57) Stretch' = -LA*LB*sin(qA-qB)*(qA'-qB')/LSpring

   (58) Q.AddForce( Ao,  -b * Stretch' * UnitVectorFromAoToQ> ) 
-> (59) Force_Q_Ao> = -LA*(k*Stretch+b*Stretch')/LSpring*Ax> - LB*(k*Stretch+b*
        Stretch')/LSpring*Bx>

   (60) A.AddTorque( -c * qA' * Az> )
-> (61) Torque_A> = -c*qA'*Az>

   (62) B.AddTorque( -c * qB' * Bz> )
-> (63) Torque_B> = -c*qB'*Bz>

   (64) %--------------------------------------------------------------------
   (65) %       Dynamics equations of motion via road-maps.
   (66) Dynamics[1] = Dot(  Nz>,  System(B,Q).GetDynamics(P)  )
-> (67) Dynamics[1] = c*qB' + mQ*LB^2*qB'' - LB*(mQ*LA*sin(qA-qB)*qA'^2-mQ*g*
        sin(qB)-LA*(k*Stretch+b*Stretch')*sin(qA-qB)/LSpring-mQ*LA*cos(qA-qB)*
        qA'')

   (68) Dynamics[2] = Dot(  Nz>,       System.GetDynamics(Ao) )
-> (69) Dynamics[2] = g*(mP*LA*sin(qA)+mQ*LA*sin(qA)+mQ*LB*sin(qB)) + c*qA'
        + c*qB' + mQ*LA*LB*sin(qA-qB)*qB'^2 + mP*LA^2*qA'' + mQ*LA^2*qA''
        + mQ*LB^2*qB'' + mQ*LA*LB*cos(qA-qB)*qA'' + mQ*LA*LB*cos(qA-qB)*qB''
        - mQ*LA*LB*sin(qA-qB)*qA'^2

   (70) Dynamics[3] = Dot(  Nx>,       System.GetDynamics()   )
-> (71) Dynamics[3] = -mP*g - mQ*g - Fx - mQ*LB*cos(qB)*qB'^2 - (mP+mQ)*LA*cos(
        qA)*qA'^2 - mQ*LB*sin(qB)*qB'' - (mP+mQ)*LA*sin(qA)*qA''

   (72) Dynamics[4] = Dot(  Ny>,       System.GetDynamics()   )
-> (73) Dynamics[4] = mQ*LB*cos(qB)*qB'' + (mP+mQ)*LA*cos(qA)*qA'' - Fy - mQ*
        LB*sin(qB)*qB'^2 - (mP+mQ)*LA*sin(qA)*qA'^2

   (74) %--------------------------------------------------------------------
   (75) %       Integration parameters and initial values.
   (76) Input  tFinal = 16,  tStep = 0.1,  absError = 1.0E-07
   (77) Input  qA = 20 deg,  qB = 60 deg,  qA' = 0 rad/sec,  qB' =0 rad/sec 
   (78) %--------------------------------------------------------------------
   (79) %       List output quantities and solve ODEs.
   (80) Output  t sec,  qA deg,  qB deg,  Fx Newtons,  Fy Newtons
   (81) ODE( Dynamics,  qA'', qB'', Fx, Fy )  MGSpringRestrainedDoublePendulumDynamicsFBD

   (82) %--------------------------------------------------------------------