MGVehicleTricycleKaneEmbeddedWithFeedForwardControl.html   (MotionGenesis input/output).
   (1) % MotionGenesis file:  MGVehicleTricycleKaneEmbeddedWithFeedForwardControl.txt
   (2) % Copyright (c) 2009 Motion Genesis LLC.  All rights reserved.
   (3) % Problem: Statics/dynamics and feed-forward control 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, qE' )
   (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*(qE'-qA')*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> + v*qA'*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> + (v*qA'+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> + (v*qA'+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> + (v*qA'+e*qA'')*Ay> - R*wF'*Ex> + 2*R*wF*(
         qE'-qA')*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> + (v*qA'+sx*qA'')*Ay>

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

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

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

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

   (125) SolveDt( Rolling = 0,  qA', wB, wC, wF )
-> (126) qA' = -tan(qE)*v/e
-> (127) wB = (1/R-b*tan(qE)/(e*R))*v
-> (128) wC = (1+b*tan(qE)/e)*v/R
-> (129) wF = v/(R*cos(qE))
-> (130) qA'' = (e*sin(qE)*qE'*qA'-cos(qE)*qE'*v-sin(qE)*v')/(e*cos(qE))
-> (131) wB' = v'/R + b*(e*sin(qE)*qE'*qA'-cos(qE)*qE'*v-sin(qE)*v')/(e*R*cos(
         qE))
-> (132) wC' = (v'-b*(e*sin(qE)*qE'*qA'-cos(qE)*qE'*v-sin(qE)*v')/(e*cos(qE)))/R
-> (133) wF' = -(e*qE'*qA'-v')/(R*cos(qE))

   (134) %--------------------------------------------------------------------
   (135) %       Replace all gravity forces with single force at Scm
   (136) mTotal = System.GetMass()
-> (137) mTotal = mA + 3*m

   (138) gravityDirection> = sin(theta)*Nx> - cos(theta)*Nz>
-> (139) gravityDirection> = sin(theta)*Nx> - cos(theta)*Nz>

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

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

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

   (148) %--------------------------------------------------------------------
   (149) %       Kane's equations of motion (constraints are embedded)
   (150) DynamicEqns = System.GetDynamicsKane()
-> (151) DynamicEqns[1] = 2*J*b^2*sin(qE)*qE'*(cos(qE)*v-e*sin(qE)*qA')/(e^2*R^2
         *cos(qE)^2) + (IAzz+3*K+2*m*b^2)*sin(qE)*qE'*(cos(qE)*v-e*sin(qE)*qA')
         /(e^2*cos(qE)^2) + K*tan(qE)*qE''/e + (m/cos(qE)^2+J/(R^2*cos(qE)^2)+(
         IAzz+3*K)*tan(qE)^2/e^2+m*(1+b*tan(qE)/e)^2+m*(1-b*tan(qE)/e)^2+mA*(1+a^2
         *tan(qE)^2/e^2)+J*(1+b*tan(qE)/e)^2/R^2+J*(1/R-b*tan(qE)/(e*R))^2)*v'
         - TB*(1/R-b*tan(qE)/(e*R)) - g*sin(theta)*mTotal*(cos(qA)+sx*sin(qA)*
         tan(qE)/e) - J*e*qE'*qA'/(R^2*cos(qE)^2) - m*(e*qA'^2-tan(qE)*(qE'*v-v
         *qA'-e*tan(qE)*qE'*qA')) - mA*a*(qA'^2+tan(qE)*(v*qA'-a*qE'*(cos(qE)*v
         -e*sin(qE)*qA')/(e*cos(qE)))/e)

-> (152) DynamicEqns[2] = K*qE'*(cos(qE)*v-e*sin(qE)*qA')/(e*cos(qE)) + K*qE''
         + K*tan(qE)*v'/e - TSteer

   (153) FactorQuadratic( DynamicEqns,  v, qE', v', qE'' )
-> (154) DynamicEqns[1] = (2*J*b^2*tan(qE)/(e^2*R^2)+(IAzz+3*K+mA*a^2+2*m*b^2)*
         tan(qE)/e^2+sin(qE)*(m+J/R^2+(2*J*b^2/(e^2*R^2)+(IAzz+3*K+mA*a^2+2*m*b^2)
         /e^2)*sin(qE)^2)/cos(qE)^3)*qE'*v + K*tan(qE)*qE''/e + (m/cos(qE)^2+J/
         (R^2*cos(qE)^2)+(IAzz+3*K)*tan(qE)^2/e^2+m*(1+b*tan(qE)/e)^2+m*(1-b*
         tan(qE)/e)^2+mA*(1+a^2*tan(qE)^2/e^2)+J*(1+b*tan(qE)/e)^2/R^2+J*(1/R-b
         *tan(qE)/(e*R))^2)*v' - TB*(1/R-b*tan(qE)/(e*R)) - g*sin(theta)*mTotal
         *(cos(qA)+sx*sin(qA)*tan(qE)/e)

-> (155) DynamicEqns[2] = K*qE'*v/(e*cos(qE)^2) + K*qE'' + K*tan(qE)*v'/e - TSteer

   (156) %--------------------------------------------------------------------
   (157) %       Output point Ao's position from point No
   (158) Variable x' = Dot( Ao.GetVelocity(N), Nx> )
-> (159) x' = cos(qA)*v

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

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

   (164) %--------------------------------------------------------------------
   (165) %       Integration parameters
   (166) Input  tFinal = 9.0 sec,  tStep = 0.002 sec,  absError = 1.0E-07
   (167) %--------------------------------------------------------------------
   (168) %       Initial values for variables (released from rest with No and Ao coincident)
   (169) Input  qA = 0 deg,   qE = 0.1 deg,  qE' = 0 rad/sec,  v = 0 m/s,  x = 0 m,  y = 0 m
   (170) %********************************************************************
   (171) %       Dynamic simulation with feed-forward control.
   (172) %********************************************************************
   (173) %       Desired motion.
   (174) Constant   accelDesired = 15 km/hour/sec
   (175) Specified  vDesired',  qEDesired''
   (176) SetDt( vDesired = accelDesired * t )
-> (177) vDesired = accelDesired*t
-> (178) vDesired' = accelDesired

   (179) SetDt( qEDesired = 5 * pi/180 )
-> (180) qEDesired = 0.08726646
-> (181) qEDesired' = 0
-> (182) qEDesired'' = 0

   (183) %--------------------------------------------------------------------
   (184) %       Construct feed-forward ControlEqns governing TB and TSteer.
   (185) Constant   kp = 1 rad/sec,   zeta = 1 noUnits,  wn = 1 rad/sec
   (186) Av = Dt(vDesired)  +  kp * (vDesired - v)
-> (187) Av = vDesired' + kp*(vDesired-v)

   (188) Aq = DtDt(qEDesired) + 2*zeta*wn*Dt(qEDesired - qE) + wn^2 * (qEDesired - qE)
-> (189) Aq = qEDesired'' + wn^2*(qEDesired-qE) + 2*wn*zeta*(qEDesired'-qE')

   (190) ControlEqns = Evaluate( DynamicEqns,  v' = Av,  qE'' = Aq )
-> (191) ControlEqns[1] = (2*J*b^2*tan(qE)/(e^2*R^2)+(IAzz+3*K+mA*a^2+2*m*b^2)*
         tan(qE)/e^2+sin(qE)*(m+J/R^2+(2*J*b^2/(e^2*R^2)+(IAzz+3*K+mA*a^2+2*m*b^2)
         /e^2)*sin(qE)^2)/cos(qE)^3)*qE'*v + K*tan(qE)*Aq/e + (m/cos(qE)^2+J/(R^2
         *cos(qE)^2)+(IAzz+3*K)*tan(qE)^2/e^2+m*(1+b*tan(qE)/e)^2+m*(1-b*tan(
         qE)/e)^2+mA*(1+a^2*tan(qE)^2/e^2)+J*(1+b*tan(qE)/e)^2/R^2+J*(1/R-b*tan
         (qE)/(e*R))^2)*Av - TB*(1/R-b*tan(qE)/(e*R)) - g*sin(theta)*mTotal*(
         cos(qA)+sx*sin(qA)*tan(qE)/e)

