% Daniel Kawano, Rose-Hulman Institute of Technology % Last modified: Feb 06, 2016 clear all close all clc % Specify symbolic parameters and symbolic variables that are functions of % time: syms psi(t) theta(t) phi(t) Dpsi(t) Dtheta(t) Dphi(t) syms m g lambdat lambdaa L3 assume((m > 0) & (g > 0) & (lambdat > 0) & (lambdaa > 0) & (L3 > 0)) % (1) Angular velocity kinematics: % Relate the space-fixed basis {E1,E2,E3} to the corotational basis % {e1,e2,e3} using a 3-1-3 set of Euler angles: R1 = [cos(psi), sin(psi), 0; -sin(psi), cos(psi), 0; 0, 0, 1]; R2 = [1, 0, 0; 0, cos(theta), sin(theta); 0, -sin(theta), cos(theta)]; R3 = [cos(phi), sin(phi), 0; -sin(phi), cos(phi), 0; 0, 0, 1]; % Express the angular velocity vector in terms of {e1,e2,e3}: omega = simplify((R3*R2*R1)*[0; 0; diff(psi)] + ... (R3*R2)*[diff(theta); 0; 0] + R3*[0; 0; diff(phi)]); % (2) Kinematic constraints: % The top's point of contact P with the ground has zero velocity. % Integrable constraints: vCOM = simplify([0; 0; 0] + transpose(R3*R2*R1)*cross(omega, [0; 0; L3])); Dx1 = [1, 0, 0]*vCOM; Dx2 = [0, 1, 0]*vCOM; Dx3 = [0, 0, 1]*vCOM; x1 = simplify(int(Dx1)); x2 = simplify(int(Dx2)); x3 = simplify(int(Dx3)); % Simplify the kinematic equations for future manipulation: omega = subs(omega, [diff(psi), diff(theta), diff(phi)], [Dpsi, Dtheta, ... Dphi]); Dx1 = subs(Dx1, [diff(psi), diff(theta), diff(phi)], [Dpsi, Dtheta, Dphi]); Dx2 = subs(Dx2, [diff(psi), diff(theta), diff(phi)], [Dpsi, Dtheta, Dphi]); Dx3 = subs(Dx3, [diff(psi), diff(theta), diff(phi)], [Dpsi, Dtheta, Dphi]); % (3) Balance of linear momentum: % Solve for the normal (constraint) force, as well as the lateral % (constraint) forces required to keep the top's contact point P fixed: F1 = m*diff(Dx1); F2 = m*diff(Dx2); N = m*diff(Dx3) + m*g; % (4) Balance of angular momentum with respect to the top's mass center: % Taking the top to be axisymmetric, evaluate the absolute time derivative % of the angular momentum about the mass center: H = diag([lambdat, lambdat, lambdaa])*omega; omegaRF = omega; DH = diff(H) + cross(omegaRF, H); % Sum moments about the mass center: sumM = cross([0; 0; -L3], (R3*R2*R1)*[F1; F2; N]); % Construct the second-order ODEs for rotational motion of the top: ODEsRot = DH == sumM; % (5) Manipulate the system of ODEs into a form suitable for numerical % integration: % Express the second-order ODEs in first-order form: ODEsRot1 = simplify(subs(ODEsRot, [diff(psi), diff(theta), diff(phi)], ... [Dpsi, Dtheta, Dphi])); % Manipulating the first two ODEs ultimately yields a cleaner form for the % state equations: ODEsRot1 = [sin(phi), -cos(phi), 0; cos(phi), sin(phi), 0; 0, 0, 1]*ODEsRot1; % Relate the state variables in first-order form: ODEsRot2 = [diff(psi) == Dpsi; diff(theta) == Dtheta; diff(phi) == Dphi]; % Compile the state equations and arrange the state variables: StateEqns = simplify([ODEsRot1; ODEsRot2]); StateVars = [Dpsi; Dtheta; Dphi; psi; theta; phi]; % Express the state equations in mass-matrix form, M(t,Y)*Y'(t) = F(t,Y): [Msym, Fsym] = massMatrixForm(StateEqns, StateVars); Msym = simplify(Msym) Fsym = simplify(Fsym) % Convert M(t,Y) and F(t,Y) to symbolic function handles with the input % parameters specified: M = odeFunction(Msym, StateVars, m, g, lambdat, lambdaa, L3); F = odeFunction(Fsym, StateVars, m, g, lambdat, lambdaa, L3); % Save M(t,Y) and F(t,Y): save lagrange_top_ODEs.mat Msym Fsym StateVars M F