MGFourBarStaticsAndDynamicsWithKaneAndLagrange.html  (MotionGenesis input/output).
   (1) % MotionGenesis file:  MGFourBarStaticsAndDynamicsWithKaneAndLagrange.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) Variable   qA'', qB'', qC''       % Angles
   (13) Specified  H = 200                % Horizontal force
-> (14) H = 200

   (15) %---------------------------
   (16) Variable   FCx, FCy               % Contact forces on C from B.
   (17) qs = [qA; qB; qC ]                % Generalized coordinates
-> (18) qs = [qA;  qB;  qC]

   (19) qDots = Dt( qs )                  % Time-derivative of generalized coordinates
-> (20) qDots = [qA';  qB';  qC']

   (21) SetGeneralizedSpeeds( qDots )     % Generalized speeds (for Kane's method).
   (22) %--------------------------------------------------------------------
   (23) A.SetMassInertia( mA = 10 kg,  0,  IA = mA*LA^2/12,  IA  )
-> (24) IA = 0.08333333*mA*LA^2

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

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

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

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

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

   (43) %--------------------------------------------------------------------
   (44) %       Translational kinematics
   (45) Ao.Translate(  No,          0> )
-> (46) p_No_Ao> = 0>
-> (47) v_Ao_N> = 0>
-> (48) a_Ao_N> = 0>

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

   (53) Bo.Translate(   Ao,      LA*Ax> )
-> (54) p_Ao_Bo> = LA*Ax>
-> (55) v_Bo_N> = LA*qA'*Ay>
-> (56) a_Bo_N> = -LA*qA'^2*Ax> + LA*qA''*Ay>

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

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

   (65) Co.Translate(   No,      LN*Ny> )
-> (66) p_No_Co> = LN*Ny>
-> (67) v_Co_N> = 0>
-> (68) a_Co_N> = 0>

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

   (73) CB.Translate(   Co,      LC*Cx> )
-> (74) p_Co_CB> = LC*Cx>
-> (75) v_CB_N> = LC*qC'*Cy>
-> (76) a_CB_N> = -LC*qC'^2*Cx> + LC*qC''*Cy>

   (77) %--------------------------------------------------------------------
   (78) %       Forces
   (79) System.AddForceGravity( g*Nx> )
-> (80) Force_Acm> = mA*g*Nx>
-> (81) Force_Bcm> = mB*g*Nx>
-> (82) Force_Ccm> = mC*g*Nx>

   (83) CB.AddForce( H*Ny> )
-> (84) Force_CB> = H*Ny>

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

   (87) %--------------------------------------------------------------------
   (88) %       Dynamics with Kane's augmented method  (Linkage "cut" at CB/BC)
   (89) KaneDynamics = System.GetDynamicsKane()
-> (90) KaneDynamics[1] = 0.5*mB*LA*LB*cos(qA-qB)*qB'' + 0.25*(mA*LA^2+4*mB*LA^2
        +4*IA)*qA'' - 0.5*LA*(2*FCx*sin(qA)-2*FCy*cos(qA)-2*mB*g*sin(qA)-mA*g*
        sin(qA)-mB*LB*sin(qA-qB)*qB'^2)

-> (91) KaneDynamics[2] = 0.5*LB*(2*FCy*cos(qB)+mB*g*sin(qB)-2*FCx*sin(qB)-mB*
        LA*sin(qA-qB)*qA'^2) + 0.25*(mB*LB^2+4*IB)*qB'' + 0.5*mB*LA*LB*cos(qA-
        qB)*qA''

-> (92) KaneDynamics[3] = 0.25*(mC*LC^2+4*IC)*qC'' - 0.5*LC*(2*H*cos(qC)+2*FCy*
        cos(qC)-2*FCx*sin(qC)-mC*g*sin(qC))

   (93) %--------------------------------------------------------------------
   (94) %       Alternate: Check Kane's equations with Lagrange's method 
   (95) K = System.GetKineticEnergy()
-> (96) K = 0.5*IA*qA'^2 + 0.5*IB*qB'^2 + 0.5*IC*qC'^2 + 0.125*mA*LA^2*qA'^2
        + 0.125*mC*LC^2*qC'^2 + 0.125*mB*(LB^2*qB'^2+4*LA^2*qA'^2+4*LA*LB*cos(
        qA-qB)*qA'*qB')

   (97) GeneralizedForces = System.GetGeneralizedForces()
-> (98) GeneralizedForces[1] = 0.5*LA*(2*FCx*sin(qA)-2*FCy*cos(qA)-2*mB*g*sin(
        qA)-mA*g*sin(qA))
-> (99) GeneralizedForces[2] = 0.5*LB*(2*FCx*sin(qB)-2*FCy*cos(qB)-mB*g*sin(qB))
-> (100) GeneralizedForces[3] = 0.5*LC*(2*H*cos(qC)+2*FCy*cos(qC)-2*FCx*sin(qC)
         -mC*g*sin(qC))

   (101) LagrangeDynamics = Dt( D(K,qDots) ) - D(K,qs) - GeneralizedForces
-> (102) LagrangeDynamics[1] = IA*qA'' + 0.25*mA*LA^2*qA'' + 0.5*mB*LA*(LB*sin(
         qA-qB)*qB'^2+2*LA*qA''+LB*cos(qA-qB)*qB'') - 0.5*LA*(2*FCx*sin(qA)-2*
         FCy*cos(qA)-2*mB*g*sin(qA)-mA*g*sin(qA))

-> (103) LagrangeDynamics[2] = IB*qB'' - 0.5*LB*(2*FCx*sin(qB)-2*FCy*cos(qB)-
         mB*g*sin(qB)) - 0.25*mB*LB*(2*LA*sin(qA-qB)*qA'^2-LB*qB''-2*LA*cos(qA-
         qB)*qA'')

