MGFourBarDynamicsWithKaneEmbedded.html  (MotionGenesis input/output).
   (1) % MotionGenesis file:  MGFourBarDynamicsWithKaneEmbedded.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           BC(B), 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) Specified  H = 200                 % Horizontal force
-> (13) H = 200

   (14) Variable   qA'', qB'', qC''        % Angles
   (15) SetGeneralizedSpeed( qA' ) 
   (16) %--------------------------------------------------------------------
   (17) A.SetMassInertia( mA = 10 kg,  0,  IA = mA*LA^2/12,  IA  )
-> (18) IA = 0.08333333*mA*LA^2

   (19) B.SetMassInertia( mB = 20 kg,  0,  IB = mB*LB^2/12,  IB  )
-> (20) IB = 0.08333333*mB*LB^2

   (21) C.SetMassInertia( mC = 20 kg,  0,  IC = mC*LC^2/12,  IC  )
-> (22) IC = 0.08333333*mC*LC^2

   (23) %--------------------------------------------------------------------
   (24) %       Rotational kinematics
   (25) A.RotateZ( N, qA )
-> (26) A_N = [cos(qA), sin(qA), 0;  -sin(qA), cos(qA), 0;  0, 0, 1]
-> (27) w_A_N> = qA'*Az>
-> (28) alf_A_N> = qA''*Az>

   (29) B.RotateZ( N, qB )
-> (30) B_N = [cos(qB), sin(qB), 0;  -sin(qB), cos(qB), 0;  0, 0, 1]
-> (31) w_B_N> = qB'*Bz>
-> (32) alf_B_N> = qB''*Bz>

   (33) C.RotateZ( N, qC )
-> (34) C_N = [cos(qC), sin(qC), 0;  -sin(qC), cos(qC), 0;  0, 0, 1]
-> (35) w_C_N> = qC'*Cz>
-> (36) alf_C_N> = qC''*Cz>

   (37) %--------------------------------------------------------------------
   (38) %       Translational kinematics
   (39) Ao.Translate(   No,          0> )
-> (40) p_No_Ao> = 0>
-> (41) v_Ao_N> = 0>
-> (42) a_Ao_N> = 0>

   (43) Acm.Translate(  Ao,  0.5*LA*Ax> )
-> (44) p_Ao_Acm> = 0.5*LA*Ax>
-> (45) v_Acm_N> = 0.5*LA*qA'*Ay>
-> (46) a_Acm_N> = -0.5*LA*qA'^2*Ax> + 0.5*LA*qA''*Ay>

   (47) Bo.Translate(   Ao,      LA*Ax> )
-> (48) p_Ao_Bo> = LA*Ax>
-> (49) v_Bo_N> = LA*qA'*Ay>
-> (50) a_Bo_N> = -LA*qA'^2*Ax> + LA*qA''*Ay>

   (51) Bcm.Translate(  Bo,  0.5*LB*Bx> )
-> (52) p_Bo_Bcm> = 0.5*LB*Bx>
-> (53) v_Bcm_N> = LA*qA'*Ay> + 0.5*LB*qB'*By>
-> (54) a_Bcm_N> = -LA*qA'^2*Ax> + LA*qA''*Ay> - 0.5*LB*qB'^2*Bx> + 0.5*LB*qB''*By>

   (55) BC.Translate(   Bo,      LB*Bx> )
-> (56) p_Bo_BC> = LB*Bx>
-> (57) v_BC_N> = LA*qA'*Ay> + LB*qB'*By>
-> (58) a_BC_N> = -LA*qA'^2*Ax> + LA*qA''*Ay> - LB*qB'^2*Bx> + LB*qB''*By>

   (59) Co.Translate(   No,      LN*Ny> )
-> (60) p_No_Co> = LN*Ny>
-> (61) v_Co_N> = 0>
-> (62) a_Co_N> = 0>

   (63) Ccm.Translate(  Co,  0.5*LC*Cx> )
-> (64) p_Co_Ccm> = 0.5*LC*Cx>
-> (65) v_Ccm_N> = 0.5*LC*qC'*Cy>
-> (66) a_Ccm_N> = -0.5*LC*qC'^2*Cx> + 0.5*LC*qC''*Cy>

   (67) CB.Translate(   Co,      LC*Cx> )
-> (68) p_Co_CB> = LC*Cx>
-> (69) v_CB_N> = LC*qC'*Cy>
-> (70) a_CB_N> = -LC*qC'^2*Cx> + LC*qC''*Cy>

   (71) %--------------------------------------------------------------------
   (72) %       Forces - replaces gravity forces with equivalent set.
   (73) Bo.AddForce( 0.5*(mA+mB)*g*Nx> )
-> (74) Force_Bo> = 0.5*(mA+mB)*g*Nx>

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

   (77) %--------------------------------------------------------------------
   (78) %       Configuration constraints and time-derivatives 
   (79) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny> 
-> (80) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny>

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

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

   (85) %--------------------------------------------------------------------
   (86) %       Solve constraints with given constants and initial value of qA.
   (87) Input  qA = 30 deg,  qA' = 0 rad/sec
   (88) SolveSetInputDt( Loop,  qB = 60 deg,  qC = 20 deg ) 

->   %  INPUT has been assigned as follows:
->   %   qB                        74.47751218592991       deg
->   %   qC                        45.52248781407007       deg

-> (89) qB' = -LA*sin(qA-qC)*qA'/(LB*sin(qB-qC))
-> (90) qC' = -LA*sin(qA-qB)*qA'/(LC*sin(qB-qC))
-> (91) qB'' = (LC*qC'^2-cos(qC)*(LB*cos(qB)*qB'^2+LA*(cos(qA)*qA'^2+sin(qA)*
        qA''))-sin(qC)*(LB*sin(qB)*qB'^2+LA*(sin(qA)*qA'^2-cos(qA)*qA'')))/(LB*
        sin(qB-qC))

