From Lesson 1, a rigid body is characterized by mass \( m \), center of mass (CoM) position \( \mathbf{c} \in \mathbb{R}^3 \), and a rotational inertia tensor about the CoM \( \mathbf{I}_C \in \mathbb{R}^{3\times 3} \), expressed in some body-fixed frame. In modern multi-body dynamics it is more convenient to work with spatial vectors:

• Spatial velocity (twist) \( \mathbf{v} = \begin{bmatrix} \boldsymbol{\omega} \\ \mathbf{v}_O \end{bmatrix} \in \mathbb{R}^6 \), concatenating angular velocity \( \boldsymbol{\omega} \) and linear velocity of a reference point \( O \).
• Spatial momentum (wrench) \( \mathbf{h} = \begin{bmatrix} \mathbf{l}_O \\ \mathbf{p} \end{bmatrix} \in \mathbb{R}^6 \), concatenating angular momentum about \( O \) and linear momentum.

The spatial inertia tensor \( \mathbf{I}_O^s \in \mathbb{R}^{6\times 6} \) at a point \( O \) is defined by the linear map

\[ \mathbf{h}_O = \mathbf{I}_O^s \, \mathbf{v}_O , \]

and it encodes both mass distribution and reference point. The kinetic energy can be written compactly as

\[ T = \tfrac{1}{2}\,\mathbf{v}_O^\top \mathbf{I}_O^s \mathbf{v}_O , \]

which generalizes the familiar expressions for translational and rotational kinetic energy. This 6×6 object is the fundamental building block used in efficient recursive dynamics (e.g., spatial Newton–Euler, articulated-body algorithms).

flowchart TD
  A["Rigid body parameters (m, com c, Ic)"] --> B["Build skew matrix S(c)"]
  B --> C["Assemble 6x6 spatial inertia Is"]
  C --> D["Map twist v to momentum h = Is * v"]
  D --> E["Compute kinetic energy T = 0.5 * v^T * Is * v"]
  E --> F["Use Is in recursive dynamics algorithms"]
        

Let frame \( C \) be attached to the body with origin at the CoM and axes aligned with a principal inertia frame (or any body-fixed orientation). Consider the twist expressed at \( C \):

\[ \mathbf{v}_C = \begin{bmatrix} \boldsymbol{\omega} \\ \mathbf{v}_C \end{bmatrix} , \quad \mathbf{h}_C = \begin{bmatrix} \mathbf{l}_C \\ \mathbf{p} \end{bmatrix}. \]

In the CoM frame, linear momentum satisfies \( \mathbf{p} = m \mathbf{v}_C \) and angular momentum about the CoM is \( \mathbf{l}_C = \mathbf{I}_C \boldsymbol{\omega} \). Hence the spatial momentum is

\[ \mathbf{h}_C = \begin{bmatrix} \mathbf{I}_C & \mathbf{0} \\ \mathbf{0} & m \mathbf{I}_3 \end{bmatrix} \begin{bmatrix} \boldsymbol{\omega} \\ \mathbf{v}_C \end{bmatrix}. \]

This yields the spatial inertia at the CoM:

\[ \mathbf{I}_C^s = \begin{bmatrix} \mathbf{I}_C & \mathbf{0} \\ \mathbf{0} & m \mathbf{I}_3 \end{bmatrix}. \]

The kinetic energy in this frame is simply

\[ T = \tfrac{1}{2}\,\boldsymbol{\omega}^\top \mathbf{I}_C \boldsymbol{\omega} + \tfrac{1}{2}\, m \, \|\mathbf{v}_C\|^2 = \tfrac{1}{2}\,\mathbf{v}_C^\top \mathbf{I}_C^s \mathbf{v}_C. \]

Thus, in the CoM frame, the spatial inertia matrix is block-diagonal, separating rotational inertia and translational inertia.

Robot dynamics typically use link-attached frames whose origins are not at the CoM. Let frame \( O \) be any frame whose origin is related to the CoM by \( \mathbf{c} = \overrightarrow{OC} \). We define the skew-symmetric matrix

\[ \mathbf{S}(\mathbf{c}) = \begin{bmatrix} 0 & -c_3 & c_2 \\ c_3 & 0 & -c_1 \\ -c_2 & c_1 & 0 \end{bmatrix}, \quad \mathbf{S}(\mathbf{c}) \mathbf{x} = \mathbf{c} \times \mathbf{x}. \]

Relations between spatial quantities at \( C \) and \( O \) (same orientation, pure translation) are:

