(1) % MotionGenesis file:  MGSpringRestrainedDoublePendulumStatics.txt
   (2) % Copyright (c) 2009 Motion Genesis LLC.  All rights reserved.
   (3) %---------------------------------------------------------
   (4) NewtonianFrame  N                   % Earth.
   (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   g = 9.8 m/s^2            % Earth's gravitational acceleration.
   (12) P.SetMass( mP = 10 kg )
   (13) Q.SetMass( mQ = 20 kg )
   (14) %---------------------------------------------------------
   (15) %   Rotational kinematics.
   (16) A.RotateZ( N, qA )
-> (17) A_N = [cos(qA), sin(qA), 0;  -sin(qA), cos(qA), 0;  0, 0, 1]

   (18) B.RotateZ( N, qB )
-> (19) B_N = [cos(qB), sin(qB), 0;  -sin(qB), cos(qB), 0;  0, 0, 1]

   (20) %---------------------------------------------------------
   (21) %   Translational kinematics.
   (22) P.SetPosition( Ao, LA*Ax> )
-> (23) p_Ao_P> = LA*Ax>

   (24) Q.SetPosition(  P, LB*Bx> )
-> (25) p_P_Q> = LB*Bx>

   (26) %---------------------------------------------------------
   (27) %   Add relevant contact and distance forces.
   (28) System.AddForceGravity( g*Nx> )
-> (29) Force_P> = mP*g*Nx>
-> (30) Force_Q> = mQ*g*Nx>

   (31) LSpring = Q.GetDistance( Ao )
-> (32) LSpring = sqrt(LA^2+LB^2+2*LA*LB*cos(qA-qB))

   (33) SpringStretch = LSpring - Ln
-> (34) SpringStretch = LSpring - Ln

   (35) UnitVectorFromNoToQ> = Q.GetPosition( Ao ) / LSpring
-> (36) UnitVectorFromNoToQ> = LA/LSpring*Ax> + LB/LSpring*Bx>

   (37) Q.AddForce( No,  -k * SpringStretch * UnitVectorFromNoToQ> )
-> (38) Force_Q_No> = -k*LA*SpringStretch/LSpring*Ax> - k*LB*SpringStretch/LSpring*Bx>

   (39) %--------------------------------------------------------------------
   (40) %   Static equations of motion via MG road-maps.
   (41) Statics[1] = Dot(  Nz>,  System(B,Q).GetStatics(P)  )
-> (42) Statics[1] = -LB*(mQ*g*sin(qB)+k*LA*sin(qA-qB)*SpringStretch/LSpring)

   (43) Statics[2] = Dot(  Nz>,       System.GetStatics(Ao) )
-> (44) Statics[2] = -g*(mP*LA*sin(qA)+mQ*LA*sin(qA)+mQ*LB*sin(qB))

   (45) %----------------------------------------------------------
   (46) %   Optional: Statics via potential energy.
   (47) PESpring = 1/2*k*SpringStretch^2
-> (48) PESpring = 0.5*k*SpringStretch^2

   (49) PEGravity = System.GetForceGravityPotentialEnergy( g*Nx>, Ao )
-> (50) PEGravity = -g*(mP*LA*cos(qA)+mQ*(LA*cos(qA)+LB*cos(qB)))

   (51) PotentialEnergy = PEGravity + PESpring
-> (52) PotentialEnergy = PEGravity + PESpring

   (53) StaticsViaPE[1] = -D( PotentialEnergy, qA )
-> (54) StaticsViaPE[1] = -LA*((mP+mQ)*g*sin(qA)-k*LB*sin(qA-qB)*SpringStretch/
        sqrt(LA^2+LB^2+2*LA*LB*cos(qA-qB)))

   (55) StaticsViaPE[2] = -D( PotentialEnergy, qB )
-> (56) StaticsViaPE[2] = -LB*(mQ*g*sin(qB)+k*LA*sin(qA-qB)*SpringStretch/sqrt(LA^2
        +LB^2+2*LA*LB*cos(qA-qB)))

   (57) %----------------------------------------------------------
   (58) %   Optional: Statics with Lagrange's method.
   (59) SetGeneralizedCoordinate( qA, qB )
   (60) LagrangeStatics = System.GetStaticsLagrange( systemPotential = PotentialEnergy )
-> (61) LagrangeStatics[1] = -LA*((mP+mQ)*g*sin(qA)-k*LB*sin(qA-qB)*SpringStre
        tch/sqrt(LA^2+LB^2+2*LA*LB*cos(qA-qB)))

-> (62) LagrangeStatics[2] = -LB*(mQ*g*sin(qB)+k*LA*sin(qA-qB)*SpringStretch/
        sqrt(LA^2+LB^2+2*LA*LB*cos(qA-qB)))

   (63) %----------------------------------------------------------
   (64) %       Solve nonlinear equations (requires a guess).
   (65) Solve( Statics = 0,  qA = -30 deg,  qB = 30 deg )
-> (66) qA = -0.8761486       %  or  qA = -50.19962 deg.
-> (67) qB = 0.6140832       %  or  qB = 35.18437 deg.

   (68) %----------------------------------------------------------