-> (192) ControlEqns[2] = K*qE'*v/(e*cos(qE)^2) + K*Aq + K*tan(qE)*Av/e - TSteer

   (193) %--------------------------------------------------------------------
   (194) %       Simulate vehicle dynamics with feed-forward controller for TB and TSteer.
   (195) OutputPlot  x meters, y meters
   (196) OutputPlot  t sec,  qE degrees,  TB N*m,  TSteer N*m,  v m/s
   (197) ODE( [DynamicEqns; ControlEqns] = 0,   v', qE'', TB, TSteer ) MGVehicleTricycleFeedForward

   (198) %********************************************************************
   (199) %       Static analysis for braking and steering torques
   (200) %********************************************************************
   (201) %       Kane's equations for statics - with simplification
   (202) ZeroStatics = System.GetStaticsKane()
-> (203) ZeroStatics[1] = TB*(1/R-b*tan(qE)/(e*R)) + g*sin(theta)*mTotal*(cos(
         qA)+sx*sin(qA)*tan(qE)/e)
-> (204) ZeroStatics[2] = TSteer

   (205) ZeroStatics[1] := Expand( ZeroStatics[1] * R*cos(qE) )  % Helps simplify
-> (206) ZeroStatics[1] = TB*cos(qE) + g*R*sin(theta)*mTotal*cos(qA)*cos(qE)
         - sin(qE)*(b*TB-g*R*sin(theta)*mTotal*sx*sin(qA))/e

   (207) StaticTorques = Solve( ZeroStatics = 0,  TB, TSteer )
-> (208) StaticTorques[1] = -g*R*sin(theta)*mTotal*(cos(qA)*cos(qE)+sx*sin(qA)*
         sin(qE)/e)/(cos(qE)-b*sin(qE)/e)

-> (209) StaticTorques[2] = 0

   (210) %********************************************************************
   (211) %       Dynamic simulation with no braking or steering  (forward/stable)
   (212) %********************************************************************
   (213) %       Total mechanical energy (should be conserved)
   (214) KE = System.GetKineticEnergy()
-> (215) 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*(qE'-qA')^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)

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

   (218) MechanicalEnergy = KE + PE
-> (219) MechanicalEnergy = PE + KE

   (220) %--------------------------------------------------------------------
   (221) %       List quantities to be output by ODE command.
   (222) ClearOutputPlot()
   (223) Output      t sec,  qA deg,  qE deg,  v m/s,  MechanicalEnergy Joules
   (224) OutputPlot  t sec,  qA deg,  qE deg
   (225) %--------------------------------------------------------------------
   (226) TB := 0;  TSteer := 0
-> (227) TB = 0
-> (228) TSteer = 0

   (229) ODE( DynamicEqns = 0,  v', qE'' )  MGVehicleTricycleFreeMotionForward

   (230) %********************************************************************
   (231) %       Dynamic simulation with no braking or steering  (backward/unstable)
   (232) %       Note: Divide-by-zero at qE = 90 deg causes problems with conservation of MechanicalEnergy
   (233) %********************************************************************
   (234) Input  qA := 180 deg
   (235) OutputPlot  t sec,  MechanicalEnergy  Joules
   (236) ODE( DynamicEqns = 0,  v', qE'' )  MGVehicleTricycleFreeMotionBackward

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