% MotionGenesis file:  MGRollingDiskFBD.txt
% Copyright (c) 2018 Motion Genesis LLC.  All rights reserved.
% Problem:  Simulation of a rolling disk with free-body diagrams.
%--------------------------------------------------------------------
NewtonianFrame  A                 % Horizontal plane.
RigidFrame      B, C              % Heading and tilt frames.
RigidBody       D                 % Rolling disk.
Point           DA( D )           % Point of D in contact with A.
%--------------------------------------------------------------------
Variable  wx',  wy',  wz'         % Disk D's angular velocity in A.
Variable  x'',  y''               % Locates contact point DA from No.
Variable  qH',  qL',  qS'         % Heading angle, lean angle, spin angle.
Constant  r = 0.343 meters        % Radius of disk (13.5 inches).
Constant  g = 9.8   m/s^2         % Earth's gravitational acceleration.
Constant  b = 0 N*m*s/rad         % Aerodynamic damping on disk.
D.SetMass( m = 2 kg )
D.SetInertia( DCm,  C,  J = 0.25*m*r^2,  I = 2*J,  J )
%--------------------------------------------------------------------
%       Rotational kinematics.
B.RotatePositiveZ( A, qH )
C.RotateNegativeX( B, qL )
D.RotatePositiveY( C, qS )
%---------------------------------------------------------------
%       For efficient dynamics, make a change of variable.
wDA> = D.GetAngularVelocity( A )
ChangeVariables[1] = wx - Dot( wDA>, Cx> )
ChangeVariables[2] = wy - Dot( wDA>, Cy> )
ChangeVariables[3] = wz - Dot( wDA>, Cz> )
Solve( ChangeVariables = 0,  qH', qL', qS' )
%---------------------------------------------------------------
%       Use the simpler version of angular velocity from here on.
D.SetAngularVelocity( A,  wx*Cx> + wy*Cy> + wz*Cz> )
%--------------------------------------------------------------------
%       Form angular acceleration (differentiating in D is more efficient).
alf_D_A> = Dt( D.GetAngularVelocity(A), D )
%---------------------------------------------------------------
%       Translational kinematics.
Dcm.Translate( Ao,  x*Ax> + y*Ay> + r*Cz> )
DA.SetPositionVelocity( Dcm,  -r*Cz> )
%---------------------------------------------------------------
%       Motion constraints (D rolls on A at point DA).
RollingConstraint[1] = Dot( DA.GetVelocity(A), Bx> )
RollingConstraint[2] = Dot( DA.GetVelocity(A), By> )
SolveDt( RollingConstraint = 0,  x', y' )
%--------------------------------------------------------------------
%       Motion constraints make v_Dcm_A> and a_Dcm_A> simpler if expressed in C.
v_Dcm_A> := Express( Explicit(v_Dcm_A>, wx, wy, wz),  C )
a_Dcm_A> := Express( Explicit(a_Dcm_A>, wx, wy, wz),  C )
%---------------------------------------------------------------
%       Add relevant forces and aerodynamic damping torque.
%       Note: Aerodynamic damping affects wx most, wy least.
Dcm.AddForce( -m*g*Az> )
D.AddTorque(  -b*( 2*wx*Cx> + 0.2*wy*Cy> + wz*Cz> ) )
%--------------------------------------------------------------------
%       Equations of motion via moments about point DA.
Dynamics[1] = Dot( Cx>,  D.GetDynamics(DA) )
Dynamics[2] = Dot( Cy>,  D.GetDynamics(DA) )
Dynamics[3] = Dot( Cz>,  D.GetDynamics(DA) )
%--------------------------------------------------------------------
%       Solve equations of motion for unknowns.
Solve( Dynamics = 0,  wx', wy', wz')
%--------------------------------------------------------------------
%       Integration parameters and initial values.
Input  tFinal = 12 sec,  tStep = 0.05 sec,  absError = 1.0E-08
Input  x = 0 m,  y = 0 m,  qH = 0 deg,  qL = 10 deg,  qS = 0 deg
Input  wx = 0 rad/sec,  wy = 5 rad/sec,  wz = 0 rad/sec
%---------------------------------------------------------------
%       List output quantities and solve ODEs.
Output  t sec,  x meters,  y meters,  qL degrees,  qH degrees
ODE() RollingDiskFBD
%--------------------------------------------------------------------
Save MGRollingDiskFBD.html
Quit