MGFourBarDynamicsFBD.html  (MotionGenesis input/output).
   (1) % MotionGenesis file:  MGFourBarDynamicsFBD.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''     % Link angles (relative to ground).
   (17) Variable   FCx, FCy               % Contact forces on C from B.
   (18) %--------------------------------------------------------------------
   (19) A.SetMassInertia( mA = 10 kg,  0,  IA = mA*LA^2/12,  IA  )
-> (20) IA = 0.08333333*mA*LA^2

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

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

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

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

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

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

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

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

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

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

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

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

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

   (73) %--------------------------------------------------------------------
   (74) %   Add relevant forces and torques.
   (75) System.AddForceGravity(  g * Nx>  )
-> (76) Force_Acm> = mA*g*Nx>
-> (77) Force_Bcm> = mB*g*Nx>
-> (78) Force_Ccm> = mC*g*Nx>

   (79) CB.AddForce(  H * Ny>  )
-> (80) Force_CB> = H*Ny>

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

   (83) A.AddTorque( N,  TA * Az> )
-> (84) Torque_A_N> = TA*Az>

   (85) %--------------------------------------------------------------------
   (86) %   Form equations of motion ("cut" linkage at CB/BC).
   (87) Dynamics[1] = Dot( Nz>,  System(A,B).GetDynamics(Ao) )
-> (88) 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'' - TA
        - LA*FCx*sin(qA) - LB*FCx*sin(qB) - 0.5*mB*LA*LB*sin(qA-qB)*qA'^2

   (89) Dynamics[2] = Dot( Nz>,            B.GetDynamics(Bo) )
-> (90) 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'')

   (91) Dynamics[3] = Dot( Nz>,            C.GetDynamics(Co) )
-> (92) 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))

   (93) %--------------------------------------------------------------------
   (94) %   Configuration (loop) constraints and their time-derivatives.
   (95) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny>
-> (96) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny>

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

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

   (101) LoopDt = Dt( Loop )
-> (102) LoopDt[1] = LC*sin(qC)*qC' - LA*sin(qA)*qA' - LB*sin(qB)*qB'
-> (103) LoopDt[2] = LA*cos(qA)*qA' + LB*cos(qB)*qB' - LC*cos(qC)*qC'

   (104) LoopDtDt = Dt( LoopDt )
-> (105) 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''

-> (106) 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''

   (107) %--------------------------------------------------------------------
   (108) %   Use the loop constraints to solve for initial values of qB, qC and qB',qC'
   (109) %   (results depend on constants and initial values of qA and qA').
   (110) Input  qA = 30 deg,  qA' = 0 rad/sec
   (111) SolveSetInput(   Loop = 0,   qB = 60 deg,      qC  = 20 deg )

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

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

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

   (113) %--------------------------------------------------------------------
   (114) %   Numerical integration parameters.
   (115) Input  tFinal = 7 sec,  tStep = 0.02 sec,  absError = 1.0E-07
   (116) %--------------------------------------------------------------------
   (117) %   List quantities to be output from ODE.
   (118) Output  t sec,  qA deg,  qB deg,  qC deg,  FCx Newtons,  FCy  Newtons
   (119) %--------------------------------------------------------------------
   (120) %   Augment dynamics with constraints and solve ODEs (plot results).
   (121) ODE( [Dynamics; LoopDtDt] = 0,  qA'', qB'', qC'', FCx, FCy )  MGFourBarDynamicsFBD

   (122) % Plot MGFourBarDynamicsFBD.1 [1, 2, 3, 4]
   (123) %
   (124) %
   (125) %
   (126) %********************************************************************
   (127) %   Statics via dynamics - simulate with damping..
   (128) H := 200 - 80 * qC'
-> (129) H = 200 - 80*qC'

   (130) ODE( [Dynamics; LoopDtDt] = 0,  qA'', qB'', qC'', FCx, FCy ) MGFourBarDynamicsFBDdamped

   (131) % Plot MGFourBarDynamicsFBDdamped.1 [1, 2, 3, 4]
   (132) %
   (133) %
   (134) %
   (135) %********************************************************************
   (136) %   Design 4-bar linkage to draw Valentines heart.
   (137) Input LN := 2,  LA := 4 m,  LB := 4 m,  LC := 4 m,  mA := 20 kg
   (138) %--------------------------------------------------------------------
   (139) %   Control motor torque so qA' is nearly 60 degrees/sec.
   (140) Constant  wMotor = 60 deg/sec     % Desired motor angular speed.
   (141) Constant  kw = 9600 N*m/rad       % Control constant.
   (142) TA := kw * (wMotor - qA')         % Motor torque on crank A from ground N.
-> (143) TA = kw*(wMotor-qA')

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

   (146) %--------------------------------------------------------------------
   (147) %   Output coupler point position for Valentines heart.
   (148) Point     CouplerPoint( B )
   (149) Constant  CouplerDistance = sqrt(8) m     % For conventional heart, use  2.66
   (150) Constant  CouplerAngle = 45 degrees       % For conventional heart, use 41.20
   (151) couplerPointBx = CouplerDistance * cos(CouplerAngle)
-> (152) couplerPointBx = CouplerDistance*cos(CouplerAngle)

   (153) couplerPointBy = CouplerDistance * sin(CouplerAngle)
-> (154) couplerPointBy = CouplerDistance*sin(CouplerAngle)

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

   (157) couplerPointHorizontal = Dot( CouplerPoint.GetPosition(No),  Ny> )
-> (158) couplerPointHorizontal = LA*sin(qA) + couplerPointBx*sin(qB) + couple
         rPointBy*cos(qB)

   (159) CouplerPointVertical  = -Dot( CouplerPoint.GetPosition(No),  Nx> )
-> (160) CouplerPointVertical = couplerPointBy*sin(qB) - LA*cos(qA) - couplerP
         ointBx*cos(qB)

   (161) Output  couplerPointHorizontal m,  CouplerPointVertical m,  qA' deg/sec, TA N*m
   (162) %--------------------------------------------------------------------
   (163) %   Initial configuration is coupler link B horizontal (qB = 90 degrees).
   (164) %   Use the loop constraints to solve for initial values of qA, qB.
   (165) %   Use loopDt to solve for initial values of qB', qC'.
   (166) Input  qB := 90 deg,  qA' := Input(wMotor, noUnitSystem) deg/sec
   (167) SolveSetInput(   Loop = 0,  qA = -15 deg,     qC  = 15 deg )

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

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

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

   (169) %--------------------------------------------------------------------
   (170) %   Augment dynamics with constraints and solve ODEs (plot results).
   (171) ODE( [Dynamics; LoopDtDt] = 0,  qA'', qB'', qC'', FCx, FCy ) MGFourBarDynamicsValentinesHeart

   (172) Plot MGFourBarDynamicsValentinesHeart.2 [1, 2]
   (173) %--------------------------------------------------------------------
   (174) %   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.