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.