MGVehicleTricycleKaneAugmented.html   (MotionGenesis input/output).
   (1) % MotionGenesis file:  VehicleTricycleKaneAugmented.txt
   (2) % Copyright (c) 2009 Motion Genesis LLC.  All rights reserved.
   (3) % Problem: Statics/dynamics of three-wheel vehicle (tricycle)
   (4) %--------------------------------------------------------------------
   (5) %       Physical objects
   (6) NewtonianFrame  N            % Newtonian reference frame (ground)
   (7) RigidBody       A            % Vehicle chassis (Ao is center of axle)
   (8) RigidBody       B, C         % Rear wheels of vehicle
   (9) RigidFrame      E            % Front fork
   (10) RigidBody       F            % Front wheel
   (11) Point           BN( B )      % Point of B in contact with N
   (12) Point           CN( C )      % Point of C in contact with N
   (13) Point           FN( F )      % Point of F in contact with N
   (14) Point           Scm( A )     % System mass center (fixed on A)
   (15) %--------------------------------------------------------------------
   (16) %       Constants and variables
   (17) Constant  b = 0.75 m         % Distance between Ao and Bcm (or Ao and Ccm)
   (18) Constant  R = 0.35 m         % Radius of wheels
   (19) Constant  e = 2.0  m         % Distance between Ao and Fcm
   (20) Constant  a = 1.64 m         % Ax> measure of Acm's position vector from Ao
   (21) Constant  h = 0.35 m         % Az> measure of Acm's position vector from Ao
   (22) Constant  g = 9.8  m/s^2     % Earth's gravitational acceleration
   (23) Constant  theta = 15 deg     % Road grade (i.e., hill angle)
   (24) Variable  TB                 % Breaking or driving torque on B from A
   (25) Variable  TSteer             % Steering torque on F from A
   (26) Variable  v'                 % Ax> measure number of velocity of Ao in N
   (27) Variable  qA''               % Az> rotation of A in N (vehicle heading angle)
   (28) Variable  wB'                % Ay> measure of angular velocity of B in A
   (29) Variable  wC'                % Ay> measure of angular velocity of B in A
   (30) Variable  qE''               % Az> rotation of E in N (steering)
   (31) Variable  wF'                % Ey> measure of angular velocity of F in E
   (32) SetGeneralizedSpeed( v, qA', qE', wF )
   (33) %--------------------------------------------------------------------
   (34) %       Mass and inertia
   (35) A.SetMass( mA = 640 kg)
   (36) B.SetMass(  m =  30 kg )
   (37) C.SetMass(  m )
   (38) F.SetMass(  m )
   (39) A.SetInertia( Acm, IAxx, IAyy, IAzz = 166.6 kg*m^2 )       % Relevant inertias of body A
   (40) B.SetInertia( Bcm, A, K = 1.0 kg*m^2, J = 2.0 kg*m^2, K )  % B's inertia expressed in A basis
   (41) C.SetInertia( Ccm, A, K, J, K )                            % C's inertia expressed in A basis
   (42) F.SetInertia( Fcm, E, K, J, K )                            % E's Inertia expressed in A basis
   (43) %--------------------------------------------------------------------
   (44) %       Rotational kinematics
   (45) A.RotateZ( N, qA )
-> (46) A_N = [cos(qA), sin(qA), 0;  -sin(qA), cos(qA), 0;  0, 0, 1]
-> (47) w_A_N> = qA'*Az>
-> (48) alf_A_N> = qA''*Az>

   (49) B.SetAngularVelocityAcceleration( A, wB*Ay> )
-> (50) w_B_A> = wB*Ay>
-> (51) w_B_N> = wB*Ay> + qA'*Az>
-> (52) alf_B_A> = wB'*Ay>
-> (53) alf_B_N> = -wB*qA'*Ax> + wB'*Ay> + qA''*Az>

   (54) C.SetAngularVelocityAcceleration( A, wC*Ay> )
-> (55) w_C_A> = wC*Ay>
-> (56) w_C_N> = wC*Ay> + qA'*Az>
-> (57) alf_C_A> = wC'*Ay>
-> (58) alf_C_N> = -wC*qA'*Ax> + wC'*Ay> + qA''*Az>

   (59) E.RotateNegativeZ( A, qE )