-> (104) LagrangeDynamics[3] = 0.25*(mC*LC^2+4*IC)*qC'' - 0.5*LC*(2*H*cos(qC)+2
         *FCy*cos(qC)-2*FCx*sin(qC)-mC*g*sin(qC))

   (105) ShouldBeEqual = KaneDynamics - LagrangeDynamics
-> (106) ShouldBeEqual = [0;  0;  0]

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

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

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

   (115) LoopDt = Dt( Loop )
-> (116) LoopDt[1] = LC*sin(qC)*qC' - LA*sin(qA)*qA' - LB*sin(qB)*qB'
-> (117) LoopDt[2] = LA*cos(qA)*qA' + LB*cos(qB)*qB' - LC*cos(qC)*qC'

   (118) LoopDtDt = Dt( LoopDt )
-> (119) 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''

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

   (121) %--------------------------------------------------------------------
   (122) %       Static solution with Kane or Lagrange augmented  (Linkage is "cut" at CB/BC)
   (123) StaticSolution = Solve( [GeneralizedForces; Loop],  qA=10 deg, qB=50 deg, qC=30 deg, FCx=0 N, FCy=0 N )
->    %  Note: qA = 0.3488373  was converted from  qA = 19.98691 deg.
->    %  Note: qB = 1.250738  was converted from  qB = 71.66202 deg.
->    %  Note: qC = 0.6688954  was converted from  qC = 38.32489 deg.
-> (124) StaticSolution = [0.3488373;  1.250738;  0.6688954;  77.92883;  -60.85662]

   (125) %--------------------------------------------------------------------
   (126) %       Solve for initial values of qB, qC and qB',qC' using the loop constraints, 
   (127) %       given values of constants, and initial values of qA and qA'.
   (128) Input  qA = 30 deg,  qA' = 0 rad/sec
   (129) SolveSetInput( Loop,    qB = 60 deg,      qC  = 20 deg ) 

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

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

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

   (131) %--------------------------------------------------------------------
   (132) %       Integration parameters and quantities to be output from ODE.
   (133) Input  tFinal = 7 sec,  tStep = 0.02,  absError = 1.0E-07
   (134) OutputPlot  t sec,  qA deg,  qB deg,  qC deg,  FCx Newtons,  FCy  Newtons
   (135) %--------------------------------------------------------------------
   (136) %       Augment equations of motion with constraints and solve ODEs.
   (137) KaneAugmented = [ KaneDynamics;  LoopDtDt ]
-> (138) KaneAugmented[1] = 0.5*mB*LA*LB*cos(qA-qB)*qB'' + 0.25*(mA*LA^2+4*mB*LA^2
         +4*IA)*qA'' - 0.5*LA*(2*FCx*sin(qA)-2*FCy*cos(qA)-2*mB*g*sin(qA)-mA*g*
         sin(qA)-mB*LB*sin(qA-qB)*qB'^2)

-> (139) KaneAugmented[2] = 0.5*LB*(2*FCy*cos(qB)+mB*g*sin(qB)-2*FCx*sin(qB)-
         mB*LA*sin(qA-qB)*qA'^2) + 0.25*(mB*LB^2+4*IB)*qB'' + 0.5*mB*LA*LB*cos(
         qA-qB)*qA''

-> (140) KaneAugmented[3] = 0.25*(mC*LC^2+4*IC)*qC'' - 0.5*LC*(2*H*cos(qC)+2*
         FCy*cos(qC)-2*FCx*sin(qC)-mC*g*sin(qC))

-> (141) KaneAugmented[4] = 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''

-> (142) KaneAugmented[5] = 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''

   (143) ODE( KaneAugmented,  qA'', qB'', qC'', FCx, FCy ) MGFourBarKaneAugmented

   (144) %--------------------------------------------------------------------
   (145) %       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.