MGVehicleTricycleFBD.html   (MotionGenesis input/output).
   (1) % MotionGenesis file:  MGVehicleTricycleFBD.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) %--------------------------------------------------------------------
   (33) %       Mass and inertia
   (34) A.SetMass( mA = 640 kg)
   (35) B.SetMass(  m =  30 kg )
   (36) C.SetMass(  m )
   (37) F.SetMass(  m )
   (38) A.SetInertia( Acm, IAxx, IAyy, IAzz = 166.6 kg*m^2 )       % Body A's relevant moments of inertia.
   (39) B.SetInertia( Bcm, A, K = 1.0 kg*m^2, J = 2.0 kg*m^2, K )  % B's inertia expressed in A basis.
   (40) C.SetInertia( Ccm, A, K, J, K )                            % C's inertia expressed in A basis.
   (41) F.SetInertia( Fcm, E, K, J, K )                            % E's Inertia expressed in A basis.
   (42) %--------------------------------------------------------------------
   (43) %       Rotational kinematics
   (44) A.RotateZ( N, qA )
-> (45) A_N = [cos(qA), sin(qA), 0;  -sin(qA), cos(qA), 0;  0, 0, 1]
-> (46) w_A_N> = qA'*Az>
-> (47) alf_A_N> = qA''*Az>

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

   (114) %--------------------------------------------------------------------
   (115) %       Motion constraints (relates qA', wB, wC, wF, v, qE')
   (116) Rolling[1] = Dot( BN.GetVelocity(N), Ax> )
-> (117) Rolling[1] = v + b*qA' - R*wB

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

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

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

   (124) %--------------------------------------------------------------------
   (125) %       Replace all gravity forces with single force at Scm.
   (126) % Note: This works since the analysis below does not require
   (127) %       separate gravity forces for A, B, C, F.
   (128) mTotal = System.GetMass()
-> (129) mTotal = mA + 3*m

   (130) gravityDirection> = sin(theta)*Nx> - cos(theta)*Nz>
-> (131) gravityDirection> = sin(theta)*Nx> - cos(theta)*Nz>

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

   (134) %--------------------------------------------------------------------
   (135) %       Roadway normal and friction contact forces on wheels B, C, F.
   (136) % Note: FBy and FCy are redundant/indeterminate.
   (137) %       The analysis below avoids their calculation.
   (138) Variable  FBx, FBy, FBz,   FCx, FCy, FCz,   FFx, FFy, FFz
   (139) BN.AddForce( FBx*Ax> + FBy*Ay> + FBz*Az> )
-> (140) Force_BN> = FBx*Ax> + FBy*Ay> + FBz*Az>

   (141) CN.AddForce( FCx*Ax> + FCy*Ay> + FCz*Az> )
-> (142) Force_CN> = FCx*Ax> + FCy*Ay> + FCz*Az>

   (143) FN.AddForce( FFx*Ex> + FFy*Ey> + FFz*Ez> )
-> (144) Force_FN> = FFx*Ex> + FFy*Ey> + FFz*Ez>

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

   (149) E.AddTorque( A, -TSteer*Az> )
-> (150) Torque_E_A> = -TSteer*Az>

   (151) %--------------------------------------------------------------------
   (152) %       FBDs (MG road maps) for statics to solve for TB.
   (153) Statics[1] = Dot( Ax>,       System.GetStatics()    )
-> (154) Statics[1] = FBx + FCx + FFx*cos(qE) + FFy*sin(qE) + g*sin(theta)*mTo
         tal*cos(qA)

   (155) Statics[2] = Dot( Ez>,  System(E,F).GetStatics(Eo)  )
-> (156) Statics[2] = -TSteer

   (157) Statics[3] = Dot( Ay>,            B.GetStatics(Bcm) )
-> (158) Statics[3] = TB - R*FBx

   (159) Statics[4] = Dot( Ay>,            C.GetStatics(Ccm) )
-> (160) Statics[4] = -R*FCx

   (161) Statics[5] = Dot( Ey>,            F.GetStatics(Fcm) )
-> (162) Statics[5] = -R*FFx

   (163) Statics[6] = Dot( Az>,       System.GetStatics(Ao)  )
-> (164) Statics[6] = b*FBx + e*FFy*cos(qE) - b*FCx - e*FFx*sin(qE) - g*sin(th
         eta)*mTotal*sx*sin(qA)

   (165) StaticSolution = Solve( Statics = 0,  FBx, FCx, FFx, FFy, TSteer, TB )
-> (166) StaticSolution[1] = g*sin(theta)*mTotal*(e*cos(qA)*cos(qE)+sx*sin(qA)*
         sin(qE))/(b*sin(qE)-e*cos(qE))

-> (167) StaticSolution[2] = 0
-> (168) StaticSolution[3] = 0
-> (169) StaticSolution[4] = -g*sin(theta)*mTotal*(b*cos(qA)+sx*sin(qA))/(b*sin
         (qE)-e*cos(qE))
-> (170) StaticSolution[5] = 0
-> (171) StaticSolution[6] = g*R*sin(theta)*mTotal*(e*cos(qA)*cos(qE)+sx*sin(
         qA)*sin(qE))/(b*sin(qE)-e*cos(qE))

   (172) %--------------------------------------------------------------------
   (173) %       FBDs (MG road maps) for dynamics to solve for forces and motion.
   (174) Dynamics[1] = Dot( Ax>,       System.GetDynamics() )