• Velocity shift:

  \[ \mathbf{v}_C = \begin{bmatrix} \mathbf{I}_3 & \mathbf{0} \\ -\mathbf{S}(\mathbf{c}) & \mathbf{I}_3 \end{bmatrix} \mathbf{v}_O. \]

• Momentum shift:

  \[ \mathbf{h}_O = \begin{bmatrix} \mathbf{I}_3 & \mathbf{S}(\mathbf{c}) \\ \mathbf{0} & \mathbf{I}_3 \end{bmatrix} \mathbf{h}_C. \]

Denote these 6×6 matrices by \( \mathbf{X}_{C\leftarrow O} \) for velocity and \( \mathbf{A}_{O\leftarrow C} \) for momentum. We have:

\[ \mathbf{v}_C = \mathbf{X}_{C\leftarrow O} \mathbf{v}_O, \quad \mathbf{h}_O = \mathbf{A}_{O\leftarrow C} \mathbf{h}_C. \]

Since \( \mathbf{h}_C = \mathbf{I}_C^s \mathbf{v}_C \), substitution yields

\[ \mathbf{h}_O = \mathbf{A}_{O\leftarrow C} \mathbf{I}_C^s \mathbf{X}_{C\leftarrow O} \mathbf{v}_O \equiv \mathbf{I}_O^s \mathbf{v}_O. \]

Therefore,

\[ \mathbf{I}_O^s = \mathbf{A}_{O\leftarrow C} \mathbf{I}_C^s \mathbf{X}_{C\leftarrow O}. \]

Carrying out the block multiplication using \( \mathbf{I}_C^s \) from Section 2 yields

\[ \mathbf{I}_O^s = \begin{bmatrix} \mathbf{I}_C - m\,\mathbf{S}(\mathbf{c})^2 & m\,\mathbf{S}(\mathbf{c}) \\ -m\,\mathbf{S}(\mathbf{c}) & m\,\mathbf{I}_3 \end{bmatrix}. \]

Using the identity \( -\mathbf{S}(\mathbf{c})^2 = \mathbf{S}(\mathbf{c}) \mathbf{S}(\mathbf{c})^\top \), one often writes the top-left block equivalently as \( \mathbf{I}_C + m\,\mathbf{S}(\mathbf{c}) \mathbf{S}(\mathbf{c})^\top \), which is precisely the inertia tensor about point \( O \) obtained via the parallel-axis theorem.

flowchart TD
  C["CoM frame C: IsC = diag(Ic, m I3)"] --> X["Velocity shift: vC = X_CO * vO"]
  C --> A2["Momentum shift: hO = A_OC * hC"]
  X --> M["Compose IsO = A_OC * IsC * X_CO"]
  A2 --> M
  M --> USE["Store IsO in link inertial model"]
        

4.1 Symmetry

The spatial inertia at any point is symmetric: \( \mathbf{I}_O^s = (\mathbf{I}_O^s)^\top \). For the block form in Section 3:

• \( \mathbf{I}_C \) is symmetric by definition.
• \( \mathbf{S}(\mathbf{c}) \) is skew-symmetric, so \( \mathbf{S}(\mathbf{c})^\top = -\mathbf{S}(\mathbf{c}) \).

The off-diagonal blocks satisfy \( (m\,\mathbf{S}(\mathbf{c}))^\top = -m\,\mathbf{S}(\mathbf{c}) \), which matches the lower-left block, so the full 6×6 matrix is symmetric.

4.2 Positive Semi-Definiteness

Physical inertias must correspond to non-negative kinetic energy for all twists. Consider the kinetic energy at the CoM:

\[ T = \tfrac{1}{2}\,\boldsymbol{\omega}^\top \mathbf{I}_C \boldsymbol{\omega} + \tfrac{1}{2} m \|\mathbf{v}_C\|^2. \]

With \( m > 0 \) and \( \mathbf{I}_C \) positive semi-definite, \( T \ge 0 \) for all nonzero \( \mathbf{v}_C \). Using \( \mathbf{v}_C = \mathbf{X}_{C\leftarrow O} \mathbf{v}_O \), we get

\[ T = \tfrac{1}{2}\,\mathbf{v}_O^\top \left(\mathbf{X}_{C\leftarrow O}^\top \mathbf{I}_C^s \mathbf{X}_{C\leftarrow O}\right) \mathbf{v}_O = \tfrac{1}{2}\,\mathbf{v}_O^\top \mathbf{I}_O^s \mathbf{v}_O. \]

