MGFourBarForcesAndMotion.html  (MotionGenesis input/output).
   (1) % MotionGenesis file:  MGFourBarForcesAndMotion.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''          % , vx'
   (15) Variable   FAx, FAy,  FCx, FCy         % Contact forces
   (16) SetGeneralizedSpeed( qA', qB', qC' )   % , vx
   (17) SetAutoZee( ON )                       % Efficient calculations.
   (18) SetNoZeeSymbol( FAx, FAy, FCx, FCy )
   (19) %---------------------------------------------------------------
   (20) A.SetMassInertia( mA = 10 kg,  0,  IA = mA*LA^2/12,  IA  )
-> (21) IA = 0.08333333*mA*LA^2

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

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

   (26) %---------------------------------------------------------------
   (27) %       Rotational kinematics
   (28) A.RotateZ( N, qA )
-> (29) z1 = cos(qA)
-> (30) z2 = sin(qA)
-> (31) A_N = [z1, z2, 0;  -z2, z1, 0;  0, 0, 1]
-> (32) w_A_N> = qA'*Az>
-> (33) alf_A_N> = qA''*Az>

   (34) B.RotateZ( N, qB )
-> (35) z3 = cos(qB)
-> (36) z4 = sin(qB)
-> (37) B_N = [z3, z4, 0;  -z4, z3, 0;  0, 0, 1]
-> (38) w_B_N> = qB'*Bz>
-> (39) alf_B_N> = qB''*Bz>

   (40) C.RotateZ( N, qC )
-> (41) z5 = cos(qC)
-> (42) z6 = sin(qC)
-> (43) C_N = [z5, z6, 0;  -z6, z5, 0;  0, 0, 1]
-> (44) w_C_N> = qC'*Cz>
-> (45) alf_C_N> = qC''*Cz>

   (46) %---------------------------------------------------------------
   (47) %       Translational kinematics
   (48) Ao.SetVelocityAcceleration( N, 0> )    % vx*Nx>
-> (49) v_Ao_N> = 0>
-> (50) a_Ao_N> = 0>

   (51) Acm.Translate(  Ao,  0.5*LA*Ax> )
-> (52) p_Ao_Acm> = 0.5*LA*Ax>
-> (53) v_Acm_N> = 0.5*LA*qA'*Ay>
-> (54) z7 = LA*qA'
-> (55) z8 = qA'*z7
-> (56) a_Acm_N> = -0.5*z8*Ax> + 0.5*LA*qA''*Ay>

   (57) Bo.Translate(   Ao,      LA*Ax> )
-> (58) p_Ao_Bo> = LA*Ax>
-> (59) v_Bo_N> = LA*qA'*Ay>
-> (60) a_Bo_N> = -z8*Ax> + LA*qA''*Ay>

   (61) Bcm.Translate(  Bo,  0.5*LB*Bx> )
-> (62) p_Bo_Bcm> = 0.5*LB*Bx>
-> (63) z9 = z1*z3 + z2*z4
-> (64) z10 = z2*z3 - z1*z4
-> (65) z11 = z1*z4 - z2*z3
-> (66) A_B = [z9, z10, 0;  z11, z9, 0;  0, 0, 1]
-> (67) z12 = LA*z11
-> (68) z13 = LA*z9
-> (69) v_Bcm_N> = z12*qA'*Bx> + (z13*qA'+0.5*LB*qB')*By>
-> (70) z14 = z9*z8
-> (71) z15 = z10*z8
-> (72) z16 = LB*qB'
-> (73) z17 = -z14 - 0.5*qB'*z16
-> (74) a_Bcm_N> = (z17+z12*qA'')*Bx> + (z13*qA''+0.5*LB*qB''-z15)*By>

   (75) BC.Translate(   Bo,      LB*Bx> )
-> (76) p_Bo_BC> = LB*Bx>
-> (77) v_BC_N> = z12*qA'*Bx> + (LB*qB'+z13*qA')*By>
-> (78) z18 = -z14 - qB'*z16
-> (79) a_BC_N> = (z18+z12*qA'')*Bx> + (LB*qB''+z13*qA''-z15)*By>

   (80) Co.Translate(   No,      LN*Ny> )
-> (81) p_No_Co> = LN*Ny>
-> (82) v_Co_N> = 0>
-> (83) a_Co_N> = 0>

   (84) Ccm.Translate(  Co,  0.5*LC*Cx> )
-> (85) p_Co_Ccm> = 0.5*LC*Cx>
-> (86) v_Ccm_N> = 0.5*LC*qC'*Cy>
-> (87) z19 = LC*qC'
-> (88) z20 = qC'*z19
-> (89) a_Ccm_N> = -0.5*z20*Cx> + 0.5*LC*qC''*Cy>

   (90) CB.Translate(   Co,      LC*Cx> )
-> (91) p_Co_CB> = LC*Cx>
-> (92) v_CB_N> = LC*qC'*Cy>
-> (93) a_CB_N> = -z20*Cx> + LC*qC''*Cy>

   (94) %---------------------------------------------------------------
   (95) %       Forces
   (96) System.AddForceGravity( g*Nx> )
-> (97) z21 = mA*g
-> (98) Force_Acm> = z21*Nx>
-> (99) z22 = mB*g
-> (100) Force_Bcm> = z22*Nx>
-> (101) z23 = mC*g
-> (102) Force_Ccm> = z23*Nx>

   (103) CB.AddForce( H*Ny> )
-> (104) Force_CB> = H*Ny>

   (105) Ao.AddForce( FAx*Nx> + FAy*Ny> )
-> (106) Force_Ao> = FAx*Nx> + FAy*Ny>

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

   (109) %---------------------------------------------------------------
   (110) %       Configuration constraints and time-derivatives 
   (111) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny> 
-> (112) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny>

   (113) Loop[1] = Dot( Loop>, Nx> )
-> (114) Loop[1] = LA*z1 + LB*z3 - LC*z5

   (115) Loop[2] = Dot( Loop>, Ny> )
-> (116) Loop[2] = LA*z2 + LB*z4 - LN - LC*z6

   (117) %--------------------------------------------------------------------
   (118) %       Solve constraints with given constants and initial value of qA.
   (119) Input  qA = 30 deg,  qA' = 0 rad/sec
   (120) SolveSetInputDt( Loop,  qB = 60 deg,  qC = 20 deg ) 
-> (121) z24 = LB*z4
-> (122) z25 = LC*z6
-> (123) z26 = LC*z5 - LA*z1 - LB*z3
-> (124) z27 = LB*z3
-> (125) z28 = LC*z5
-> (126) z29 = LN + LC*z6 - LA*z2 - LB*z4
-> (127) z30 = LA*z2
-> (128) z31 = LA*z1

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


-> (129) z32 = z24*z28 - z25*z27
-> (130) z33 = (z25*z31-z28*z30)/z32
-> (131) z34 = (z24*z31-z27*z30)/z32
-> (132) z35 = LA*z1*qA'
-> (133) z36 = qA'*z35
-> (134) z37 = z28*qC'^2 - z27*qB'^2
-> (135) z38 = z36 - z37
-> (136) z39 = LA*z2*qA'
-> (137) z40 = qA'*z39
-> (138) z41 = z25*qC'^2 - z24*qB'^2
-> (139) z42 = z40 - z41
-> (140) qB' = z33*qA'
-> (141) qC' = z34*qA'
-> (142) z43 = (z25*z42+z28*z38)/z32
-> (143) z44 = (z24*z42+z27*z38)/z32
-> (144) qB'' = z33*qA'' - z43
-> (145) qC'' = z34*qA'' - z44

   (146) %--------------------------------------------------------------------
   (147) %       Equations of motion - with Kane's method.
   (148) Zero = System.GetDynamicsKane()
-> (149) z45 = 0.25*mA*LA^2 + IA
-> (150) z46 = z45 + mB*(z12^2+z13^2)
-> (151) z47 = mB*LB
-> (152) z48 = z47*z13
-> (153) z49 = mB*(z12*z17-z13*z15)
-> (154) z50 = 0.25*mB*LB^2 + IB
-> (155) z51 = z47*z15
-> (156) z52 = 0.25*mC*LC^2 + IC
-> (157) z53 = z4*z13 - z3*z12
-> (158) z54 = -z3*z13 - z4*z12
-> (159) z55 = LA*z21
-> (160) z56 = z22*(z3*z12-z4*z13) - 0.5*z55*z2
-> (161) z57 = LB*z22
-> (162) z58 = z57*z4
-> (163) z59 = LC*(z23*z6-2*H*z5)
-> (164) z60 = z49 - z56
-> (165) z61 = 0.5*z58 - 0.5*z51
-> (166) Zero[1] = z60 + z46*qA'' + 0.5*z48*qB'' - FCx*z53 - FCy*z54
-> (167) Zero[2] = FCy*z27 + z61 + z50*qB'' + 0.5*z48*qA'' - FCx*z24
-> (168) Zero[3] = 0.5*z59 + FCx*z25 + z52*qC'' - FCy*z28

   (169) %--------------------------------------------------------------------
   (170) %       Integration parameters and quantities to be output from ODE.
   (171) Input  tFinal = 7 sec,  tStep = 0.02 sec,  absError = 1.0E-07
   (172) ECheck = System.GetEnergyCheckKane() 
-> (173) z62 = z24*qB' + z53*qA' - z25*qC'
-> (174) z63 = z28*qC' + z54*qA' - z27*qB'
-> (175) z64 = z56*qA' - 0.5*z58*qB' - 0.5*z59*qC'
-> (176) WCheck1' = -z64 - FCx*z62 - FCy*z63
-> (177) z65 = mA*LA^2
-> (178) z66 = mC*LC^2
-> (179) z67 = 0.125*z65*qA'^2 + 0.125*z66*qC'^2 + 0.5*IA*qA'^2 + 0.5*IB*qB'^2
         + 0.5*IC*qC'^2 + 0.125*mB*(4*z12^2*qA'^2+(LB*qB'+2*z13*qA')^2)

-> (180) ECheck = WCheck1 + z67

   (181) Output  t sec,  qA deg,  qB deg,  qC deg,  Loop[1] m,  Loop[2] m,  ECheck Joules
   (182) Output  t sec,  FCx Newtons,  FCy Newtons      %, FAx Newtons
   (183) %--------------------------------------------------------------------
   (184) %       Numerical solve the ODEs for qA''  (and perhaps FCx, FCy, FAx).
   (185) ODE( Zero, qA'', FCx, FCy )  MGFourBarForcesAndMotion
-> (186) z68 = z46 + 0.5*z33*z48
-> (187) z69 = 0.5*z48*z43 - z60
-> (188) z70 = 0.5*z48 + z50*z33
-> (189) z71 = z50*z43 - z61
-> (190) z72 = z52*z34
-> (191) z73 = z52*z44 - 0.5*z59

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