% MotionGenesis file:  MGSpringRestrainedDoublePendulumDynamicsFBD.txt
% Copyright (c) 2015 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
%--------------------------------------------------------------------
%       Rotational and translational kinematics
A.RotateZ( N, qA )
B.RotateZ( N, qB )
Ao.SetVelocityAcceleration( N, 0> )
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> )
%--------------------------------------------------------------------
%       Dynamics equations of motion via road-maps.
Dynamics[1] = Dot(  Nz>,  System(B,Q).GetDynamics(P)  )
Dynamics[2] = Dot(  Nz>,       System.GetDynamics(Ao) )
Dynamics[3] = Dot(  Nx>,       System.GetDynamics()   )
Dynamics[4] = Dot(  Ny>,       System.GetDynamics()   )
%--------------------------------------------------------------------
%       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.
Output  t sec,  qA deg,  qB deg,  Fx Newtons,  Fy Newtons
ODE( Dynamics,  qA'', qB'', Fx, Fy )  MGSpringRestrainedDoublePendulumDynamicsFBD
%--------------------------------------------------------------------
Save MGSpringRestrainedDoublePendulumDynamicsFBD.html
Quit