-> (60) E_A = [cos(qE), -sin(qE), 0;  sin(qE), cos(qE), 0;  0, 0, 1]
-> (61) w_E_A> = -qE'*Ez>
-> (62) w_E_N> = (qA'-qE')*Az>
-> (63) alf_E_A> = -qE''*Ez>
-> (64) alf_E_N> = (qA''-qE'')*Az>

   (65) F.SetAngularVelocityAcceleration( E, wF*Ey> )
-> (66) w_F_E> = wF*Ey>
-> (67) w_F_N> = wF*Ey> + (qA'-qE')*Ez>
-> (68) alf_F_E> = wF'*Ey>
-> (69) alf_F_N> = -wF*(qA'-qE')*Ex> + wF'*Ey> + (qA''-qE'')*Ez>

   (70) %--------------------------------------------------------------------
   (71) %       Translational kinematics
   (72) Ao.SetVelocityAcceleration( N,  v*Ax> )
-> (73) v_Ao_N> = v*Ax>
-> (74) a_Ao_N> = v'*Ax> + qA'*v*Ay>

   (75) Acm.Translate( Ao,  a*Ax> + h*Az> )
-> (76) p_Ao_Acm> = a*Ax> + h*Az>
-> (77) v_Acm_N> = v*Ax> + a*qA'*Ay>
-> (78) a_Acm_N> = (v'-a*qA'^2)*Ax> + (qA'*v+a*qA'')*Ay>

   (79) Bcm.Translate( Ao, -b*Ay> )
-> (80) p_Ao_Bcm> = -b*Ay>
-> (81) v_Bcm_N> = (v+b*qA')*Ax>
-> (82) a_Bcm_N> = (v'+b*qA'')*Ax> + qA'*(v+b*qA')*Ay>

   (83) Ccm.Translate( Ao,  b*Ay> )
-> (84) p_Ao_Ccm> = b*Ay>
-> (85) v_Ccm_N> = (v-b*qA')*Ax>
-> (86) a_Ccm_N> = (v'-b*qA'')*Ax> + qA'*(v-b*qA')*Ay>

   (87) Fcm.Translate( Ao,  e*Ax> )
-> (88) p_Ao_Fcm> = e*Ax>
-> (89) v_Fcm_N> = v*Ax> + e*qA'*Ay>
-> (90) a_Fcm_N> = (v'-e*qA'^2)*Ax> + (qA'*v+e*qA'')*Ay>

   (91) BN.Translate( Bcm, -R*Az> )
-> (92) p_Bcm_BN> = -R*Az>
-> (93) v_BN_N> = (v+b*qA'-R*wB)*Ax>
-> (94) a_BN_N> = (v'+b*qA''-R*wB')*Ax> - qA'*(2*R*wB-v-b*qA')*Ay> + R*wB^2*Az>

   (95) CN.Translate( Ccm, -R*Az> )
-> (96) p_Ccm_CN> = -R*Az>
-> (97) v_CN_N> = (v-R*wC-b*qA')*Ax>
-> (98) a_CN_N> = (v'-R*wC'-b*qA'')*Ax> + qA'*(v-2*R*wC-b*qA')*Ay> + R*wC^2*Az>

   (99) FN.Translate( Fcm, -R*Az> )
-> (100) p_Fcm_FN> = -R*Az>
-> (101) v_FN_N> = v*Ax> + e*qA'*Ay> - R*wF*Ex>
-> (102) a_FN_N> = (v'-e*qA'^2)*Ax> + (qA'*v+e*qA'')*Ay> - R*wF'*Ex> - 2*R*wF*(
         qA'-qE')*Ey> + R*wF^2*Ez>

   (103) Eo.SetPosition( Ao, E*ax> + h*Az> )
-> (104) p_Ao_Eo> = e*Ax> + h*Az>

   (105) %--------------------------------------------------------------------
   (106) %       Translational kinematics of system center of mass
   (107) sx = Dot( System.GetCMPosition(Ao), Ax> )     % Introduce new symbol
-> (108) sx = (m*e+mA*a)/(mA+3*m)

   (109) sz = Dot( System.GetCMPosition(Ao), Az> )     % Introduce new symbol
-> (110) sz = mA*h/(mA+3*m)

   (111) Scm.Translate( Ao,  sx*Ax> + sz*Az> )
-> (112) p_Ao_Scm> = sx*Ax> + sz*Az>
-> (113) v_Scm_N> = v*Ax> + sx*qA'*Ay>
-> (114) a_Scm_N> = (v'-sx*qA'^2)*Ax> + (qA'*v+sx*qA'')*Ay>

   (115) %--------------------------------------------------------------------
   (116) %       Motion constraints (relate wB, wC,  to  v, qA')
   (117) RollingToEmbed[1]   = Dot( BN.GetVelocity(N), Ax> )
-> (118) RollingToEmbed[1] = v + b*qA' - R*wB

   (119) RollingToEmbed[2]   = Dot( CN.GetVelocity(N), Ax> )
-> (120) RollingToEmbed[2] = v - R*wC - b*qA'

   (121) RollingToAugment[1] = Dot( FN.GetVelocity(N), Ex> )
-> (122) RollingToAugment[1] = cos(qE)*v - R*wF - e*sin(qE)*qA'

   (123) RollingToAugment[2] = Dot( FN.GetVelocity(N), Ey> )
-> (124) RollingToAugment[2] = sin(qE)*v + e*cos(qE)*qA'

   (125) SolveDt( RollingToEmbed = 0,  wB, wC )
-> (126) wB = (v+b*qA')/R
-> (127) wC = (v-b*qA')/R
-> (128) wB' = (v'+b*qA'')/R
-> (129) wC' = (v'-b*qA'')/R

   (130) %--------------------------------------------------------------------
   (131) %       Replace all gravity forces with single force at Scm
   (132) mTotal = System.GetMass()
-> (133) mTotal = mA + 3*m

   (134) gravityDirection> = sin(theta)*Nx> - cos(theta)*Nz>
-> (135) gravityDirection> = sin(theta)*Nx> - cos(theta)*Nz>

   (136) Scm.AddForce( mTotal * g * gravityDirection> )
-> (137) Force_Scm> = g*sin(theta)*mTotal*Nx> - g*cos(theta)*mTotal*Nz>

   (138) %--------------------------------------------------------------------
   (139) %       Relevant forces on front wheel F (friction force).
   (140) Variable  FFx, FFy
   (141) FN.AddForce( FFx*Ex> + FFy*Ey> )
-> (142) Force_FN> = FFx*Ex> + FFy*Ey>

   (143) %--------------------------------------------------------------------
   (144) %       Relevant torques
   (145) B.AddTorque( A,  TB*Ay> )
-> (146) Torque_B_A> = TB*Ay>

   (147) E.AddTorque( A, -TSteer*Az> )
-> (148) Torque_E_A> = -TSteer*Az>

   (149) %--------------------------------------------------------------------
   (150) %       Kane's equations for statics - with simplification
   (151) ZeroStatics = System.GetStaticsKane()
-> (152) ZeroStatics[1] = TB/R + FFx*cos(qE) + FFy*sin(qE) + g*sin(theta)*mTot
         al*cos(qA)
-> (153) ZeroStatics[2] = b*TB/R - g*sin(theta)*mTotal*sx*sin(qA) - e*(FFx*sin(
         qE)-FFy*cos(qE))
-> (154) ZeroStatics[3] = TSteer
-> (155) ZeroStatics[4] = -R*FFx

   (156) StaticSolution = Solve( ZeroStatics = 0,  TB, TSteer, FFx, FFy )
-> (157) StaticSolution[1] = g*R*sin(theta)*mTotal*(e*cos(qA)*cos(qE)+sx*sin(
         qA)*sin(qE))/(b*sin(qE)-e*cos(qE))

-> (158) StaticSolution[2] = 0
-> (159) StaticSolution[3] = 0
-> (160) StaticSolution[4] = -g*sin(theta)*mTotal*(b*cos(qA)+sx*sin(qA))/(b*sin
         (qE)-e*cos(qE))

   (161) %--------------------------------------------------------------------
   (162) %       Kane's equations of motion - augmented with constraints
   (163) DynamicEqns = System.GetDynamicsKane()
-> (164) DynamicEqns[1] = (mA+3*m+2*J/R^2)*v' - TB/R - FFx*cos(qE) - FFy*sin(qE)
         - g*sin(theta)*mTotal*cos(qA) - (m*e+mA*a)*qA'^2

-> (165) DynamicEqns[2] = g*sin(theta)*mTotal*sx*sin(qA) + e*(FFx*sin(qE)-FFy*
         cos(qE)) + (m*e+mA*a)*qA'*v + (IAzz+3*K+m*e^2+mA*a^2+2*m*b^2+2*J*b^2/R^2)*qA''
         - b*TB/R - K*qE''

-> (166) DynamicEqns[3] = K*qE'' - TSteer - K*qA''
-> (167) DynamicEqns[4] = R*FFx + J*wF'

   (168) Zero = [ DynamicEqns;  Dt(RollingToAugment) ]
-> (169) Zero[1] = (mA+3*m+2*J/R^2)*v' - TB/R - FFx*cos(qE) - FFy*sin(qE)
         - g*sin(theta)*mTotal*cos(qA) - (m*e+mA*a)*qA'^2

-> (170) Zero[2] = g*sin(theta)*mTotal*sx*sin(qA) + e*(FFx*sin(qE)-FFy*cos(qE))
         + (m*e+mA*a)*qA'*v + (IAzz+3*K+m*e^2+mA*a^2+2*m*b^2+2*J*b^2/R^2)*qA''
         - b*TB/R - K*qE''

-> (171) Zero[3] = K*qE'' - TSteer - K*qA''
-> (172) Zero[4] = R*FFx + J*wF'
-> (173) Zero[5] = cos(qE)*v' - sin(qE)*qE'*v - e*cos(qE)*qA'*qE' - R*wF'
         - e*sin(qE)*qA''
-> (174) Zero[6] = cos(qE)*qE'*v + sin(qE)*v' + e*cos(qE)*qA'' - e*sin(qE)*qA'*qE'

   (175) %--------------------------------------------------------------------
   (176) %       Output point Ao's position from point No
   (177) Variable x' = Dot( Ao.GetVelocity(N), Nx> )
-> (178) x' = cos(qA)*v

   (179) Variable y' = Dot( Ao.GetVelocity(N), Ny> )
-> (180) y' = sin(qA)*v

   (181) Ao.SetPosition( No, x*Nx> + y*Ny> )
-> (182) p_No_Ao> = x*Nx> + y*Ny>

   (183) %--------------------------------------------------------------------
   (184) %       Total mechanical energy (should be conserved)
   (185) KE = System.GetKineticEnergy()
-> (186) KE = 0.5*J*wB^2 + 0.5*J*wC^2 + K*qA'^2 + 0.5*IAzz*qA'^2 + 0.5*J*wF^2
         + 0.5*K*(qA'-qE')^2 + 0.5*m*(v+b*qA')^2 + 0.5*m*(v-b*qA')^2 + 0.5*m*(v^2
         +e^2*qA'^2) + 0.5*mA*(v^2+a^2*qA'^2)

   (187) PE = mTotal*g*Dot( Scm.GetPosition(No), -gravityDirection> )
-> (188) PE = g*mTotal*(cos(theta)*sz-sin(theta)*x-sin(theta)*sx*cos(qA))

   (189) MechanicalEnergy = KE + PE
-> (190) MechanicalEnergy = PE + KE

   (191) %--------------------------------------------------------------------
   (192) %       Integration parameters
   (193) Input  tFinal = 7.0 sec,  tStep = 0.002 sec,  absError = 1.0E-07
   (194) %--------------------------------------------------------------------
   (195) %       Initial values for variables (released from rest with No and Ao coincident)
   (196) Input  qA = 0 deg,   qE = 0.1 deg,  qE' = 0 rad/sec,  v = 0 m/s,  x = 0 m,  y = 0 m
   (197) SolveSetInput( RollingToAugment = 0,  qA' = 0 rad/sec,  wF = 0 rad/sec  )

->    %  INPUT has been assigned as follows:
->    %   qA'                       0                       rad/sec
->    %   wF                        0                       rad/sec

   (198) %--------------------------------------------------------------------
   (199) %       List quantities to be output by ODE command.
   (200) Output      t sec,  qA deg,  qE deg,  v m/s,  MechanicalEnergy Joules
   (201) OutputPlot  t sec,  qA deg,  qE deg
   (202) %--------------------------------------------------------------------
   (203) %       Dynamic simulation with no breaking or steering
   (204) TB = 0;  TSteer = 0
-> (205) TB = 0
-> (206) TSteer = 0

   (207) ODE( Zero = 0,  v', qE'', qA'', wF', FFx, FFy )  MGVehicleTricycleFreeMotionForward

   (208) %--------------------------------------------------------------------
   (209) Input  qA := 180 deg
   (210) ODE( Zero = 0,  v', qE'', qA'', wF', FFx, FFy )  MGVehicleTricycleFreeMotionBackward

   (211) %--------------------------------------------------------------------
Saved by Motion Genesis LLC.   Command names and syntax: Copyright (c) 2009-2021 Motion Genesis LLC. All rights reserved.