MGFourBarStaticsKaneLagrangeAugmented.html  (MotionGenesis input/output).
   (1) % MotionGenesis file:  MGFourBarStaticsKaneLagrangeAugmented.txt
   (2) % Copyright (c) 2018 Motion Genesis LLC.  All rights reserved.
   (3) %---------------------------------------------------
   (4) %     Physical objects.
   (5) NewtonianFrame  N            % Earth.
   (6) RigidBody       A, B, C      % Links in 4-bar.
   (7) Point           CB( C )      % Point of C connecting to B.
   (8) Point           BC(B)        % Point of B connecting to C.
   (9) %---------------------------------------------------
   (10) %     Mathematical declarations.
   (11) Constant  LA = 1 m,  LB = 2 m,  LC = 2 m,  LN = 1 m
   (12) Constant  g = 9.81 m/s^2     % Earth's gravity.
   (13) Constant  H = 200 Newtons    % Horizontal force.
   (14) Variable  qA', qB', qC'      % 4-bar angles.
   (15) Variable  Fx, Fy             % Constraint forces.
   (16) SetGeneralizedSpeed( qA', qB', qC' )
   (17) %---------------------------------------------------
   (18) A.SetMass( mA = 10 kg )
   (19) B.SetMass( mB = 20 kg )
   (20) C.SetMass( mC = 20 kg )
   (21) %---------------------------------------------------
   (22) %     Rotational kinematics.
   (23) A.RotateZ( N, qA )
-> (24) A_N = [cos(qA), sin(qA), 0;  -sin(qA), cos(qA), 0;  0, 0, 1]
-> (25) w_A_N> = qA'*Az>

   (26) B.RotateZ( N, qB )
-> (27) B_N = [cos(qB), sin(qB), 0;  -sin(qB), cos(qB), 0;  0, 0, 1]
-> (28) w_B_N> = qB'*Bz>

   (29) C.RotateZ( N, qC )
-> (30) C_N = [cos(qC), sin(qC), 0;  -sin(qC), cos(qC), 0;  0, 0, 1]
-> (31) w_C_N> = qC'*Cz>

   (32) %---------------------------------------------------
   (33) %     Translational kinematics.
   (34) Bo.SetPositionVelocity(  No,  LA*Ax> )
-> (35) p_No_Bo> = LA*Ax>
-> (36) v_Bo_N> = LA*qA'*Ay>

   (37) BC.SetPositionVelocity(  Bo,  LB*Bx> )
-> (38) p_Bo_BC> = LB*Bx>
-> (39) v_BC_N> = LA*qA'*Ay> + LB*qB'*By>

   (40) CB.SetPositionVelocity(  No,  LN*Ny> + LC*Cx> )
-> (41) p_No_CB> = LC*Cx> + LN*Ny>
-> (42) v_CB_N> = LC*qC'*Cy>

   (43) %---------------------------------------------------
   (44) %     Relevant forces - replace gravity forces with equivalent set.
   (45) Bo.AddForce( 1/2*( A.GetMass()*g*Nx> + B.GetMass()*g*Nx> ) )
-> (46) Force_Bo> = 0.5*(mA+mB)*g*Nx>

   (47) CB.AddForce( 1/2*( B.GetMass()*g*Nx> + C.GetMass()*g*Nx> ) )
-> (48) Force_CB> = 0.5*(mB+mC)*g*Nx>

   (49) CB.AddForce( H*Ny> )      % Horizontal force on CB
-> (50) Force_CB> = 0.5*(mB+mC)*g*Nx> + H*Ny>

   (51) CB.AddForce( BC,  Fx*Nx> + Fy*Ny> )
-> (52) Force_CB_BC> = Fx*Nx> + Fy*Ny>

   (53) %---------------------------------------------------
   (54) %     Statics by setting generalized forces to 0.
   (55) %     Simplify each statics equation.
   (56) Statics = System.GetStaticsKane()
-> (57) Statics[1] = 0.5*LA*(2*Fx*sin(qA)-2*Fy*cos(qA)-(mA+mB)*g*sin(qA))
-> (58) Statics[2] = LB*(Fx*sin(qB)-Fy*cos(qB))
-> (59) Statics[3] = 0.5*LC*(2*H*cos(qC)+2*Fy*cos(qC)-2*Fx*sin(qC)-(mB+mC)*g*
        sin(qC))

   (60) Statics[1] /= LA
-> (61) Statics[1] = Fx*sin(qA) - Fy*cos(qA) - 0.5*(mA+mB)*g*sin(qA)

   (62) Statics[2] /= LB
-> (63) Statics[2] = Fx*sin(qB) - Fy*cos(qB)

   (64) Statics[3] /= LC
-> (65) Statics[3] = H*cos(qC) + Fy*cos(qC) - Fx*sin(qC) - 0.5*(mB+mC)*g*sin(qC)

   (66) %---------------------------------------------------
   (67) %     Configuration (loop) constraint.
   (68) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny>
-> (69) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny>

   (70) Loop[1] = Dot( Loop>, Nx> )
-> (71) Loop[1] = LA*cos(qA) + LB*cos(qB) - LC*cos(qC)

   (72) Loop[2] = Dot( Loop>, Ny> )
-> (73) Loop[2] = LA*sin(qA) + LB*sin(qB) - LN - LC*sin(qC)

   (74) %---------------------------------------------------
   (75) %     Solve nonlinear equations for static equilibrium.
   (76) Solve( [Statics; Loop] = 0,  qA = 30 deg,  qB = 60 deg,  qC = 30 deg, Fx = 0 N, Fy = 0 N )
-> (77) qA = 0.3488373       %  or  qA = 19.98691 deg.
-> (78) qB = 1.250738       %  or  qB = 71.66202 deg.
-> (79) qC = 0.6688954       %  or  qC = 38.32489 deg.
-> (80) Fx = -20.17117
-> (81) Fy = -60.85662

   (82) %---------------------------------------------------
Saved by Motion Genesis LLC.   Portions copyright (c) 2009-2018 Motion Genesis LLC. Rights reserved. Only for use with MotionGenesis.