MGFourBarStaticsKaneLagrangeAugmented.html  (MotionGenesis input/output).
   (1) % MotionGenesis file:  MGFourBarStaticsKaneLagrangeAugmented.txt
   (2) % Copyright (c) 2018 Motion Genesis LLC.  All rights reserved.
   (3) %--------------------------------------------------------------------
   (4) NewtonianFrame  N                 % Ground link.
   (5) RigidBody       A, B, C           % Crank, coupler, rocker links.
   (6) Point           BC( B )           % Point of B connected to C.
   (7) Point           CB( C )           % Point of C connected to B.
   (8) %--------------------------------------------------------------------
   (9) Constant   LN = 1 m,  LA = 1 m    % Length of ground link, crank link.
   (10) Constant   LB = 2 m,  LC = 2 m    % Length of coupler link, rocker link.
   (11) Constant   g = 9.81 m/s^2         % Earth's gravitational acceleration.
   (12) Constant   H = 200 Newtons        % Horizontal force at point CB.
   (13) Variable   qA',  qB',  qC'        % Link angles (relative to ground).
   (14) Variable   FCx, FCy               % Contact forces on C from B.
   (15) SetGeneralizedSpeed( qA', qB', qC' )
   (16) %--------------------------------------------------------------------
   (17) A.SetMass( mA = 10 kg )
   (18) B.SetMass( mB = 20 kg )
   (19) C.SetMass( mC = 20 kg )
   (20) %--------------------------------------------------------------------
   (21) %   Rotational kinematics.
   (22) A.RotateZ( N,  qA )
-> (23) A_N = [cos(qA), sin(qA), 0;  -sin(qA), cos(qA), 0;  0, 0, 1]
-> (24) w_A_N> = qA'*Az>

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

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

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

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

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

   (42) %--------------------------------------------------------------------
   (43) %   Add relevant forces (replace gravity forces with equivalent set).
   (44) Bo.AddForce( 0.5*(mA+mB)*g*Nx> )
-> (45) Force_Bo> = 0.5*(mA+mB)*g*Nx>

   (46) CB.AddForce( 0.5*(mB+mC)*g*Nx> + H*Ny> )
-> (47) Force_CB> = 0.5*(mB+mC)*g*Nx> + H*Ny>

   (48) CB.AddForce( BC,  FCx*Nx> + FCy*Ny> )
-> (49) Force_CB_BC> = FCx*Nx> + FCy*Ny>

   (50) %--------------------------------------------------------------------
   (51) %   Statics equations by setting generalized forces to 0 (subsequently simplify)
   (52) Statics = System.GetStaticsKane()
-> (53) Statics[1] = 0.5*LA*(2*FCx*sin(qA)-2*FCy*cos(qA)-(mA+mB)*g*sin(qA))
-> (54) Statics[2] = LB*(FCx*sin(qB)-FCy*cos(qB))
-> (55) Statics[3] = 0.5*LC*(2*H*cos(qC)+2*FCy*cos(qC)-2*FCx*sin(qC)-(mB+mC)*g*
        sin(qC))

   (56) Statics[1] /= LA
-> (57) Statics[1] = FCx*sin(qA) - FCy*cos(qA) - 0.5*(mA+mB)*g*sin(qA)

   (58) Statics[2] /= LB
-> (59) Statics[2] = FCx*sin(qB) - FCy*cos(qB)

   (60) Statics[3] /= LC
-> (61) Statics[3] = H*cos(qC) + FCy*cos(qC) - FCx*sin(qC) - 0.5*(mB+mC)*g*sin(qC)

   (62) %--------------------------------------------------------------------
   (63) %   Loop (configuration) constraint.
   (64) Loop> = LA*Ax>  +  LB*Bx>  -  LC*Cx>  -  LN*Ny>
-> (65) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny>

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

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

   (70) %--------------------------------------------------------------------
   (71) %   Solve statics with constraints (nonlinear algebraic equations need a guess for its solution).
   (72) Solve( [Statics; Loop] = 0,  qA = 30 deg,  qB = 60 deg,  qC = 30 deg, FCx = 0 N, FCy = 0 N )
-> (73) qA = 0.3488373       %  or  qA = 19.98691 deg.
-> (74) qB = 1.250738       %  or  qB = 71.66202 deg.
-> (75) qC = 0.6688954       %  or  qC = 38.32489 deg.
-> (76) FCx = -20.17117
-> (77) FCy = -60.85662

   (78) %--------------------------------------------------------------------
Saved by Motion Genesis LLC.   Command names and syntax: Copyright (c) 2009-2018 Motion Genesis LLC. All rights reserved.