Spinning Top

This notebook shows how to model a spinning top using the quaternion representation for the top attitude and constrained Lagrangian mechanics. We follow the very same logic as that outlined in the getting started notebook.

# Our stuff
import heyoka as hy
import quivira as qv

# Third party
import numpy as np

# Plots and visualization
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from IPython.display import HTML

Generalized Coordinates

We describe the top by the Cartesian position of its center of mass and a unit quaternion for its attitude:

\[\bm q = (x, y, z, q_1, q_2, q_3, q_4).\]

The quaternion is scalar-first: \(q_1\) is its scalar component and \((q_2,q_3,q_4)\) are its vector components.

# Generalized coordinates: the Cartesian position of the bead.
x, y, z, q1, q2, q3, q4 = hy.make_vars("x", "y", "z", "q1", "q2", "q3", "q4")
dx, dy, dz, dq1, dq2, dq3, dq4 = hy.make_vars(
    "dx", "dy", "dz", "dq1", "dq2", "dq3", "dq4"
)
q = [x, y, z, q1, q2, q3, q4]
dq = [dx, dy, dz, dq1, dq2, dq3, dq4]

Kinetic Energy

The kinetic energy includes center-of-mass translation and rotation about the center of mass:

\[T = \frac{1}{2}m(\dot{x}^2 + \dot{y}^2 + \dot{z}^2) + \frac{1}{2}\bm\omega_B^{\mathsf T}\mathbf I\bm\omega_B.\]

Here \(\bm\omega_B\) is the body’s angular velocity in body coordinates and \(\mathbf I\) is the body-frame inertia tensor. The implementation adds a radial quaternion-rate term to regularize the mass matrix; it vanishes for unit-quaternion motion.

# Relate quaternion rates to the top angular velocity in the body frame.
omega = np.array([
    2 * (q1 * dq2 - q2 * dq1 - q3 * dq4 + q4 * dq3),
    2 * (q1 * dq3 + q2 * dq4 - q3 * dq1 - q4 * dq2),
    2 * (q1 * dq4 - q2 * dq3 + q3 * dq2 - q4 * dq1),
])

# Use an axisymmetric inertia tensor with body z as the symmetry axis.
m = 1.0  # kg
Ixx = Iyy = 1.0
Izz = 0.5
I = np.diag([Ixx, Iyy, Izz])

T = 0.5 * (m * (dx**2 + dy**2 + dz**2) + omega.T @ I @ omega)

# If we were to use the above kinetic energy expression
# the mass matrix would be singular as any varation of the
# quaternion norm would not affect the kinetic energy. We must then
# regularize the kinetic energy with a term that accounts for quaternion norm
# changes. We do this adding a term proportional to the radial quaternion rate, a
# q dq. Its zero anyway during a valid motion!
quaternion_radial_rate = sum(
    qi * dqi for qi, dqi in zip(q[3:], dq[3:])
)
T += 0.5 * Izz * quaternion_radial_rate**2

Potential energy.

With world \(z\) chosen as the vertical axis, gravity gives the potential energy

\[V = mgz.\]

g = 9.80665  # m/s^2
V = m * g * z

Lagrangian

The Lagrangian is the difference between kinetic and potential energy:

\[L = T - V.\]

L = T - V

Holonomic constraints.

We fix the top’s contact point at the origin. World \(z\) is vertical, and body \(z\) is the symmetry axis. The contact point is a distance \(d\) below the center of mass in body coordinates, so

\[\mathbf r_P = \mathbf r_C + \mathbf R \mathbf r_{P/C} = \mathbf 0, \qquad \mathbf r_{P/C} = (0, 0, -d).\]

We also constrain the quaternion to have unit norm.

# First we define R as the rotation matrix describing the orientation of the Top
R = np.array([
    [1 - 2*(q3**2 + q4**2), 2*(q2*q3 - q1*q4), 2*(q2*q4 + q1*q3)],
    [2*(q2*q3 + q1*q4), 1 - 2*(q2**2 + q4**2), 2*(q3*q4 - q1*q2)],
    [2*(q2*q4 - q1*q3), 2*(q3*q4 + q1*q2), 1 - 2*(q2**2 + q3**2)]
])
# Now we can express the position of the point of contact of the Top with the ground as
d=1.
rC = np.array([x, y, z])
rPC = np.array([0, 0, -d])  # Vector from the center of mass to the point of contact in the body frame
rp = rC + R @ rPC
# And assemble the constraints
F = [rp[0], rp[1], rp[2], q1**2 + q2**2 + q3**2 + q4**2 - 1]

EOM

We assemble the constrained equations of motion for the top. The three contact constraints fix the point of contact, and the fourth constraint enforces unit quaternion norm.

