MGSpringRestrainedDoublePendulumDynamicsKane.html   (MotionGenesis input/output).
   (1) % MotionGenesis file:  MGSpringRestrainedDoublePendulumDynamicsKane.txt
   (2) % Copyright (c) 2009 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) Variable   vx', vy'                 % Auxiliary generalized speeds to calculate Fx and Fy.
   (20) SetGeneralizedSpeed( qA', qB', vx, vy )
   (21) SetDt( vx = 0 );    SetDt( vy = 0 )
-> (22) vx = 0
-> (23) vx' = 0
-> (24) vy = 0
-> (25) vy' = 0

   (26) %--------------------------------------------------------------------
   (27) %       Rotational and translational kinematics
   (28) A.RotateZ( N, qA )
-> (29) A_N = [cos(qA), sin(qA), 0;  -sin(qA), cos(qA), 0;  0, 0, 1]
-> (30) w_A_N> = qA'*Az>
-> (31) alf_A_N> = qA''*Az>

   (32) B.RotateZ( N, qB )
-> (33) B_N = [cos(qB), sin(qB), 0;  -sin(qB), cos(qB), 0;  0, 0, 1]
-> (34) w_B_N> = qB'*Bz>
-> (35) alf_B_N> = qB''*Bz>

   (36) Ao.SetVelocityAcceleration( N,  vx*Nx> + vy*Ny> ) 
-> (37) v_Ao_N> = vx*Nx> + vy*Ny>
-> (38) a_Ao_N> = 0>

   (39) P.Translate( Ao, LA*Ax> )
-> (40) p_Ao_P> = LA*Ax>
-> (41) v_P_N> = LA*qA'*Ay> + vx*Nx> + vy*Ny>
-> (42) a_P_N> = -LA*qA'^2*Ax> + LA*qA''*Ay>

   (43) Q.Translate(  P, LB*Bx> )
-> (44) p_P_Q> = LB*Bx>
-> (45) v_Q_N> = LA*qA'*Ay> + LB*qB'*By> + vx*Nx> + vy*Ny>
-> (46) a_Q_N> = -LA*qA'^2*Ax> + LA*qA''*Ay> - LB*qB'^2*Bx> + LB*qB''*By>

   (47) %--------------------------------------------------------------------
   (48) %       Relevant forces for statics (gravity, spring, contact forces).
   (49) System.AddForceGravity( g*Nx> )
-> (50) Force_P> = mP*g*Nx>
-> (51) Force_Q> = mQ*g*Nx>

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

   (54) SetNoDt( Stretch = LSpring - Ln )                % Calculate spring stretch 
-> (55) Stretch = LSpring - Ln

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

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

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

   (62) %--------------------------------------------------------------------
   (63) %       Damping force and torques.
   (64) Stretch' = Dot( Q.GetVelocity(N), UnitVectorFromAoToQ> ) 
-> (65) Stretch' = -(LA*LB*sin(qA-qB)*qA'-LA*sin(qA)*vy-LA*cos(qA)*vx-LB*sin(
        qB)*vy-LB*cos(qB)*vx-LA*LB*sin(qA-qB)*qB')/LSpring

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

   (68) A.AddTorque( -c * qA' * Az> )
-> (69) Torque_A> = -c*qA'*Az>

   (70) B.AddTorque( -c * qB' * Bz> )
-> (71) Torque_B> = -c*qB'*Bz>

   (72) %--------------------------------------------------------------------
   (73) %       Equations of motion via Kane's method.
   (74) Zero = System.GetDynamicsKane()
-> (75) Zero[1] = mP*g*LA*sin(qA) + mQ*g*LA*sin(qA) + c*qA' + mQ*LA*LB*sin(qA-
        qB)*qB'^2 + (mP+mQ)*LA^2*qA'' + mQ*LA*LB*cos(qA-qB)*qB'' - LA*LB*(k*St
        retch+b*Stretch')*sin(qA-qB)/LSpring

-> (76) Zero[2] = mQ*g*LB*sin(qB) + LA*LB*(k*Stretch+b*Stretch')*sin(qA-qB)/LSpring
        + c*qB' + mQ*LB^2*qB'' + mQ*LA*LB*cos(qA-qB)*qA'' - mQ*LA*LB*sin(qA-qB)
        *qA'^2

-> (77) Zero[3] = -mP*g - mQ*g - Fx - mP*LA*cos(qA)*qA'^2 - mQ*(LA*cos(qA)*qA'^2
        +LB*cos(qB)*qB'^2) - mQ*LB*sin(qB)*qB'' - (mP+mQ)*LA*sin(qA)*qA''

-> (78) Zero[4] = mQ*LB*cos(qB)*qB'' + (mP+mQ)*LA*cos(qA)*qA'' - Fy - mP*LA*sin
        (qA)*qA'^2 - mQ*(LA*sin(qA)*qA'^2+LB*sin(qB)*qB'^2)

   (79) %--------------------------------------------------------------------
   (80) %       Integration parameters and initial values.
   (81) Input  tFinal = 16,  tStep = 0.1,  absError = 1.0E-07
   (82) Input  qA = 20 deg,  qB = 60 deg,  qA' = 0 rad/sec,  qB' =0 rad/sec 
   (83) %--------------------------------------------------------------------
   (84) %       List output quantities and solve ODEs.
   (85) EnergyCheck = System.GetEnergyCheckKane() 
-> (86) WCheck1' = qB'*(mQ*g*LB*sin(qB)+LA*LB*(k*Stretch+b*Stretch')*sin(qA-qB)
        /LSpring+c*qB') - qA'*(LA*LB*(k*Stretch+b*Stretch')*sin(qA-qB)/LSpring-
        mP*g*LA*sin(qA)-mQ*g*LA*sin(qA)-c*qA')

-> (87) EnergyCheck = WCheck1 + 0.5*mP*LA^2*qA'^2 + 0.5*mQ*(LA^2*qA'^2+LB^2*qB'^2
        +2*LA*LB*cos(qA-qB)*qA'*qB')

   (88) OutputPlot  t sec,  qA deg,  qB deg,  Fx Newtons, Fy Newtons,  EnergyCheck Joules
   (89) ODE( Zero,  qA'', qB'', Fx, Fy )  MGSpringRestrainedDoublePendulumDynamicsKane

   (90) %--------------------------------------------------------------------
Saved by Motion Genesis LLC.   MotionGenesis 6.3 Professional.
Command names and syntax: Copyright (c) 2009-2023 Motion Genesis LLC. All rights reserved.