Thus \( \mathbf{I}_O^s \) is positive semi-definite and positive definite whenever the body has nonzero mass and \( \mathbf{I}_C \) is positive definite on the space of angular velocities.

4.3 Energy Invariance Under Reference Point Shifts

Let \( T_C \) and \( T_O \) denote kinetic energy computed in frames \( C \) and \( O \), respectively:

\[ T_C = \tfrac{1}{2}\,\mathbf{v}_C^\top \mathbf{I}_C^s \mathbf{v}_C, \quad T_O = \tfrac{1}{2}\,\mathbf{v}_O^\top \mathbf{I}_O^s \mathbf{v}_O. \]

Using \( \mathbf{v}_C = \mathbf{X}_{C\leftarrow O} \mathbf{v}_O \) and the definition of \( \mathbf{I}_O^s \) in Section 3:

\[ T_C = \tfrac{1}{2}\,\mathbf{v}_O^\top \mathbf{X}_{C\leftarrow O}^\top \mathbf{I}_C^s \mathbf{X}_{C\leftarrow O} \mathbf{v}_O = \tfrac{1}{2}\,\mathbf{v}_O^\top \mathbf{I}_O^s \mathbf{v}_O = T_O. \]

Hence kinetic energy is invariant under shifts of the reference point, provided that spatial velocity and inertia are transformed consistently.

In Python we can construct spatial inertias directly using numpy. Robotics libraries such as pinocchio or the Python Robotics Toolbox provide similar abstractions, but it is instructive to see the bare implementation.

import numpy as np

def skew(v):
    """Return 3x3 skew-symmetric matrix S(v) such that S(v) @ x = v x x."""
    v = np.asarray(v).reshape(3,)
    return np.array([[0.0,    -v[2],  v[1]],
                     [v[2],   0.0,   -v[0]],
                     [-v[1],  v[0],  0.0]])

def spatial_inertia(m, c, Ic):
    """
    Construct 6x6 spatial inertia Is at reference point O, given:
      m  : scalar mass
      c  : 3-vector from O to center of mass C
      Ic : 3x3 rotational inertia about C, expressed in frame O's axes
    """
    c = np.asarray(c).reshape(3,)
    Ic = np.asarray(Ic).reshape(3, 3)

    S = skew(c)
    I3 = np.eye(3)

    # Top-left block: Ic - m * S @ S  (equivalently: Ic + m * S @ S.T)
    I11 = Ic - m * S @ S
    I12 = m * S
    I21 = -m * S
    I22 = m * I3

    top = np.hstack((I11, I12))
    bottom = np.hstack((I21, I22))
    return np.vstack((top, bottom))

def kinetic_energy(Is, v):
    """
    Compute kinetic energy T = 0.5 * v^T * Is * v
    where v is a 6-vector spatial velocity at O.
    """
    v = np.asarray(v).reshape(6,)
    return 0.5 * float(v.T @ Is @ v)

def momentum(Is, v):
    """
    Spatial momentum h = Is * v.
    """
    v = np.asarray(v).reshape(6,)
    return Is @ v

# Example: simple link
m = 5.0  # kg
c = np.array([0.0, 0.0, 0.2])  # CoM 20 cm along z from joint frame
Ic = np.diag([0.1, 0.2, 0.15])  # inertia about CoM (kg m^2)

Is = spatial_inertia(m, c, Ic)
print("Spatial inertia Is:\n", Is)

# A sample spatial velocity: omega = [0, 0, 1], v_O = [0.1, 0, 0]
v = np.array([0.0, 0.0, 1.0, 0.1, 0.0, 0.0])
h = momentum(Is, v)
T = kinetic_energy(Is, v)

print("Spatial momentum h:", h)
print("Kinetic energy T:", T)
      

This implementation exactly follows the derivation in Section 3 and can be used as a building block inside forward or inverse dynamics routines developed in later chapters.

In C++, a common practice is to represent spatial inertia with Eigen matrices and provide helper functions for skew matrices and energy computations.

#include <Eigen/Dense>

using Matrix3 = Eigen::Matrix<double, 3, 3>;
using Matrix6 = Eigen::Matrix<double, 6, 6>;
using Vector3 = Eigen::Matrix<double, 3, 1>;
using Vector6 = Eigen::Matrix<double, 6, 1>;

