MGFourBarStaticsKaneEmbedded.html  (MotionGenesis input/output).
   (1) % MotionGenesis file:  MGFourBarStaticsKaneEmbedded.txt
   (2) % Copyright (c) 2009 Motion Genesis LLC.  All rights reserved.
   (3) %---------------------------------------------------
   (4) %       Physical objects.
   (5) NewtonianFrame  N
   (6) RigidBody       A, B, C
   (7) Point           CB(C)
   (8) %---------------------------------------------------
   (9) %       Mathematical declarations.
   (10) Constant   LA = 1 m,  LB = 2 m,  LC = 2 m,  LN = 1 m
   (11) Constant   g = 9.81 m/s^2          % Gravity
   (12) Constant   H = 200 Newtons         % Horizontal force
   (13) Variable   qA', qB', qC'           % Angles
   (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) %       Forces - replaces gravity forces with equivalent set
   (40) Bo.AddForce( 1/2*( A.GetMass()*g*Nx> + B.GetMass()*g*Nx> ) )
-> (41) Force_Bo> = 0.5*(mA+mB)*g*Nx>

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

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

   (46) %---------------------------------------------------
   (47) %       Configuration constraints and motion constraints.
   (48) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny> 
-> (49) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny>

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

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

   (54) MotionConstraint = Dt( Loop )
-> (55) MotionConstraint[1] = LC*sin(qC)*qC' - LA*sin(qA)*qA' - LB*sin(qB)*qB'
-> (56) MotionConstraint[2] = LA*cos(qA)*qA' + LB*cos(qB)*qB' - LC*cos(qC)*qC'

   (57) Solve( MotionConstraint,  qB', qC' )
-> (58) qB' = -LA*sin(qA-qC)*qA'/(LB*sin(qB-qC))
-> (59) qC' = -LA*sin(qA-qB)*qA'/(LC*sin(qB-qC))

   (60) %---------------------------------------------------
   (61) %       Equations of motion
   (62) Zero = System.GetStaticsKane()
-> (63) Zero[1] = -0.5*LA*((mA+mB)*g*sin(qA)+sin(qA-qB)*(2*H*cos(qC)-(mB+mC)*g*
        sin(qC))/sin(qB-qC))

   (64) Zero[2] = Loop[1]
-> (65) Zero[2] = LA*cos(qA) + LB*cos(qB) - LC*cos(qC)

   (66) Zero[3] = Loop[2]
-> (67) Zero[3] = LA*sin(qA) + LB*sin(qB) - LN - LC*sin(qC)

   (68) %---------------------------------------------------
   (69) %       Solve nonlinear equations
   (70) Solve( Zero,  qA = 30 deg,  qB = 60 deg,  qC = 30 deg )
-> (71) qA = 0.3488373       %  or  qA = 19.98691 deg.
-> (72) qB = 1.250738       %  or  qB = 71.66202 deg.
-> (73) qC = 0.6688954       %  or  qC = 38.32489 deg.

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