eom = qv.lagrange_eom(L, F, q, dq)

Taylor Integrator

We instantiate a heyoka Taylor adaptive integrator from the assembled equations of motion.

ta = hy.taylor_adaptive(eom, compact_mode=True)

Initial conditions

We start with a small tilt and a fast spin about the body symmetry axis. The center position is chosen so that the contact point is at the origin, and the quaternion rates are computed from the initial body angular velocity.

# Set a small tilt and a fast spin about the body symmetry axis.
theta0 = 0.1
phi0 = 0.0
psi0 = 0.0
omega_body0 = np.array([0.0, 0.0, 2.])

# Build a scalar-first quaternion from the initial Euler angles.
quaternion0 = np.array([
    np.cos(theta0 / 2) * np.cos((phi0 + psi0) / 2),
    np.sin(theta0 / 2) * np.cos((phi0 - psi0) / 2),
    np.sin(theta0 / 2) * np.sin((phi0 - psi0) / 2),
    np.cos(theta0 / 2) * np.sin((phi0 + psi0) / 2),
])

# Convert body angular velocity into quaternion rates for a scalar-first quaternion.
wx, wy, wz = omega_body0
omega_matrix = np.array([
    [0.0, -wx, -wy, -wz],
    [wx, 0.0, wz, -wy],
    [wy, -wz, 0.0, wx],
    [wz, wy, -wx, 0.0],
])
quaternion_rate0 = 0.5 * omega_matrix @ quaternion0

# Place the center opposite the contact offset using the world direction of body z.
qw0, qx0, qy0, qz0 = quaternion0
body_z_world0 = np.array([
    2 * (qx0 * qz0 + qw0 * qy0),
    2 * (qy0 * qz0 - qw0 * qx0),
    1 - 2 * (qx0**2 + qy0**2),
])
r_C0 = d * body_z_world0

# Assemble coordinates, quaternion, zero center velocity, and quaternion rates.
ic = np.concatenate((r_C0, quaternion0, np.zeros(3), quaternion_rate0))

Simulation

We set the integrator to the constraint-consistent initial state and sample the motion from \(t=0\) to \(t=30\). The final item returned by propagate_grid is the array of sampled states. The next cell visualizes the center-of-mass trajectory.

ta.time = 0.0
ta.state[:] = ic
tgrid = np.linspace(0.0, 30.0, 1000)
sol = ta.propagate_grid(tgrid)
states = sol[-1]

com_x = states[:, 0]
com_y = states[:, 1]

fig, ax = plt.subplots(figsize=(6, 6))
ax.plot(com_x, com_y, color="0.8", linewidth=1)
trail, = ax.plot([], [], color="tab:blue", linewidth=1.5)
top, = ax.plot([], [], "o", color="tab:orange", markersize=7)

padding = 0.1 * max(np.ptp(com_x), np.ptp(com_y), 0.1)
ax.set_xlim(com_x.min() - padding, com_x.max() + padding)
ax.set_ylim(com_y.min() - padding, com_y.max() + padding)
ax.set_aspect("equal", adjustable="box")
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.grid(True)


def animate(frame):
    top.set_data([com_x[frame]], [com_y[frame]])
    trail.set_data(com_x[: frame + 1], com_y[: frame + 1])
    return top, trail


frame_step = max(1, len(states) // 200)
animation = FuncAnimation(
    fig,
    animate,
    frames=range(0, len(states), frame_step),
    interval=25,
    blit=True,
)
display(HTML(animation.to_jshtml()))
plt.close(fig)

Once Loop Reflect

Analysis

We check the holonomic constraints over the simulated trajectory. The first three residuals measure the contact-point position, while the fourth measures deviation from unit quaternion norm.

constraint_func = hy.cfunc(list(F), vars=q)
constraint_inputs = np.ascontiguousarray(states[:, :7].T)
constraint_values = constraint_func(constraint_inputs).T
print("Initial maximum constraint residual:", np.max(np.abs(constraint_values[0])))
print("Maximum constraint residual during motion:", np.max(np.abs(constraint_values)))

fig, ax = plt.subplots(figsize=(8, 4))
for index in range(constraint_values.shape[1]):
    ax.semilogy(
        tgrid,
        np.maximum(np.abs(constraint_values[:, index]), np.finfo(float).tiny),
        label=f"$|F_{index + 1}|$",
    )

ax.set_xlabel("Time")
ax.set_ylabel("Absolute constraint residual")
ax.set_title("Spinning-top constraint drift")
ax.grid(True, which="both")
ax.legend()
plt.ylim(1e-20, 1e-6)
plt.show()

Initial maximum constraint residual: 0.0
Maximum constraint residual during motion: 1.1428635815491361e-12