Spatial Vector Algebra and Inertia

In Chapters 3 and 4, we unified translation and rotation into 6-dimensional Twists ( ) and Wrenches ( ). While the geometry of the Screw Axis inherently…

Spatial Vector Algebra and Inertia

Featherstone’s Spatial Algebra

In Chapters 3 and 4, we unified translation and rotation into 6-dimensional Twists (\(\twist\)) and Wrenches (\(\wrench\)). While the geometry of the Screw Axis inherently links them, operating on these 6-dimensional vectors using standard 3D linear algebra creates a bloated, confusing mess of cross products and offsets.

To fix this, Roy Featherstone formalized Spatial Vector Algebra. Rather than writing Newton’s equations as two separate 3D systems (one for \(F = ma\) and one for \(\tau = I\alpha + \omega \times I\omega\)), Spatial Algebra unifies them into a single, breathtakingly elegant 6D format.

Note

In 3D mechanics, linear motion (\(v\)) and angular motion (\(\omega\)) behave differently. In Spatial Algebra, they are fused. A “Spatial Vector” does not simply have a direction and a magnitude; it fundamentally exists as a line floating in the universe with a pitch. When math is done in this space, the separation between “turning” and “pushing” disappears completely.

The Spatial Cross Product

In classical 3D mechanics, the cross product \(a \times b\) finds a perpendicular vector. In 6D Spatial Algebra, because Twists (\(\twist\)) and Wrenches (\(\wrench\)) transform differently under coordinate frame shifts, there are actually two spatial cross products: one for motion vectors (like velocity) and one for force vectors.

The spatial cross product for motion (\(\times\)) is defined as a \(6 \times 6\) matrix operator. If a frame has an angular velocity \(\omega\) and linear velocity \(v\), its Spatial Motion Cross operator \([\twist \times]\) is:

\[\begin{equation} [\twist \times] = \begin{bmatrix} [\omega] & 0 \\ [v] & [\omega] \end{bmatrix} \in \Reals^{6 \times 6} \end{equation}\] where \([\omega]\) and \([v]\) are the standard \(3 \times 3\) skew-symmetric matrices.

The spatial force cross product (\(\times^*\)) is simply the negative transpose of the motion operator: \[\begin{equation} [\twist \times^*] = -[\twist \times]^T = \begin{bmatrix} [\omega] & [v] \\ 0 & [\omega] \end{bmatrix} \end{equation}\]

By keeping forces and motions in their respective dual spaces, calculating the Coriolis forces tearing at the joints of a swinging golf club requires only a single line of 6D matrix multiplication rather than pages of complex calculus.

The Spatial Inertia Matrix

Just as mass \(m\) resists linear acceleration, and a \(3 \times 3\) inertia tensor \(I\) resists angular acceleration, standard 6D Spatial Algebra unifies them into a single \(6 \times 6\) Spatial Inertia Matrix \(\mathcal{I}_s\).

For a rigid body with mass \(m\), a center of mass located at vector \(\bm{c}\), and a \(3 \times 3\) rotational inertia tensor \(I_c\) about that center of mass, the total Spatial Inertia Matrix \(\mathcal{I}_s\) about the origin of the frame is:

\[\begin{equation} \mathcal{I}_s = \begin{bmatrix} I_c + m[\bm{c}][\bm{c}]^T & m[\bm{c}] \\ m[\bm{c}]^T & m \mathbf{1}_{3 \times 3} \end{bmatrix} \in \Reals^{6 \times 6} \end{equation}\]

This single matrix fundamentally dictates how hard it is to alter the trajectory of a physical object crossing through any dimensional axis.

Worked Example: Spatial Inertia of a Cylinder

Let us build a computational example verifying these principles. In robot design, human body modeling (like the 15-DOF golfer), or physics engines (MuJoCo), we must frequently construct \(6 \times 6\) inertia tensors from basic geometric primitive shapes.

Here is the python code required to compute the Spatial Inertia Matrix for a solid cylinder (such as a robotic forearm or a humerus bone) of mass \(m\), radius \(r\), and length \(L\), whose center of mass is shifted along the Z-axis by a distance \(d\).

import numpy as np

def skew(v):
    # Converts a 3D vector into a 3x3 skew-symmetric matrix
    return np.array([
        [ 0,    -v[2],  v[1]],
        [ v[2],  0,    -v[0]],
        [-v[1],  v[0],  0   ]
    ])

def cylinder_spatial_inertia(m, r, L, d):
    # 1. Rotational inertia tensor at the Center of Mass (CoM)
    I_xx = (1/12) * m * (3*r**2 + L**2)
    I_yy = I_xx
    I_zz = (1/2) * m * r**2
    I_c = np.diag([I_xx, I_yy, I_zz])
    
    # 2. Center of Mass offset vector
    c = np.array([0.0, 0.0, d])
    c_skew = skew(c)
    
    # 3. Assemble the 6x6 Spatial Inertia Matrix
    I_s = np.zeros((6, 6))
    
    # Top Left: I_c + m * [c][c]^T (Parallel Axis Theorem)
    I_s[0:3, 0:3] = I_c + m * (c_skew @ np.transpose(c_skew))
    
    # Top Right: m * [c]
    I_s[0:3, 3:6] = m * c_skew
    
    # Bottom Left: m * [c]^T
    I_s[3:6, 0:3] = m * np.transpose(c_skew)
    
    # Bottom Right: m * Identity
    I_s[3:6, 3:6] = m * np.eye(3)
    
    return I_s

# Example: A 2kg forearm cylinder, 5cm radius, 30cm long, offset 15cm down
I_spatial = cylinder_spatial_inertia(m=2.0, r=0.05, L=0.30, d=0.15)
print("6x6 Spatial Inertia Matrix:\n", np.round(I_spatial, 4))

When you execute this code, the Parallel Axis Theorem is automatically handled natively inside the \(6 \times 6\) matrix. The resulting matrix perfectly defines how hard that cylindrical arm is to accelerate, twist, wrench, or twist-push simultaneously around the joint.