-> (175) Dynamics[1] = mA*(v'-a*qA'^2) + m*(3*v'-e*qA'^2) - FBx - FCx - FFx*cos(qE)
         - FFy*sin(qE) - g*sin(theta)*mTotal*cos(qA)

   (176) Dynamics[2] = Dot( Ez>,  System(E,F).GetDynamics(Eo)  )
-> (177) Dynamics[2] = TSteer + K*(qA''-qE'')

   (178) Dynamics[3] = Dot( Ay>,            B.GetDynamics(Bcm) )
-> (179) Dynamics[3] = R*FBx + J*wB' - TB

   (180) Dynamics[4] = Dot( Ay>,            C.GetDynamics(Ccm) )
-> (181) Dynamics[4] = R*FCx + J*wC'

   (182) Dynamics[5] = Dot( Ey>,            F.GetDynamics(Fcm) )
-> (183) Dynamics[5] = R*FFx + J*wF'

   (184) Dynamics[6] = Dot( Az>,       System.GetDynamics(Ao)  )
-> (185) Dynamics[6] = b*FCx + e*FFx*sin(qE) + g*sin(theta)*mTotal*sx*sin(qA)
         + IAzz*qA'' + 3*K*qA'' + 2*m*b^2*qA'' + m*e*(v*qA'+e*qA'') + mA*a*(v*
         qA'+a*qA'') - b*FBx - e*FFy*cos(qE) - K*qE''

   (186) Dynamics[7] = Dot( Ay>,       System.GetDynamics(Ao)  )  % For normal forces.
-> (187) Dynamics[7] = e*FFz + R*FBx + R*FCx + J*wB' + J*wC' + cos(qE)*(R*FFx+J
         *wF') + mA*h*(v'-a*qA'^2) + sin(qE)*(R*FFy+J*wF*(qA'-qE')) - g*cos(th
         eta)*mTotal*sx - g*sin(theta)*mTotal*sz*cos(qA)

   (188) Dynamics[8] = Dot( Ax>,       System.GetDynamics(BN)  )  % For normal forces.
-> (189) Dynamics[8] = b*g*cos(theta)*mTotal + J*sin(qE)*wF' - 2*b*FCz - b*FFz
         - g*sin(theta)*mTotal*(R+sz)*sin(qA) - J*wB*qA' - J*wC*qA' - 3*m*R*v*qA'
         - J*wF*cos(qE)*(qA'-qE') - m*e*R*qA'' - mA*(h+R)*(v*qA'+a*qA'')

   (190) Dynamics[9] = Dot( Az>,       System.GetDynamics()    )  % For normal forces.
-> (191) Dynamics[9] = g*cos(theta)*mTotal - FBz - FCz - FFz

   (192) %--------------------------------------------------------------------
   (193) %       Output point Ao's position from point No.
   (194) Variable x' = Dot( Ao.GetVelocity(N), Nx> )
-> (195) x' = v*cos(qA)

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

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

   (200) %--------------------------------------------------------------------
   (201) %       Total mechanical energy (conserved, unless encounter singularity).
   (202) KE = System.GetKineticEnergy()
-> (203) KE = 0.5*J*wB^2 + 0.5*J*wC^2 + 0.5*J*wF^2 + K*qA'^2 + 0.5*IAzz*qA'^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)

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

   (206) MechanicalEnergy = KE + PE
-> (207) MechanicalEnergy = PE + KE

   (208) %--------------------------------------------------------------------
   (209) %       Integration parameters.
   (210) Input  tFinal = 7.0 sec,  tStep = 0.002 sec,  absError = 1.0E-07
   (211) %--------------------------------------------------------------------
   (212) %       Initial values for variables (released from rest with No and Ao coincident).
   (213) Input  qA = 0 deg,   qE = 0.1 deg,  qE' = 0 rad/sec,  v = 0 m/s,  x = 0 m,  y = 0 m
   (214) SolveSetInput( Rolling = 0,  qA' = 0 rad/sec,  wB = 0 rad/sec,  wC = 0 rad/sec,  wF = 0 rad/sec  )

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

   (215) %--------------------------------------------------------------------
   (216) %       List quantities to be output by ODE command.
   (217) OutputPlot  t sec,  qA deg,  qE deg,  v m/s,  MechanicalEnergy Joules,  FBz N,  FCz N,  FFx N,  FFy N,  FFz N,  sqrt(FFx^2+FFy^2)/FFz noUnits
   (218) %--------------------------------------------------------------------
   (219) %       Dynamic simulation with no breaking or steering.
   (220) TB = 0;  TSteer = 0
-> (221) TB = 0
-> (222) TSteer = 0

   (223) ODE( [Dynamics; Dt(Rolling)],  FBx, FBz, FCx, FCz, FFx, FFy, FFz, v', qE'', qA'', wB', wC', wF' ) MGVehicleTricycleFreeMotionForward

   (224) %--------------------------------------------------------------------
   (225) Input  qA := 180 deg
   (226) ODE( [Dynamics; Dt(Rolling)],  FBx, FBz, FCx, FCz, FFx, FFy, FFz, v', qE'', qA'', wB', wC', wF' ) MGVehicleTricycleFreeMotionBackward

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