-> (92) qC'' = -(LB*qB'^2-cos(qB)*(LC*cos(qC)*qC'^2-LA*(cos(qA)*qA'^2+sin(qA)*
        qA''))-sin(qB)*(LC*sin(qC)*qC'^2-LA*(sin(qA)*qA'^2-cos(qA)*qA'')))/(LC*
        sin(qB-qC))

   (93) %--------------------------------------------------------------------
   (94) %       Equations of motion - with Kane's method.
   (95) Zero = System.GetDynamicsKane()
-> (96) Zero[1] = 0.25*(mA*LA^2+4*IA+4*LA^2*IB*sin(qA-qC)^2/(LB^2*sin(qB-qC)^2)
        +LA^2*(mC+4*IC/LC^2)*sin(qA-qB)^2/sin(qB-qC)^2+mB*LA^2*(4+sin(qA-qC)^2/
        sin(qB-qC)^2-4*sin(qA-qC)*cos(qA-qB)/sin(qB-qC)))*qA'' - 0.25*LA*(4*IB*
        sin(qA-qC)*(LC*qC'^2-sin(qC)*(LA*sin(qA)*qA'^2+LB*sin(qB)*qB'^2)-cos(
        qC)*(LA*cos(qA)*qA'^2+LB*cos(qB)*qB'^2))/(LB^2*sin(qB-qC)^2)-2*(mA+mB)*
        g*sin(qA)-2*sin(qA-qB)*(2*H*cos(qC)-(mB+mC)*g*sin(qC))/sin(qB-qC)-mC*
        sin(qA-qB)*(LB*qB'^2+sin(qB)*(LA*sin(qA)*qA'^2-LC*sin(qC)*qC'^2)+cos(
        qB)*(LA*cos(qA)*qA'^2-LC*cos(qC)*qC'^2))/sin(qB-qC)^2-4*IC*sin(qA-qB)*(
        LB*qB'^2+sin(qB)*(LA*sin(qA)*qA'^2-LC*sin(qC)*qC'^2)+cos(qB)*(LA*cos(
        qA)*qA'^2-LC*cos(qC)*qC'^2))/(LC^2*sin(qB-qC)^2)-mB*(2*LB*sin(qA-qB)*qB'^2
        +(2*cos(qA-qB)*(LC*qC'^2-sin(qC)*(LA*sin(qA)*qA'^2+LB*sin(qB)*qB'^2)-
        cos(qC)*(LA*cos(qA)*qA'^2+LB*cos(qB)*qB'^2))+sin(qA-qC)*(2*LA*sin(qA-
        qB)*qA'^2-(LC*qC'^2-sin(qC)*(LA*sin(qA)*qA'^2+LB*sin(qB)*qB'^2)-cos(qC)
        *(LA*cos(qA)*qA'^2+LB*cos(qB)*qB'^2))/sin(qB-qC)))/sin(qB-qC)))

   (97) Solve( Zero, qA'' )
-> (98) qA'' = LA*(4*IB*sin(qA-qC)*(LC*qC'^2-sin(qC)*(LA*sin(qA)*qA'^2+LB*sin(
        qB)*qB'^2)-cos(qC)*(LA*cos(qA)*qA'^2+LB*cos(qB)*qB'^2))/(LB^2*sin(qB-
        qC)^2)-2*(mA+mB)*g*sin(qA)-2*sin(qA-qB)*(2*H*cos(qC)-(mB+mC)*g*sin(qC))
        /sin(qB-qC)-mC*sin(qA-qB)*(LB*qB'^2+sin(qB)*(LA*sin(qA)*qA'^2-LC*sin(
        qC)*qC'^2)+cos(qB)*(LA*cos(qA)*qA'^2-LC*cos(qC)*qC'^2))/sin(qB-qC)^2-4*
        IC*sin(qA-qB)*(LB*qB'^2+sin(qB)*(LA*sin(qA)*qA'^2-LC*sin(qC)*qC'^2)+cos
        (qB)*(LA*cos(qA)*qA'^2-LC*cos(qC)*qC'^2))/(LC^2*sin(qB-qC)^2)-mB*(2*LB*
        sin(qA-qB)*qB'^2+(2*cos(qA-qB)*(LC*qC'^2-sin(qC)*(LA*sin(qA)*qA'^2+LB*
        sin(qB)*qB'^2)-cos(qC)*(LA*cos(qA)*qA'^2+LB*cos(qB)*qB'^2))+sin(qA-qC)*
        (2*LA*sin(qA-qB)*qA'^2-(LC*qC'^2-sin(qC)*(LA*sin(qA)*qA'^2+LB*sin(qB)*qB'^2)
        -cos(qC)*(LA*cos(qA)*qA'^2+LB*cos(qB)*qB'^2))/sin(qB-qC)))/sin(qB-qC)))
        /(mA*LA^2+4*IA+4*LA^2*IB*sin(qA-qC)^2/(LB^2*sin(qB-qC)^2)+LA^2*(mC+4*
        IC/LC^2)*sin(qA-qB)^2/sin(qB-qC)^2+mB*LA^2*(4+sin(qA-qC)^2/sin(qB-qC)^2
        -4*sin(qA-qC)*cos(qA-qB)/sin(qB-qC)))

   (99) %--------------------------------------------------------------------
   (100) %       Integration parameters and quantities to be output from ODE.
   (101) Input  tFinal = 7 sec,  tStep = 0.02 sec,  absError = 1.0E-07
   (102) OutputPlot  t sec,  qA deg,  qB deg,  qC deg
   (103) ODE() MGFourBarDynamicsWithKaneEmbedded

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