% MotionGenesis file:  RattlebackKane.txt
% Copyright (c) 2009 Motion Genesis LLC.  All rights reserved.
%--------------------------------------------------------------------
NewtonianFrame  N
RigidBody       B            % Rattleback.
RigidFrame      S            % Ellipsoidal surface of B (So is centroid of surface).
Point           Bn(B)        % Point of B in contact with N.
%--------------------------------------------------------------------
Variable  wx', wy', wz'           % Measures of B's angular velocity in N.
Variable  q1', q2', q3'           % Orientation angles of B in N.
Variable  sx', sy', sz', Lambda'  % Variables that describe the surface of B.
Constant  h = 1.0 cm              % Distance between So and Bcm.
Constant  g = 9.81 m/sec^2        % Gravity.
B.SetMassInertia( m = 1.0 kg,  Ixx = 17 kg*cm^2, Iyy = 2 kg*cm^2, Izz = 16 kg*cm^2, 0, Iyz = 0.2 kg*cm^2, 0 )
SetGeneralizedSpeed( wx, wy, wz )
%--------------------------------------------------------------------
%   Rotational kinematics.
B.SetAngularVelocityAcceleration( N,  wx*Bx> + wy*By> + wz*Bz> )
B.SetRotationMatrixODE( N,  BodyXYZ,  q1, q2, q3 )
SetDt( C11 = N_B[1,1] );  SetDt( C12 = N_B[1,2] );  SetDt( C13 = N_B[1,3] )
SetDt( C21 = N_B[2,1] );  SetDt( C22 = N_B[2,2] );  SetDt( C23 = N_B[2,3] )
SetDt( C31 = N_B[3,1] );  SetDt( C32 = N_B[3,2] );  SetDt( C33 = N_B[3,3] )
B_N := Transpose( [C11, C12, C13;  C21, C22, C23;  C31, C32, C33] )
%--------------------------------------------------------------------
%   Position vectors.
So.SetPosition( Bcm,  h*Bx> )
Bn.SetPosition( So,   sx*Bx> + sy*By> + sz*Bz> )
%--------------------------------------------------------------------
%   Surface Geometry: The outward normal to the surface S is parallel
%   to the gradient of the function that characterizes S's surface.
%   And the outward normal to S at Bn is in the -Nx> direction.
%   Lambda is a convenient scalar quantity (intermediate variable).
Constant  a+ = 2.0 cm,  b+ = 20 cm,  c+ = 3.0 cm
Surface = (sx/a)^2 + (sy/b)^2 + (sz/c)^2 - 1
Gradient> = D(Surface, sx)*Bx> + D(Surface, sy)*By> + D(Surface, sz)*Bz>
GradientIsNegativeNxDirection> = Gradient> + 2*Lambda*Nx>
GradientEquations = Dot( GradientIsNegativeNxDirection>,  [Bx>; By>; Bz>] )
SolveDt( GradientEquations = 0,  sx, sy, sz )
%--------------------------------------------------------------------
%   By inspecting the rattleback schematic, it can be seen that when q1 = q2 = q3 = 0,
%   point Bn is below So so sx must be negative. Since sx = a^2*C11*Lambda and
%   since C11 = 1 when q1 = q2 = q3 = 0, Lambda must be the positive root.
FactorQuadratic( Surface, Lambda )
SolveQuadraticPositiveRootDt( Surface,  Lambda )
%--------------------------------------------------------------------
%   Translational kinematics.
Bn.SetVelocity( N, 0> )           % Automatically imposes motion constraint.
Bcm.SetVelocity( N, Bn )
%--------------------------------------------------------------------
%   Relevant forces on B for generalized force calculation.
System.AddForceGravity( -g*Nx> )
%--------------------------------------------------------------------
%   Form Kane's equations of motion.
Dynamics = System.GetDynamicsKane()
%--------------------------------------------------------------------
%   Expressions for quantities to be output by ODE command.
delta = AngleBetweenUnitVectors( Nx>, Bx> )   % Wobble/inclination angle
%--------------------------------------------------------------------
%   Input numerical integration parameters and initial values.
Input  tFinal = 5 sec, tStep = 0.01 sec,  absError = 1.0E-07,  relError = 1.0E-07
Input  q1 = 0.0 deg,    q2 = 0.5 deg,      q3 = -0.5 deg
Input  wx = 5 rad/sec,  wy = 0.0 rad/sec,  wz = 0.0 rad/sec
%--------------------------------------------------------------------
%   List output quantities and solve ODEs (or write MATLAB, C, Fortran, ... code).
OutputPlot  t seconds,  q1 degrees,  delta degrees
ODE( Dynamics = 0,  wx', wy', wz' )  RattlebackKane.m
%--------------------------------------------------------------------
%   Note: With the given initial values and parameters, the simulation
%   predicts spin reversal and the following values at t = 5.0 seconds.
%   t (sec)        q1 (degees)    delta (degrees)
%   5.000000       157.5726       2.813488
%--------------------------------------------------------------------
Save RattlebackKane.html
Quit


% Slightly different form if declare using Specified rather than Variable.
% Specified  sx', sy', sz', Lambda'
Kane1 = wy*(Iyz*wy+Izz*wz) + (Ixx+m*(sy^2+sz^2))*wx' - m*g*(sy*C13-sz*C12) - wz*(Iyy*wy+Iyz*wz) - m*(sz*(sx'*wz+(h+sx)*wx*wy-sz'*wx-wz*(sy*wz-sz*wy))+sy*(sx'*wy-sy'*wx-(h+sx)*wx*wz-wy*(sy*wz-sz*wy))) - m*sy*(h+sx)*wy' - m*sz*(h+sx)*wz'
Kane2 = wx*(Ixx*wz-Iyz*wy-Izz*wz) + (Iyz-m*sy*sz)*wz' + (Iyy+m*(sz^2+(h+sx)^2))*wy' - m*g*(sz*C11-(h+sx)*C13) - m*(sz*(sy'*wz-sz'*wy-sy*wx*wy-wz*(sz*wx-(h+sx)*wz))+(h+sx)*(sy'*wx+sy*wy*wz-sx'*wy-wx*(sz*wx-(h+sx)*wz))) - m*sy*(h+sx)*wx'
Kane3 = m*g*(sy*C11-(h+sx)*C12) + m*(sy*(sy'*wz-sz'*wy-sz*wx*wz-wy*(sy*wx-(h+sx)*wy))+(h+sx)*(sx'*wz+sz*wy*wz-sz'*wx-wx*(sy*wx-(h+sx)*wy))) + (Iyz-m*sy*sz)*wy' + (Izz+m*(sy^2+(h+sx)^2))*wz' - wx*(Ixx*wy-Iyy*wy-Iyz*wz) - m*sz*(h+sx)*wx'
ShouldBeZero[1] = Dynamics[1] - Rhs(Kane1)
ShouldBeZero[2] = Dynamics[1] - Rhs(Kane2)
ShouldBeZero[3] = Dynamics[1] - Rhs(Kane3)