% MotionGenesis file:  MGSpringRestrainedDoublePendulumDynamicsKane.txt
% Copyright (c) 2009 Motion Genesis LLC.  All rights reserved.
%---------------------------------------------------------
NewtonianFrame  N
RigidFrame      A, B                % Rods
Particle        P, Q                % Particles at end of A, B
%---------------------------------------------------------
Variable   qA'',  qB''              % Angles for A and B
Constant   LA = 1 m,  LB = 2 m      % Lengths of A, B
Constant   k = 200 N/m,   Ln = 1 m  % Spring constant, natural length
Constant   b = 100 N*s/m            % Damping constant
Constant   c = 100 N*m*s/rad        % Damping constant
Constant   g = 9.8 m/s^2            % Earth's gravitational acceleration
P.SetMass( mP = 10 kg )
Q.SetMass( mQ = 20 kg )
Variable   Fx, Fy                   % Contact forces on Ao from No
Specified  Stretch'                 % Elongation of spring between O and Q
%--------------------------------------------------------------------
Variable   vx', vy'                 % Auxiliary generalized speeds to calculate Fx and Fy.
SetGeneralizedSpeed( qA', qB', vx, vy )
SetDt( vx = 0 );    SetDt( vy = 0 )
%--------------------------------------------------------------------
%       Rotational and translational kinematics
A.RotateZ( N, qA )
B.RotateZ( N, qB )
Ao.SetVelocityAcceleration( N,  vx*Nx> + vy*Ny> ) 
P.Translate( Ao, LA*Ax> )
Q.Translate(  P, LB*Bx> )
%--------------------------------------------------------------------
%       Relevant forces for statics (gravity, spring, contact forces).
System.AddForceGravity( g*Nx> )
LSpring = Q.GetDistance( Ao )                    % Distance between Q and Ao
SetNoDt( Stretch = LSpring - Ln )                % Calculate spring stretch 
UnitVectorFromAoToQ> = Q.GetPosition( Ao ) / LSpring 
Q.AddForce( Ao,  -k * Stretch * UnitVectorFromAoToQ> ) 
Ao.AddForce( No,  Fx*Nx> + Fy*Ny> )              % Contact force on A from N 
%--------------------------------------------------------------------
%       Damping force and torques.
Stretch' = Dot( Q.GetVelocity(N), UnitVectorFromAoToQ> ) 
Q.AddForce( Ao,  -b * Stretch' * UnitVectorFromAoToQ> ) 
A.AddTorque( -c * qA' * Az> )
B.AddTorque( -c * qB' * Bz> )
%--------------------------------------------------------------------
%       Equations of motion via Kane's method.
Zero = System.GetDynamicsKane()
%--------------------------------------------------------------------
%       Integration parameters and initial values.
Input  tFinal = 16,  tStep = 0.1,  absError = 1.0E-07
Input  qA = 20 deg,  qB = 60 deg,  qA' = 0 rad/sec,  qB' =0 rad/sec 
%--------------------------------------------------------------------
%       List output quantities and solve ODEs.
EnergyCheck = System.GetEnergyCheckKane() 
OutputPlot  t sec,  qA deg,  qB deg,  Fx Newtons, Fy Newtons,  EnergyCheck Joules
ODE( Zero,  qA'', qB'', Fx, Fy )  MGSpringRestrainedDoublePendulumDynamicsKane
%--------------------------------------------------------------------
Save MGSpringRestrainedDoublePendulumDynamicsKane.html
Quit