% MotionGenesis file:  MGSpringRestrainedDoublePendulumStatics.txt
% Copyright (c) 2009 Motion Genesis LLC.  All rights reserved.
%---------------------------------------------------------
NewtonianFrame  N                   % Earth.
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   g = 9.8 m/s^2            % Earth's gravitational acceleration.
P.SetMass( mP = 10 kg )
Q.SetMass( mQ = 20 kg )
%---------------------------------------------------------
%   Rotational kinematics.
A.RotateZ( N, qA )
B.RotateZ( N, qB )
%---------------------------------------------------------
%   Translational kinematics.
P.SetPosition( Ao, LA*Ax> )
Q.SetPosition(  P, LB*Bx> )
%---------------------------------------------------------
%   Add relevant contact and distance forces.
System.AddForceGravity( g*Nx> )
LSpring = Q.GetDistance( Ao )
SpringStretch = LSpring - Ln
UnitVectorFromNoToQ> = Q.GetPosition( Ao ) / LSpring
Q.AddForce( No,  -k * SpringStretch * UnitVectorFromNoToQ> )
%--------------------------------------------------------------------
%   Static equations of motion via MG road-maps.
Statics[1] = Dot(  Nz>,  System(B,Q).GetStatics(P)  )
Statics[2] = Dot(  Nz>,       System.GetStatics(Ao) )
%----------------------------------------------------------
%   Optional: Statics via potential energy.
PESpring = 1/2*k*SpringStretch^2
PEGravity = System.GetForceGravityPotentialEnergy( g*Nx>, Ao )
PotentialEnergy = PEGravity + PESpring
StaticsViaPE[1] = -D( PotentialEnergy, qA )
StaticsViaPE[2] = -D( PotentialEnergy, qB )
%----------------------------------------------------------
%   Optional: Statics with Lagrange's method.
SetGeneralizedCoordinate( qA, qB )
LagrangeStatics = System.GetStaticsLagrange( systemPotential = PotentialEnergy )
%----------------------------------------------------------
%       Solve nonlinear equations (requires a guess).
Solve( Statics = 0,  qA = -30 deg,  qB = 30 deg )
%----------------------------------------------------------
Save MGSpringRestrainedDoublePendulumStatics.html
Quit


%----------------------------------------------------------
%   Optional: Statics with Kane's method.
SetGeneralizedSpeed( qA', qB' )
KaneStatics = System.GetStaticsKane( systemPotential = PotentialEnergy )