MGFourBarForcesAndMotion.html  (MotionGenesis input/output).
   (1) % MotionGenesis file:  MGFourBarForcesAndMotion.txt
   (2) % Copyright (c) 2009 Motion Genesis LLC.  All rights reserved.
   (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) Specified  H = 200                % Horizontal force at point CB.
-> (13) H = 200

   (14) Specified  TA = 0                 % Motor torque on crank A from ground N.
-> (15) TA = 0

   (16) Variable   qA'',  qB'',  qC''          % , vx'
   (17) Variable   FAx, FAy,  FCx, FCy         % Contact forces
   (18) SetGeneralizedSpeed( qA', qB', qC' )   % , vx
   (19) SetAutoZee( ON )                       % Efficient calculations.
   (20) SetNoZeeSymbol( FAx, FAy, FCx, FCy )
   (21) %---------------------------------------------------------------
   (22) A.SetMassInertia( mA = 10 kg,  0,  IA = mA*LA^2/12,  IA  )
-> (23) IA = 0.08333333*mA*LA^2

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

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

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

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

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

   (48) %--------------------------------------------------------------------
   (49) %   Translational kinematics.
   (50) Ao.Translate(   No,          0> )  % Ao.SetVelocityAcceleration( N, vx*Nx> )
-> (51) p_No_Ao> = 0>
-> (52) v_Ao_N> = 0>
-> (53) a_Ao_N> = 0>

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

   (60) Bo.Translate(   Ao,      LA*Ax> )
-> (61) p_Ao_Bo> = LA*Ax>
-> (62) v_Bo_N> = LA*qA'*Ay>
-> (63) a_Bo_N> = -z8*Ax> + LA*qA''*Ay>

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

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

   (83) Co.Translate(   No,      LN*Ny> )
-> (84) p_No_Co> = LN*Ny>
-> (85) v_Co_N> = 0>
-> (86) a_Co_N> = 0>

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

   (93) CB.Translate(   Co,      LC*Cx> )
-> (94) p_Co_CB> = LC*Cx>
-> (95) v_CB_N> = LC*qC'*Cy>
-> (96) a_CB_N> = -z20*Cx> + LC*qC''*Cy>

   (97) %--------------------------------------------------------------------
   (98) %   Add relevant forces and torques.
   (99) System.AddForceGravity(  g * Nx>  )
-> (100) z21 = mA*g
-> (101) Force_Acm> = z21*Nx>
-> (102) z22 = mB*g
-> (103) Force_Bcm> = z22*Nx>
-> (104) z23 = mC*g
-> (105) Force_Ccm> = z23*Nx>

   (106) CB.AddForce(  H * Ny>  )
-> (107) Force_CB> = H*Ny>

   (108) Ao.AddForce( FAx*Nx> + FAy*Ny> )
-> (109) Force_Ao> = FAx*Nx> + FAy*Ny>

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

   (112) A.AddTorque( N,  TA * Az> )
-> (113) Torque_A_N> = TA*Az>

   (114) %--------------------------------------------------------------------
   (115) %   Configuration (loop) constraints and their time-derivatives.
   (116) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny>
-> (117) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny>

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

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

   (122) %--------------------------------------------------------------------
   (123) %   Use the loop constraints to solve for initial values of qB, qC and qB',qC'
   (124) %   (results depend on constants and initial values of qA and qA').
   (125) Input  qA = 30 deg,  qA' = 0 rad/sec
   (126) SolveSetInputDt( Loop = 0,   qB = 60 deg,  qC = 20 deg )
-> (127) z24 = LB*z4
-> (128) z25 = LC*z6
-> (129) z26 = LC*z5 - LA*z1 - LB*z3
-> (130) z27 = LB*z3
-> (131) z28 = LC*z5
-> (132) z29 = LN + LC*z6 - LA*z2 - LB*z4
-> (133) z30 = LA*z2
-> (134) z31 = LA*z1

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


-> (135) z32 = z24*z28 - z25*z27
-> (136) z33 = (z25*z31-z28*z30)/z32
-> (137) z34 = (z24*z31-z27*z30)/z32
-> (138) z35 = LA*z1*qA'
-> (139) z36 = qA'*z35
-> (140) z37 = z28*qC'^2 - z27*qB'^2
-> (141) z38 = z36 - z37
-> (142) z39 = LA*z2*qA'
-> (143) z40 = qA'*z39
-> (144) z41 = z25*qC'^2 - z24*qB'^2
-> (145) z42 = z40 - z41
-> (146) qB' = z33*qA'
-> (147) qC' = z34*qA'
-> (148) z43 = (z25*z42+z28*z38)/z32
-> (149) z44 = (z24*z42+z27*z38)/z32
-> (150) qB'' = z33*qA'' - z43
-> (151) qC'' = z34*qA'' - z44

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

   (175) %--------------------------------------------------------------------
   (176) %   Numerical integration parameters.
   (177) Input  tFinal = 7 sec,  tStep = 0.02 sec,  absError = 1.0E-07
   (178) %--------------------------------------------------------------------
   (179) %   List quantities to be output from ODE.
   (180) ECheck = System.GetEnergyCheckKane()
-> (181) z62 = z24*qB' + z53*qA' - z25*qC'
-> (182) z63 = z28*qC' + z54*qA' - z27*qB'
-> (183) z64 = z56*qA' - 0.5*z58*qB' - 0.5*z59*qC'
-> (184) WCheck1' = -z64 - FCx*z62 - FCy*z63
-> (185) z65 = mA*LA^2
-> (186) z66 = mC*LC^2
-> (187) 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)

-> (188) ECheck = WCheck1 + z67

   (189) Output  t sec,  qA deg,  qB deg,  qC deg,  Loop[1] m,  Loop[2] m,  ECheck Joules
   (190) Output  t sec,  FCx Newtons,  FCy Newtons      %, FAx Newtons
   (191) %--------------------------------------------------------------------
   (192) %   Numerically solve the ODEs for qA''  (and perhaps FCx, FCy, FAx).
   (193) ODE( Dynamics = 0,  qA'', FCx, FCy )  MGFourBarForcesAndMotion
-> (194) z68 = z46 + 0.5*z33*z48
-> (195) z69 = 0.5*z48*z43 - z60
-> (196) z70 = 0.5*z48 + z50*z33
-> (197) z71 = z50*z43 - z61
-> (198) z72 = z52*z34
-> (199) z73 = z52*z44 - 0.5*z59

   (200) %
   (201) %
   (202) %
   (203) %********************************************************************
   (204) %   Design 4-bar linkage to draw Valentines heart.
   (205) Input LN := 2,  LA := 4 m,  LB := 4 m,  LC := 4 m,  mA := 20 kg
   (206) %--------------------------------------------------------------------
   (207) %   Control motor torque so qA' is nearly 60 degrees/sec.
   (208) Constant  wMotor = 60 deg/sec     % Desired motor angular speed.
   (209) Constant  kw = 9600 N*m/rad       % Control constant.
   (210) TA := kw * (wMotor - qA')         % Motor torque on crank A from ground N.
-> (211) TA = kw*(wMotor-qA')

   (212) H := 0                            % Remove horizontal force at point CB.
-> (213) H = 0

   (214) %--------------------------------------------------------------------
   (215) %   Output coupler point position for Valentines heart.
   (216) Point     CouplerPoint( B )
   (217) Constant  CouplerDistance = sqrt(8) m     % For conventional heart, use  2.66
   (218) Constant  CouplerAngle = 45 degrees       % For conventional heart, use 41.20
   (219) couplerPointBx = CouplerDistance * cos(CouplerAngle)
-> (220) couplerPointBx = CouplerDistance*cos(CouplerAngle)

   (221) couplerPointBy = CouplerDistance * sin(CouplerAngle)
-> (222) couplerPointBy = CouplerDistance*sin(CouplerAngle)

   (223) CouplerPoint.SetPosition( Bo,  couplerPointBx * Bx> + couplerPointBy * By> )
-> (224) p_Bo_CouplerPoint> = couplerPointBx*Bx> + couplerPointBy*By>

   (225) couplerPointHorizontal = Dot( CouplerPoint.GetPosition(No),  Ny> )
-> (226) couplerPointHorizontal = LA*z2 + couplerPointBx*z4 + couplerPointBy*z3

   (227) CouplerPointVertical  = -Dot( CouplerPoint.GetPosition(No),  Nx> )
-> (228) CouplerPointVertical = couplerPointBy*z4 - LA*z1 - couplerPointBx*z3

   (229) Output  couplerPointHorizontal m,  CouplerPointVertical m,  qA' deg/sec, TA N*m
   (230) %--------------------------------------------------------------------
   (231) %   Initial configuration is coupler link B horizontal (qB = 90 degrees).
   (232) %   Use the loop constraints to solve for initial values of qA, qB.
   (233) %   Use loopDt to solve for initial values of qB', qC'.
   (234) Input  qB := 90 deg,  qA' := Input(wMotor, noUnitSystem) deg/sec
   (235) SolveSetInput( Loop = 0,   qA = -15 deg,   qC  = 15 deg )

->    %  INPUT has been assigned as follows:
->    %   qA                       -14.47751218592985       deg
->    %   qC                        14.47751218592986       deg

   (236) %--------------------------------------------------------------------
   (237) %   Augment dynamics with constraints and solve ODEs (plot results).
   (238) ODE( Dynamics = 0,  qA'', FCx, FCy )  MGFourBarDynamicsValentinesHeart

   (239) Plot MGFourBarDynamicsValentinesHeart.3 [1, 2]
   (240) %--------------------------------------------------------------------
   (241) %   Save input together with program responses.
Saved by Motion Genesis LLC.   Command names and syntax: Copyright (c) 2009-2018 Motion Genesis LLC. All rights reserved.