inline Matrix3 skew(const Vector3& v) {
    Matrix3 S;
    S << 0.0,   -v(2),  v(1),
          v(2),  0.0,   -v(0),
         -v(1),  v(0),  0.0;
    return S;
}

struct SpatialInertia {
    double  m;   // mass
    Vector3 c;   // O to CoM
    Matrix3 Ic;  // inertia about CoM, expressed in O's axes

    Matrix6 matrix() const {
        Matrix3 S = skew(c);
        Matrix3 I3 = Matrix3::Identity();

        Matrix3 I11 = Ic - m * (S * S); // or Ic + m * S * S.transpose()
        Matrix3 I12 = m * S;
        Matrix3 I21 = -m * S;
        Matrix3 I22 = m * I3;

        Matrix6 Is;
        Is.setZero();
        Is.block<3,3>(0,0) = I11;
        Is.block<3,3>(0,3) = I12;
        Is.block<3,3>(3,0) = I21;
        Is.block<3,3>(3,3) = I22;
        return Is;
    }

    Vector6 momentum(const Vector6& v) const {
        return matrix() * v;
    }

    double kineticEnergy(const Vector6& v) const {
        Vector6 h = momentum(v);
        return 0.5 * v.dot(h);
    }
};

// Example usage
int main() {
    SpatialInertia I;
    I.m = 5.0;
    I.c = Vector3(0.0, 0.0, 0.2);
    I.Ic = Matrix3::Zero();
    I.Ic.diagonal() << 0.1, 0.2, 0.15;

    Matrix6 Is = I.matrix();
    Vector6 v;
    v << 0.0, 0.0, 1.0, 0.1, 0.0, 0.0;

    Vector6 h = I.momentum(v);
    double T = I.kineticEnergy(v);
    std::cout << "Is =\n" << Is << std::endl;
    std::cout << "h = " << h.transpose() << std::endl;
    std::cout << "T = " << T << std::endl;
    return 0;
}
      

This pattern directly supports articulated-body algorithms, where each link stores a SpatialInertia object and associated transforms.

In Java, matrix libraries such as EJML (org.ejml) are often used. Below is an implementation using EJML's SimpleMatrix. For teaching purposes, we keep the code as transparent as possible.

import org.ejml.simple.SimpleMatrix;

public class SpatialInertia {
    private double m;
    private SimpleMatrix c;   // 3x1
    private SimpleMatrix Ic;  // 3x3

    public SpatialInertia(double mass, SimpleMatrix com, SimpleMatrix IcCom) {
        this.m = mass;
        this.c = com;
        this.Ic = IcCom;
    }

    public static SimpleMatrix skew(SimpleMatrix v) {
        double vx = v.get(0);
        double vy = v.get(1);
        double vz = v.get(2);
        double[][] data = {
                {0.0,   -vz,   vy},
                {vz,    0.0,  -vx},
                {-vy,   vx,   0.0}
        };
        return new SimpleMatrix(data);
    }

    public SimpleMatrix matrix() {
        SimpleMatrix S = skew(c);
        SimpleMatrix I3 = SimpleMatrix.identity(3);

        SimpleMatrix I11 = Ic.minus(S.mult(S).scale(m));
        SimpleMatrix I12 = S.scale(m);
        SimpleMatrix I21 = S.scale(-m);
        SimpleMatrix I22 = I3.scale(m);

        SimpleMatrix Is = new SimpleMatrix(6, 6);
        Is.insertIntoThis(0, 0, I11);
        Is.insertIntoThis(0, 3, I12);
        Is.insertIntoThis(3, 0, I21);
        Is.insertIntoThis(3, 3, I22);
        return Is;
    }

    public SimpleMatrix momentum(SimpleMatrix v) {
        // v is 6x1
        return matrix().mult(v);
    }

    public double kineticEnergy(SimpleMatrix v) {
        SimpleMatrix h = momentum(v);
        return 0.5 * v.dot(h);
    }

    public static void main(String[] args) {
        double m = 5.0;
        SimpleMatrix c = new SimpleMatrix(new double[][] {
                {0.0}, {0.0}, {0.2}
        });
        SimpleMatrix Ic = SimpleMatrix.diag(0.1, 0.2, 0.15);

        SpatialInertia I = new SpatialInertia(m, c, Ic);
        SimpleMatrix Is = I.matrix();
        SimpleMatrix v = new SimpleMatrix(new double[][] {
                {0.0}, {0.0}, {1.0}, {0.1}, {0.0}, {0.0}
        });

        SimpleMatrix h = I.momentum(v);
        double T = I.kineticEnergy(v);

        System.out.println("Is =\n" + Is);
        System.out.println("h =\n" + h);
        System.out.println("T = " + T);
    }
}
      

