% MotionGenesis file:  MGVehicleSkidKaneEmbedded.txt
% Copyright (c) 2009 Motion Genesis LLC.  All rights reserved.
% Problem: Analysis of Skidding Vehicle.
%--------------------------------------------------------------------
%       Physical declarations.
NewtonianFrame N        % Newtonian reference frame (ground).
RigidBody      A        % Vehicle chassis (Ao is center of axle).
RigidBody      B, C     % Vehicle's rolling wheels.
Point          BN( B )  % Point of wheel B in contact with N.
Point          CN( C )  % Point of wheel C in contact with N.
%--------------------------------------------------------------------
%       Mathematical declarations (constants and variables).
Constant  b = 0.75 m    % Distance between Ao and Bcm (or Ao and Ccm).
Constant  R = 0.35 m    % Wheel radii.
Constant  a = 1.64 m    % Distance from Ao to Acm (A's mass center).
Constant  g = 9.8 m/s^2 % Earth's gravitational acceleration.
Variable  v'            % Ax> measure of Ao's velocity in N.
Variable  wB'           % Ay> measure of B's angular velocity in A.
Variable  wC'           % Ay> measure of C's angular velocity in A.
Variable  theta''       % Vehicle heading angle (car yaw).
Variable  FBx, FBy      % Rolling contact forces on wheel B (Ax>, Ay> directions).
Variable  FCx, FCy      % Rolling contact forces on wheel C (Ax>, Ay> directions).
SetGeneralizedSpeed( v, theta' )
%--------------------------------------------------------------------
%       Mass and inertia.
A.SetMass( mA = 640 kg )
B.SetMass( m = 30 kg )
C.SetMass( m )
A.SetInertia( Acm,    IAxx, IAyy, IAzz = 166 kg*m^2)
B.SetInertia( Bcm, A, K, J = 2.0 kg*m^2, K = 1.0 kg*m^2 )
C.SetInertia( Ccm, A, K, J, K  )
%--------------------------------------------------------------------
%       Rotational kinematics.
A.RotateZ( N, theta )
B.SetAngularVelocityAcceleration( A, wB*Ay> )
C.SetAngularVelocityAcceleration( A, wC*Ay> )
%--------------------------------------------------------------------
%       Translational kinematics.
Ao.SetVelocityAcceleration( N, v*Ax> )
Acm.Translate( Ao,  a*Ax> )
Bcm.Translate( Ao, -b*Ay> )
Ccm.Translate( Ao,  b*Ay> )
BN.SetPositionVelocity(  Bcm,  -R*Az> )
CN.SetPositionVelocity(  Ccm,  -R*Az> )
%--------------------------------------------------------------------
%       Rolling constraints (relates wB and wC to v and theta').
RollingConstraint[1] = Dot( BN.GetVelocity(N), Ax> )
RollingConstraint[2] = Dot( CN.GetVelocity(N), Ax> )
SolveDt( RollingConstraint = 0,  wB, wC )
%--------------------------------------------------------------------
%       Add forces (although none of these appear in DynamicEquations).
%       Note: Gravity and normal contact forces also do not appear in dynamics.
System.AddForceGravity( -g*Az> )
BN.AddForce( FBx*Ax> + FBy*Ay> )
CN.AddForce( FCx*Ax> + FCy*Ay> )
%--------------------------------------------------------------------
%       Form Kane's equations of motion.
DynamicEquations = System.GetDynamicsKane()
%--------------------------------------------------------------------
%       Differential equations governing x and y.
Variable x' = Dot( Ao.GetVelocity(N),  Nx> )
Variable y' = Dot( Ao.GetVelocity(N),  Ny> )
%--------------------------------------------------------------------
%       Integration parameters and initial values.
Input  tFinal = 1 sec,  tStep = 0.01 sec,  absError = 1.0E-07
Input  theta = 0 deg,  x = 0 m,  y = 0 m
Input  theta' = 0.01 rad/sec           % Perturbation in angular rate.
Input  v = -25 m/s                     % Approximately 55 miles/hour.
%--------------------------------------------------------------------
%       List output quantities and solve ODEs.
Output  t sec,  x m,  y m,  v m/sec,  theta deg,  theta' rad/sec
ODE( DynamicEquations,  v',  theta'' )  MGVehicleSkidKaneEmbedded
% Plot MGVehicleSkidKaneEmbedded.1
%--------------------------------------------------------------------
%       At times it is helpful, particularly for control system design
%       and stability analysis, to produce linearized equations of motion.
%       For this system, useful information about the stability of certain
%       motions can be obtained be linearizing the equations of motion
%       in dv, dtheta', perturbations of v and theta', about a nominal (possible)
%       solution. One nominal solution is v=Vo (a constant) and theta'=0.
Constant    Vo               % Forward Speed of Cart v=Vo
Variable    dv', dw'         % Perturbations of v, theta'
%--------------------------------------------------------------------
%       Linearize equations of motion about the nominal solution
LinearizedDynamics = Taylor( DynamicEquations, 1, v=Vo:dv, v'=0:dv', theta'=0:dw, theta''=0:dw')
%--------------------------------------------------------------------
%       Solve for dv' and dw' and  factor the constants in the resulting expression.
Solve( LinearizedDynamics = 0,  dv',  dw' )
Zee( dw',  dw, Vo )
%--------------------------------------------------------------------
%       Closed-form solutions for linearized differential equations.
Constant    dv0, dw0         % Initial values of dv and dw
dv = dv0
dw = dw0*exp(-z1*Vo*t)
%--------------------------------------------------------------------
Save  MGVehicleSkidKaneEmbedded.html
Quit