MGFourBarDynamicsKaneEmbedded.html  (MotionGenesis input/output).
   (1) % MotionGenesis file:  MGFourBarDynamicsKaneEmbedded.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               % Length of ground link.
   (10) Constant   LA = 1 m               % Length of crank link.
   (11) Constant   LB = 2 m,  LC = 2 m    % Length of coupler, rocker links.
   (12) Constant   g = 9.81 m/s^2         % Earth's gravitational acceleration.
   (13) Specified  H = 200                % Horizontal force at point CB.
-> (14) H = 200

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

   (17) Variable   qA'',  qB'',  qC''     % Link angles (relative to ground).
   (18) SetGeneralizedSpeed( qA' )
   (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) A_N = [cos(qA), sin(qA), 0;  -sin(qA), cos(qA), 0;  0, 0, 1]
-> (30) w_A_N> = qA'*Az>
-> (31) alf_A_N> = qA''*Az>

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

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

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

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

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

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

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

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

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

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

   (74) %--------------------------------------------------------------------
   (75) %   Add relevant forces and torques (replace gravity forces with equivalent set).
   (76) Bo.AddForce( 0.5*(mA+mB)*g*Nx> )
-> (77) Force_Bo> = 0.5*(mA+mB)*g*Nx>

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

   (80) A.AddTorque( N,  TA * Az> )
-> (81) Torque_A_N> = TA*Az>

   (82) %--------------------------------------------------------------------
   (83) %   Configuration (loop) constraints and their time-derivatives.
   (84) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny>
-> (85) Loop> = LA*Ax> + LB*Bx> - LC*Cx> - LN*Ny>

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

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

   (90) %--------------------------------------------------------------------
   (91) %   Use the loop constraints to solve for initial values of qB, qC and qB',qC'
   (92) %   (results depend on constants and initial values of qA and qA').
   (93) Input  qA = 30 deg,  qA' = 0 rad/sec
   (94) SolveSetInputDt( Loop,  qB = 60 deg,  qC = 20 deg )

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

-> (95) qB' = -LA*sin(qA-qC)*qA'/(LB*sin(qB-qC))
-> (96) qC' = -LA*sin(qA-qB)*qA'/(LC*sin(qB-qC))
-> (97) 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))

-> (98) 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))

   (99) %--------------------------------------------------------------------
   (100) %   Equations of motion - with Kane's method.
   (101) Zero = System.GetDynamicsKane()
-> (102) Zero[1] = 0.5*(mA+mB)*g*LA*sin(qA) + 0.5*LA*sin(qA-qB)*(2*H*cos(qC)-(
         mB+mC)*g*sin(qC))/sin(qB-qC) + 0.25*(4*IA+mA*LA^2+4*IB*LA^2*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''
         - TA - 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)-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)))

   (103) Solve( Zero, qA'' )
-> (104) qA'' = -(2*(mA+mB)*g*LA*sin(qA)+2*LA*sin(qA-qB)*(2*H*cos(qC)-(mB+mC)*g
         *sin(qC))/sin(qB-qC)-4*TA-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)-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))))/(4*IA+mA*LA^2+4*IB*LA^2*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)))

   (105) %--------------------------------------------------------------------
   (106) %   Integration parameters and quantities to be output from ODE.
   (107) Input  tFinal = 7 sec,  tStep = 0.02 sec,  absError = 1.0E-07
   (108) Output t sec,  qA deg,  qB deg,  qC deg
   (109) ODE() MGFourBarDynamicsKaneEmbedded

   (110) % Plot  MGFourBarDynamicsKaneEmbedded.1 [1, 2, 3, 4]
   (111) %--------------------------------------------------------------------
   (112) %   Design 4-bar linkage to draw Valentines heart.
   (113) Input LN := 2,  LA := 4 m,  LB := 4 m,  LC := 4 m,  mA := 20 kg
   (114) %--------------------------------------------------------------------
   (115) %   Control motor torque so qA' is nearly 60 degrees/sec.
   (116) Constant  wMotor = 60 deg/sec     % Desired motor angular speed.
   (117) Constant  kw = 9600 N*m/rad       % Control constant.
   (118) TA := kw * (wMotor - qA')         % Motor torque on crank A from ground N.
-> (119) TA = kw*(wMotor-qA')

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

   (122) %--------------------------------------------------------------------
   (123) %   Output coupler point position for Valentines heart.
   (124) Point     CouplerPoint( B )
   (125) Constant  CouplerDistance = sqrt(8) m     % For conventional heart, use  2.66
   (126) Constant  CouplerAngle = 45 degrees       % For conventional heart, use 41.20
   (127) couplerPointBx = CouplerDistance * cos(CouplerAngle)
-> (128) couplerPointBx = CouplerDistance*cos(CouplerAngle)

   (129) couplerPointBy = CouplerDistance * sin(CouplerAngle)
-> (130) couplerPointBy = CouplerDistance*sin(CouplerAngle)

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

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

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

   (137) Output  couplerPointHorizontal m,  CouplerPointVertical m,  qA' deg/sec, TA N*m
   (138) %--------------------------------------------------------------------
   (139) %   Initial configuration is coupler link B horizontal (qB = 90 degrees).
   (140) %   Use the loop constraints to solve for initial values of qA, qB.
   (141) %   Use loopDt to solve for initial values of qB', qC'.
   (142) Input  qB := 90 deg,  qA' := Input(wMotor, noUnitSystem) deg/sec
   (143) SolveSetInput( Loop = 0,   qA = -15 deg,   qC  = 15 deg )

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

   (144) %--------------------------------------------------------------------
   (145) %   Augment dynamics with constraints and simulate.
   (146) ODE() MGFourBarDynamicsValentinesHeart

   (147) Plot MGFourBarDynamicsValentinesHeart.2 [1, 2]
   (148) %--------------------------------------------------------------------
   (149) %   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.