MGFourBarDynamicsFBD.html  (MotionGenesis input/output).
   (1) % MotionGenesis file: MGFourBarDynamicsFBD.txt
   (2) % Copyright (c) 2009 Motion Genesis LLC.  All rights reserved.
   (3) %--------------------------------------------------------------------
   (4) NewtonianFrame  N
   (5) RigidBody       A, B, C
   (6) Point           BC(B), CB(C)
   (7) %--------------------------------------------------------------------
   (8) Constant   LA = 1 m,  LB = 2 m,  LC = 2 m,  LN = 1 m
   (9) Constant   g = 9.81 m/s^2         % Earth's gravity.
   (10) Variable   qA'',  qB'',  qC''     % Link angles.
   (11) Specified  H = 200                % Horizontal force on CB.
-> (12) H = 200

   (13) Variable   FCx, FCy               % Contact forces on C from B.
   (14) %--------------------------------------------------------------------
   (15) A.SetMassInertia( mA = 10 kg,  0,  IA = mA*LA^2/12,  IA  )
-> (16) IA = 0.08333333*mA*LA^2

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

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

   (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) alf_A_N> = qA''*Az>

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

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

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

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

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

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

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

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

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

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

   (69) %--------------------------------------------------------------------
   (70) %       Add relevant forces.
   (71) System.AddForceGravity( g*Nx> )
-> (72) Force_Acm> = mA*g*Nx>
-> (73) Force_Bcm> = mB*g*Nx>
-> (74) Force_Ccm> = mC*g*Nx>

   (75) CB.AddForce( H*Ny> )
-> (76) Force_CB> = H*Ny>

   (77) CB.AddForce( BC,  FCx*Nx> + FCy*Ny> )   % "Cut" linkage at CB/BC
-> (78) Force_CB_BC> = FCx*Nx> + FCy*Ny>

   (79) %--------------------------------------------------------------------
   (80) %       Form equations of motion ("cut" linkage at CB/BC).
   (81) Dynamics[1] = Dot( Nz>,  System(A,B).GetDynamics(Ao) )
-> (82) Dynamics[1] = LA*FCy*cos(qA) + LB*FCy*cos(qB) + mB*g*LA*sin(qA) + 0.5*
        mA*g*LA*sin(qA) + 0.5*mB*g*LB*sin(qB) + 0.5*mB*LA*LB*sin(qA-qB)*qB'^2
        + IA*qA'' + IB*qB'' + mB*LA^2*qA'' + 0.25*mA*LA^2*qA'' + 0.25*mB*LB^2*qB''
        + 0.5*mB*LA*LB*cos(qA-qB)*qA'' + 0.5*mB*LA*LB*cos(qA-qB)*qB'' - LA*FCx*sin(qA)
        - LB*FCx*sin(qB) - 0.5*mB*LA*LB*sin(qA-qB)*qA'^2

   (83) Dynamics[2] = Dot( Nz>,            B.GetDynamics(Bo) )
-> (84) Dynamics[2] = IB*qB'' + 0.25*mB*LB^2*qB'' - 0.5*LB*(2*FCx*sin(qB)-2*FCy
        *cos(qB)-mB*g*sin(qB)) - 0.5*mB*LA*LB*(sin(qA-qB)*qA'^2-cos(qA-qB)*qA'')

   (85) Dynamics[3] = Dot( Nz>,            C.GetDynamics(Co) )
-> (86) Dynamics[3] = IC*qC'' + 0.25*mC*LC^2*qC'' - 0.5*LC*(2*H*cos(qC)+2*FCy*
        cos(qC)-2*FCx*sin(qC)-mC*g*sin(qC))

   (87) %--------------------------------------------------------------------
   (88) %       Configuration constraints and their time-derivatives.
   (89) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny> 
-> (90) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny>

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

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

   (95) LoopDt = Dt( Loop )
-> (96) LoopDt[1] = LC*sin(qC)*qC' - LA*sin(qA)*qA' - LB*sin(qB)*qB'
-> (97) LoopDt[2] = LA*cos(qA)*qA' + LB*cos(qB)*qB' - LC*cos(qC)*qC'

   (98) LoopDtDt = Dt( LoopDt )
-> (99) LoopDtDt[1] = LC*cos(qC)*qC'^2 + LC*sin(qC)*qC'' - LA*cos(qA)*qA'^2
        - LB*cos(qB)*qB'^2 - LA*sin(qA)*qA'' - LB*sin(qB)*qB''

-> (100) LoopDtDt[2] = LC*sin(qC)*qC'^2 + LA*cos(qA)*qA'' + LB*cos(qB)*qB''
         - LA*sin(qA)*qA'^2 - LB*sin(qB)*qB'^2 - LC*cos(qC)*qC''

   (101) %--------------------------------------------------------------------
   (102) %       Use the loop constraints to solve for initial values of qB, qC and qB',qC'
   (103) %       (results depend on constants and initial values of qA and qA').
   (104) Input  qA = 30 deg,  qA' = 0 rad/sec
   (105) SolveSetInput( Loop,    qB = 60 deg,      qC  = 20 deg ) 

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

   (106) SolveSetInput( LoopDt,  qB' = 0 rad/sec,  qC' = 0 rad/sec ) 

->    %  INPUT has been assigned as follows:
->    %   qB'                       0                       rad/sec
->    %   qC'                       0                       rad/sec

   (107) %--------------------------------------------------------------------
   (108) %       Integration parameters and quantities to be output from ODE.
   (109) Input  tFinal = 7 sec,  tStep = 0.02,  absError = 1.0E-07
   (110) Output  t sec,  qA deg,  qB deg,  qC deg,  FCx Newtons,  FCy  Newtons
   (111) %--------------------------------------------------------------------
   (112) %       Augment dynamics with constraints and solve nonlinear algebraic equations.
   (113) ODE( [Dynamics; LoopDtDt],  qA'', qB'', qC'', FCx, FCy ) MGFourBarDynamicsFBD

   (114) %--------------------------------------------------------------------
   (115) %       Save input together with program responses
Saved by Motion Genesis LLC.   Portions copyright (c) 2009-2017 Motion Genesis LLC. Rights reserved. Only for use with MotionGenesis.