MGFourBarStaticsKaneEmbedded.html  (MotionGenesis input/output).
(1) % MotionGenesis file:  MGFourBarStaticsKaneEmbedded.txt
(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) SetGeneralizedSpeed( qA' )
(15) %--------------------------------------------------------------------
(16) A.SetMass( mA = 10 kg )
(17) B.SetMass( mB = 20 kg )
(18) C.SetMass( mC = 20 kg )
(19) %--------------------------------------------------------------------
(20) %   Rotational kinematics.
(21) A.RotateZ( N,  qA )
-> (22) A_N = [cos(qA), sin(qA), 0;  -sin(qA), cos(qA), 0;  0, 0, 1]
-> (23) w_A_N> = qA'*Az>

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

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

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

(35) CB.SetPositionVelocity(  No,  LN*Ny> + LC*Cx> )
-> (36) p_No_CB> = LC*Cx> + LN*Ny>
-> (37) v_CB_N> = LC*qC'*Cy>

(38) %--------------------------------------------------------------------
(39) %   Add relevant forces (replace gravity forces with equivalent set).
-> (41) Force_Bo> = 0.5*(mA+mB)*g*Nx>

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

(44) %--------------------------------------------------------------------
(45) %   Loop (configuration) constraint.
(46) Loop> = LA*Ax>  +  LB*Bx>  -  LC*Cx>  -  LN*Ny>
-> (47) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny>

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

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

(52) %--------------------------------------------------------------------
(53) %   For Kane's embedded statics method, solve qB', qC' in terms of qA'.
(54) Solve( Dt(Loop) = 0,   qB', qC' )
-> (55) qB' = -LA*sin(qA-qC)*qA'/(LB*sin(qB-qC))
-> (56) qC' = -LA*sin(qA-qB)*qA'/(LC*sin(qB-qC))

(57) %--------------------------------------------------------------------
(58) %   Statics equation by setting generalized force to 0.
(59) Statics = System.GetStaticsKane()
-> (60) Statics[1] = -0.5*LA*((mA+mB)*g*sin(qA)+sin(qA-qB)*(2*H*cos(qC)-(mB+mC)
*g*sin(qC))/sin(qB-qC))

(61) %--------------------------------------------------------------------
(62) %   Solve statics with constraints (nonlinear algebraic equations need a guess for its solution).
(63) Solve( [Statics; Loop] = 0,   qA = 30 deg,  qB = 60 deg,  qC = 30 deg )
-> (64) qA = 0.3488373       %  or  qA = 19.98691 deg.
-> (65) qB = 1.250738       %  or  qB = 71.66202 deg.
-> (66) qC = 0.6688954       %  or  qC = 38.32489 deg.

(67) %--------------------------------------------------------------------