Such a class integrates naturally into Java-based simulators or control software that require explicit rigid-body dynamics modeling.

MATLAB is widely used in control and robotics. We first provide a function to construct the spatial inertia tensor, then show how this relates to Simulink or Simscape Multibody models.

function Is = spatialInertia(m, c, Ic)
%SPATIALINERTIA Construct 6x6 spatial inertia at reference frame O.
%   m  : scalar mass
%   c  : 3x1 vector from O to CoM C
%   Ic : 3x3 inertia tensor about C

    S = skew3(c);
    I3 = eye(3);

    I11 = Ic - m * (S * S);   % or Ic + m * S * S'
    I12 = m * S;
    I21 = -m * S;
    I22 = m * I3;

    Is = [I11, I12;
          I21, I22];
end

function S = skew3(v)
%SKEW3 3x3 skew-symmetric matrix S(v) such that S(v) * x = v x x.
    vx = v(1); vy = v(2); vz = v(3);
    S = [0,   -vz,  vy;
         vz,   0,  -vx;
        -vy,   vx,  0];
end

% Example script
m = 5.0;
c = [0; 0; 0.2];
Ic = diag([0.1, 0.2, 0.15]);
Is = spatialInertia(m, c, Ic)

% Spatial velocity v = [omega; vO]
v = [0; 0; 1; 0.1; 0; 0];
h = Is * v;
T = 0.5 * v' * Is * v;
      

In Simscape Multibody, each rigid body block takes classical inertial parameters (mass, CoM, inertia about CoM). Internally the simulator constructs spatial inertias similar to Is. For a Robotics System Toolbox rigidBody, one can directly set:

body = rigidBody("link1");
% inertia vector: [Ixx Iyy Izz Ixy Iyz Izx]
inertiaVec = [0.1 0.2 0.15 0 0 0];
setFixedTransform(body.Joint, trvec2tform([0 0 0])); % example
body.Inertia = [inertiaVec, m, c'];  % [Ixx Iyy Izz Ixy Iyz Izx m cx cy cz]
      

Simulink models can wrap these MATLAB objects, and the underlying spatial inertia representation is used automatically when computing dynamics.

Mathematica is well suited to symbolic manipulation of spatial inertia tensors, which is particularly useful for verifying properties such as symmetry and positive definiteness.

(* Skew-symmetric matrix for cross product *)
Skew[v_List] := {
  {0, -v[[3]], v[[2]]},
  {v[[3]], 0, -v[[1]]},
  {-v[[2]], v[[1]], 0}
};

SpatialInertia[m_, c_List, Ic_] := Module[
  {S, I3, I11, I12, I21, I22},
  S = Skew[c];
  I3 = IdentityMatrix[3];
  I11 = Ic - m S.S;  (* or Ic + m S.Transpose[S] *)
  I12 = m S;
  I21 = -m S;
  I22 = m I3;
  ArrayFlatten[{ {I11, I12}, {I21, I22} }]
];

(* Example with symbolic parameters *)
m = Symbol["m", Positive];
c = {cx, cy, cz};
Ic = { {Ixx, Ixy, Ixz}, {Ixy, Iyy, Iyz}, {Ixz, Iyz, Izz} };

Is = SpatialInertia[m, c, Ic];

(* Verify symmetry *)
Simplify[Transpose[Is] - Is]

(* Kinetic energy with a symbolic twist v = {wx, wy, wz, vx, vy, vz} *)
v = {wx, wy, wz, vx, vy, vz};
T = Simplify[1/2 v . Is . v];
      

Symbolic expressions like T can then be inspected or simplified to confirm equivalence with textbook kinetic energy formulas.

Spatial inertia tensors provide a compact, coordinate-invariant representation of rigid-body inertia that couples translational and rotational effects and explicitly handles shifts of reference frames. Starting from classical inertial parameters \( (m, \mathbf{c}, \mathbf{I}_C) \), we derived the 6×6 spatial inertia at both the CoM and an arbitrary frame and showed that kinetic energy and momentum can be expressed as quadratic and linear forms in the spatial velocity. We established symmetry, positive semi-definiteness, and energy invariance, and we implemented the spatial inertia construction in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica. These tensors are the fundamental objects manipulated by modern articulated-body dynamics algorithms used later in this course.