For a floating-base robot (e.g., a legged system in contact or flight), we cannot treat the base as rigidly fixed to the world. Instead, the entire robot behaves like a multi-body system whose total momentum is determined by all link motions. Centroidal dynamics focuses on the total linear and angular momentum expressed about the system center of mass (COM), often called the centroid.

Let \( N \) be the number of rigid links. For link \( i \), denote its mass by \( m_i \), COM position by \( \mathbf{p}_i \), linear velocity by \( \mathbf{v}_i \), angular velocity by \( \boldsymbol{\omega}_i \), and inertia tensor about its COM by \( \mathbf{I}_i \). The total linear momentum \( \mathbf{p} \) and angular momentum about some reference point \( O \) are

\[ \mathbf{p} = \sum_{i=1}^{N} m_i \mathbf{v}_i, \quad \mathbf{l}_O = \sum_{i=1}^{N} \left( \mathbf{I}_i^O \boldsymbol{\omega}_i + (\mathbf{p}_i - \mathbf{o}) \times m_i \mathbf{v}_i \right), \]

where \( \mathbf{I}_i^O \) is the inertia of link \( i \) about \( O \), and \( \mathbf{o} \) is the position vector of \( O \). Choosing the reference point as the system COM simplifies the structure of the angular momentum and exposes important invariants used in whole-body motion design.

flowchart TD
  L["Link states: poses, masses, inertias"] --> C["Compute total mass M and COM G"]
  C --> J["Compute per-link Jacobians J_i(q)"]
  J --> A["Assemble centroidal matrix A(q) = sum I_i^G * J_i(q)"]
  A --> H["Centroidal momentum h_G = A(q) * qdot"]
  H --> DYN["Centroidal dynamics: hdot_G = sum external wrenches at G"]
        

The main objects of this lesson are:

• The system center of mass and a centroidal frame attached at COM.
• The centroidal momentum vector \( \mathbf{h}_G \in \mathbb{R}^6 \).
• The centroidal momentum matrix \( \mathbf{A}(q) \) such that \( \mathbf{h}_G = \mathbf{A}(q)\dot{\mathbf{q}} \).
• The centroidal momentum theorem: \( \dot{\mathbf{h}}_G \) equals the sum of external wrenches expressed at the COM.

The system total mass \( M \) and COM position \( \mathbf{p}_G \) are defined as

\[ M = \sum_{i=1}^{N} m_i, \quad \mathbf{p}_G = \frac{1}{M} \sum_{i=1}^{N} m_i \mathbf{p}_i. \]

Differentiating \( \mathbf{p}_G \) with respect to time gives the COM velocity \( \mathbf{v}_G \):

\[ \dot{\mathbf{p}}_G = \mathbf{v}_G = \frac{1}{M} \sum_{i=1}^{N} m_i \mathbf{v}_i = \frac{1}{M} \mathbf{p}. \]

Thus the COM linear velocity is directly proportional to the total linear momentum. This property will appear again when we discuss conservation under external forces.

We attach a centroidal frame \( \{G\} \) whose origin is at \( \mathbf{p}_G \). Its orientation can be chosen in various ways (e.g., aligned with the world frame or with the base link). For centroidal dynamics, the most important fact is that the origin is at the COM; the orientation affects matrix expressions but not the conservation properties.

The angular momentum about the COM, denoted \( \mathbf{l}_G \), is

\[ \mathbf{l}_G = \sum_{i=1}^{N} \left( \mathbf{I}_i^G \boldsymbol{\omega}_i + (\mathbf{p}_i - \mathbf{p}_G) \times m_i \mathbf{v}_i \right), \]

where \( \mathbf{I}_i^G \) is the inertia tensor of link \( i \) about its own COM, but expressed in the same orientation as the chosen frame (for instance the world frame or the centroidal frame).

The centroidal momentum vector is the stacked linear and angular momenta:

\[ \mathbf{h}_G = \begin{bmatrix} \mathbf{p} \\ \mathbf{l}_G \end{bmatrix} \in \mathbb{R}^6. \]

In floating-base robotics, \( \mathbf{h}_G \) is a crucial global descriptor of the robot state, often used to define whole-body tasks (e.g., regulating angular momentum in legged locomotion). In this chapter, however, we limit ourselves to modeling \( \mathbf{h}_G \) and its dynamics, without designing control laws.

Using the spatial vector notation introduced in earlier chapters, we represent the motion of each rigid body by a twist \( \mathbf{V}_i^G \in \mathbb{R}^6 \) (angular and linear velocity) expressed at the COM (or shifted to the COM) and the spatial inertia \( \mathbf{I}_i^G \in \mathbb{R}^{6\times 6} \). The spatial momentum of link \( i \) about the COM is

\[ \mathbf{h}_{i,G} = \mathbf{I}_i^G \mathbf{V}_i^G. \]

The total centroidal momentum is then the sum over all links:

\[ \mathbf{h}_G = \sum_{i=1}^{N} \mathbf{h}_{i,G} = \sum_{i=1}^{N} \mathbf{I}_i^G \mathbf{V}_i^G. \]

For a floating-base system, the generalized coordinates are typically \( \mathbf{q} = (\mathbf{q}_b, \mathbf{q}_j) \), where \( \mathbf{q}_b \in \mathbb{R}^6 \) describes the base pose (position and orientation) and \( \mathbf{q}_j \in \mathbb{R}^n \) are the joint coordinates. The generalized velocities are \( \dot{\mathbf{q}} = (\mathbf{v}_b, \dot{\mathbf{q}}_j) \).

For each link we can express its spatial velocity as a linear function of generalized velocities via the link Jacobian:

\[ \mathbf{V}_i^G = \mathbf{J}_i^G(\mathbf{q}) \dot{\mathbf{q}}, \quad \mathbf{J}_i^G(\mathbf{q}) \in \mathbb{R}^{6\times n_q}, \quad n_q = 6 + n. \]

Substituting this into the expression for \( \mathbf{h}_G \), we obtain

\[ \mathbf{h}_G(\mathbf{q}, \dot{\mathbf{q}}) = \sum_{i=1}^{N} \mathbf{I}_i^G \mathbf{J}_i^G(\mathbf{q}) \dot{\mathbf{q}} = \left( \sum_{i=1}^{N} \mathbf{I}_i^G \mathbf{J}_i^G(\mathbf{q}) \right) \dot{\mathbf{q}}. \]

This defines the centroidal momentum matrix \( \mathbf{A}(\mathbf{q}) \in \mathbb{R}^{6\times n_q} \):

\[ \boxed{ \mathbf{h}_G(\mathbf{q}, \dot{\mathbf{q}}) = \mathbf{A}(\mathbf{q}) \dot{\mathbf{q}}, \quad \mathbf{A}(\mathbf{q}) = \sum_{i=1}^{N} \mathbf{I}_i^G \mathbf{J}_i^G(\mathbf{q}). } \]

Key properties.

• \( \mathbf{A}(\mathbf{q}) \) depends only on the configuration \( \mathbf{q} \), because both \( \mathbf{I}_i^G \) (in world/centroidal orientation) and \( \mathbf{J}_i^G(\mathbf{q}) \) are configuration-dependent but not velocity-dependent.
• The relation \( \mathbf{h}_G = \mathbf{A}(\mathbf{q})\dot{\mathbf{q}} \) is linear in generalized velocities.
• The first three rows of \( \mathbf{A} \) map generalized velocities to linear momentum; the last three rows map them to angular momentum about the COM.

Proof that \( \mathbf{A}(\mathbf{q}) \) is independent of velocities. Each link’s spatial velocity is linear in \( \dot{\mathbf{q}} \) with coefficients given by the Jacobian \( \mathbf{J}_i^G(\mathbf{q}) \). Spatial inertia \( \mathbf{I}_i^G \) depends only on the pose (rotation to world/centroidal frame and the COM location). Therefore the product \( \mathbf{I}_i^G(\mathbf{q}) \mathbf{J}_i^G(\mathbf{q}) \) depends only on \( \mathbf{q} \). Summing over links preserves this property.

Let \( {}^G\mathbf{w}_k \in \mathbb{R}^6 \) be the spatial wrench (force and torque) exerted on the robot by external agent \( k \) (e.g., ground contact, environment interaction), expressed at the COM and in the same orientation as \( \mathbf{h}_G \). The centroidal momentum theorem states:

\[ \boxed{ \dot{\mathbf{h}}_G = \sum_{k=1}^{n_{\text{ext}}} {}^G\mathbf{w}_k. } \]

Sketch of proof.

1. For each link \( i \), the Newton–Euler equations in spatial form are

   \[ \dot{\mathbf{h}}_{i,G} = \mathbf{w}_{i,\text{ext}} + \sum_{j \in \mathcal{N}(i)} \mathbf{w}_{ij}, \]

   where \( \mathbf{w}_{i,\text{ext}} \) are external wrenches on link \( i \) and \( \mathbf{w}_{ij} \) are internal wrenches on \( i \) from joint/constraint connections with neighbor link \( j \).
2. Summing over all links,

   \[ \sum_{i=1}^{N} \dot{\mathbf{h}}_{i,G} = \sum_{i=1}^{N} \mathbf{w}_{i,\text{ext}} + \sum_{i=1}^{N} \sum_{j \in \mathcal{N}(i)} \mathbf{w}_{ij}. \]

3. By Newton’s third law, internal forces and torques appear in equal and opposite pairs: for each joint pair \( (i,j) \), \( \mathbf{w}_{ij} + \mathbf{w}_{ji} = 0 \). Therefore the double sum of internal wrenches cancels.
4. Recognizing that \( \sum_i \dot{\mathbf{h}}_{i,G} = \dot{\mathbf{h}}_G \) and that the remaining wrenches are exactly those external to the robot yields the theorem.

If the only external wrench is gravity (uniform field) with force \( \mathbf{f}_g = M \mathbf{g} \) applied at the COM, then

\[ \dot{\mathbf{h}}_G = \begin{bmatrix} M \mathbf{g} \\ \mathbf{0} \end{bmatrix}, \]

i.e., gravity changes the total linear momentum but does not produce torque about the COM in a uniform field. Thus the angular momentum about the COM is conserved during free flight.

Combining with the kinematic relation \( \mathbf{h}_G = \mathbf{A}(\mathbf{q})\dot{\mathbf{q}} \), we can express the derivative in generalized coordinates:

\[ \dot{\mathbf{h}}_G = \mathbf{A}(\mathbf{q}) \ddot{\mathbf{q}} + \dot{\mathbf{A}}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} = \sum_{k=1}^{n_{\text{ext}}} {}^G\mathbf{w}_k. \]

This relation is equivalent to the standard joint-space equations of motion derived in earlier chapters, but expressed in the 6D momentum coordinates rather than in full generalized coordinates.

Consider a planar robot composed of a floating base (link 0) and two planar links (links 1 and 2) with revolute joints. The configuration is \( \mathbf{q} = (x_b, y_b, \theta_b, q_1, q_2) \), and the generalized velocities are \( \dot{\mathbf{q}} = (\dot{x}_b, \dot{y}_b, \dot{\theta}_b, \dot{q}_1, \dot{q}_2) \).

Let \( m_0, m_1, m_2 \) be the masses and \( c_0, c_1, c_2 \) the COM positions of each link in the world frame. The planar system has scalar inertia values \( I_0, I_1, I_2 \) about the out-of-plane axis through each COM. Then the COM position is

\[ \mathbf{p}_G = \frac{1}{M} \left( m_0 c_0 + m_1 c_1 + m_2 c_2 \right), \quad M = m_0 + m_1 + m_2. \]

The linear momentum is

\[ \mathbf{p} = m_0 \mathbf{v}_0 + m_1 \mathbf{v}_1 + m_2 \mathbf{v}_2, \]

and the scalar angular momentum about the COM (out-of-plane axis) is

\[ l_G = \sum_{i=0}^{2} \left( I_i \omega_i + ((c_i - \mathbf{p}_G) \times m_i \mathbf{v}_i)_z \right), \]

where \( (\cdot)_z \) denotes the scalar out-of-plane component of the cross product. The centroidal momentum vector in planar case can be taken as \( \mathbf{h}_G^{\text{2D}} = [p_x, p_y, l_G]^\top \in \mathbb{R}^3 \), and there exists a \( 3\times 5 \) centroidal matrix \( \mathbf{A}^{\text{2D}}(\mathbf{q}) \) such that \( \mathbf{h}_G^{\text{2D}} = \mathbf{A}^{\text{2D}}(\mathbf{q}) \dot{\mathbf{q}} \).

While a full derivation is algebraically lengthy, the structure mirrors the 3D case: per-link planar Jacobians map generalized velocities to link COM velocities and angular velocities; multiplying by mass/inertia and summing yields \( \mathbf{A}^{\text{2D}}(\mathbf{q}) \).

In Python, popular libraries for rigid-body dynamics and floating-base robotics include pinocchio, pybullet, and roboticstoolbox-python. We show (i) how to use a high-level library (Pinocchio-like API) and (ii) how to implement centroidal momentum from scratch given spatial inertias and Jacobians.

6.1 Library-based Implementation (Pinocchio-style)

import numpy as np
import pinocchio as pin

# Load model from URDF
model = pin.buildModelFromUrdf("humanoid.urdf")
data = model.createData()

# Example configuration and velocity (floating base + joints)
q = pin.neutral(model)   # nominal configuration
v = np.zeros(model.nv)   # zero generalized velocity

# Compute centroidal momentum and centroidal momentum matrix
pin.computeCentroidalMomentum(model, data, q, v)
# data.hg is 6D centroidal momentum (linear; angular) about COM
hG = data.hg.copy()

# Compute centroidal momentum matrix A(q)
pin.computeCentroidalMap(model, data, q)
A = data.Ag.copy()  # A has shape (6, nv) so that hG = A @ v

print("Centroidal momentum h_G:", hG)
print("Centroidal momentum matrix A(q):\n", A)
      

The computeCentroidalMomentum and computeCentroidalMap functions internally evaluate link Jacobians, transform spatial inertias to the COM, and assemble \( \mathbf{A}(\mathbf{q}) \) so that \( \mathbf{h}_G = \mathbf{A}(\mathbf{q})\dot{\mathbf{q}} \).

6.2 From-Scratch Centroidal Momentum Assembly

Suppose we have \( N \) links with spatial inertias \( \mathbf{I}_i^G \), Jacobians \( \mathbf{J}_i^G(\mathbf{q}) \), and generalized velocities \( \dot{\mathbf{q}} \). We can assemble \( \mathbf{A}(\mathbf{q}) \) via:

import numpy as np

def assemble_centroidal_matrix(I_list, J_list):
    """
    I_list: list of 6x6 spatial inertia matrices I_i^G
    J_list: list of 6 x nq Jacobians J_i^G(q)
    returns A(q): 6 x nq centroidal momentum matrix
    """
    assert len(I_list) == len(J_list)
    nq = J_list[0].shape[1]
    A = np.zeros((6, nq))
    for I_i, J_i in zip(I_list, J_list):
        A += I_i @ J_i
    return A

def centroidal_momentum(I_list, J_list, qdot):
    A = assemble_centroidal_matrix(I_list, J_list)
    return A @ qdot

# Example with dummy data
N = 3
nq = 8
I_list = [np.eye(6) for _ in range(N)]
J_list = [np.random.randn(6, nq) for _ in range(N)]
qdot = np.random.randn(nq)

hG = centroidal_momentum(I_list, J_list, qdot)
print("h_G:", hG)
      

In a complete implementation, I_list and J_list would be obtained by traversing the kinematic tree, computing COM poses and spatial inertias, and evaluating body Jacobians for each link.

In C++, typical libraries for multi-body dynamics include RBDL, Pinocchio, and DART. Below is a schematic implementation using a C++-style API to assemble the centroidal momentum matrix from spatial inertias and Jacobians.

#include <Eigen/Dense>
#include <vector>

using Eigen::MatrixXd;
using Eigen::VectorXd;

/// Assemble centroidal momentum matrix A(q) from per-link inertias and Jacobians.
MatrixXd assembleCentroidalMatrix(
    const std::vector<MatrixXd>& I_list,  // each 6x6
    const std::vector<MatrixXd>& J_list   // each 6 x nq
) {
    assert(I_list.size() == J_list.size());
    const std::size_t N = I_list.size();
    const int nq = static_cast<int>(J_list[0].cols());

    MatrixXd A = MatrixXd::Zero(6, nq);
    for (std::size_t i = 0; i < N; ++i) {
        A.noalias() += I_list[i] * J_list[i];
    }
    return A;
}

VectorXd centroidalMomentum(
    const std::vector<MatrixXd>& I_list,
    const std::vector<MatrixXd>& J_list,
    const VectorXd& qdot
) {
    MatrixXd A = assembleCentroidalMatrix(I_list, J_list);
    return A * qdot;  // 6x1 vector h_G
}

// Example usage with a generic rigid-body library:
// 1. Traverse kinematic tree;
// 2. For each link i, compute spatial inertia I_i^G and Jacobian J_i^G(q);
// 3. Call centroidalMomentum(I_list, J_list, qdot).
      

Libraries such as RBDL typically provide functions to compute COM position, composite rigid-body inertia, and spatial momentum. The above pattern makes explicit how centroidal momentum is a sum over link contributions, and how this sum yields a configuration-dependent matrix \( \mathbf{A}(\mathbf{q}) \).

While Java has fewer mainstream robotics libraries, we can use numerical libraries such as Ejml or Apache Commons Math to implement the matrix operations. The following sketch uses EJML-style notation for assembling \( \mathbf{A}(\mathbf{q}) \).

import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;
import java.util.List;

public class CentroidalMomentum {

    public static DMatrixRMaj assembleCentroidalMatrix(
            List<DMatrixRMaj> Ilist,
            List<DMatrixRMaj> Jlist) {

        if (Ilist.size() != Jlist.size()) {
            throw new IllegalArgumentException("Ilist and Jlist must have same length");
        }
        int N = Ilist.size();
        int nq = Jlist.get(0).getNumCols();

        DMatrixRMaj A = new DMatrixRMaj(6, nq);
        A.zero();

        for (int i = 0; i < N; ++i) {
            DMatrixRMaj I = Ilist.get(i);  // 6x6
            DMatrixRMaj J = Jlist.get(i);  // 6xnq
            DMatrixRMaj IJ = new DMatrixRMaj(6, nq);
            CommonOps_DDRM.mult(I, J, IJ);
            CommonOps_DDRM.addEquals(A, IJ);
        }
        return A;
    }

    public static DMatrixRMaj centroidalMomentum(
            List<DMatrixRMaj> Ilist,
            List<DMatrixRMaj> Jlist,
            DMatrixRMaj qdot) {

        DMatrixRMaj A = assembleCentroidalMatrix(Ilist, Jlist);
        DMatrixRMaj hG = new DMatrixRMaj(6, 1);
        CommonOps_DDRM.mult(A, qdot, hG);
        return hG;
    }
}
      

A Java-based robotics engine (for instance, humanoid stacks that use Java for high-level reasoning and C++ for low-level dynamics) can use such routines to compute centroidal momentum, provided that spatial inertias and Jacobians are exposed by the underlying kinematics/dynamics layer.

MATLAB’s Robotics System Toolbox represents robots as rigidBodyTree objects. We can compute centroidal momentum by combining COM positions, body Jacobians, and inertias. Below is a MATLAB function that approximates \( \mathbf{A}(\mathbf{q}) \) and \( \mathbf{h}_G \).

function [hG, A] = centroidalMomentum(robot, q, qdot)
% robot: rigidBodyTree (floating-base or fixed-base with added base)
% q    : configuration vector
% qdot : generalized velocity vector (same length as q, with appropriate mapping)

% Number of bodies (excluding base)
bodies = robot.Bodies;
N = numel(bodies);

% Compute COM of each body and total mass
M = 0;
p_list = zeros(3, N);
m_list = zeros(1, N);

for i = 1:N
    bi = bodies{i};
    m_list(i) = bi.Mass;
    M = M + bi.Mass;
    p_list(:,i) = centerOfMass(robot, q, bi.Name);
end

pG = (p_list * m_list.') / M;  % system COM in world frame

% Assemble spatial inertias and Jacobians at COM
nq = numel(q);
A = zeros(6, nq);
for i = 1:N
    bi = bodies{i};
    % Spatial inertia in body frame (mass + inertia about COM)
    Ibody = bodySpatialInertia(bi); % user-defined helper (6x6)

    % Geometric Jacobian for body frame
    J = geometricJacobian(robot, q, bi.Name); % 6 x ndof

    % Transform inertia and Jacobian so that they are expressed at system COM
    % (approximation: neglecting small differences in orientation)
    % Shift linear part by (p_i - pG)
    r = p_list(:,i) - pG;
    X = spatialTransform(r); % 6x6 transform from body COM to system COM

    I_Gi = X.' * Ibody * X;
    J_Gi = X * J;

    A = A + I_Gi * J_Gi;
end

hG = A * qdot;
end
      

Helper functions bodySpatialInertia and spatialTransform build the \( 6\times 6 \) inertia matrix and spatial transform, respectively, using the spatial vector algebra introduced in earlier chapters.

In Simulink, one can wrap the above function in a MATLAB Function block. The Simulink diagram then connects joint states \( (\mathbf{q}, \dot{\mathbf{q}}) \) from a kinematic/dynamic model block to this function block, which outputs \( \mathbf{h}_G \). The resulting signal can feed into monitoring or higher-level planning blocks (without implementing control in this chapter).

flowchart TD
  Q["Joint states (q, qdot)"] --> F["MATLAB Function: centroidalMomentum"]
  F --> H["Output h_G (6x1)"]
  H --> LOG["Logging / analysis"]
        

Wolfram Mathematica is convenient for symbolic derivations of centroidal momentum and conservation properties. Below is a simplified symbolic setup for a planar two-link floating system.

(* Define symbolic parameters *)
ClearAll["Global`*"];
n = 2;
m = Array[m, 0 ;; n];
I = Array[I, 0 ;; n]; (* planar inertias *)

(* Generalized coordinates and velocities *)
q  = {x0[t], y0[t], th0[t], q1[t], q2[t]};
qd = D[q, t];

(* Example COM positions c0, c1, c2 (symbolic) *)
c0 = {x0[t], y0[t]};
c1 = c0 + {l1 Cos[th0[t] + q1[t]]/2, l1 Sin[th0[t] + q1[t]]/2};
c2 = c1 + {l2 Cos[th0[t] + q1[t] + q2[t]]/2, 
           l2 Sin[th0[t] + q1[t] + q2[t]]/2};

(* Velocities *)
v0 = D[c0, t];
v1 = D[c1, t];
v2 = D[c2, t];

(* Angular velocities (out-of-plane) *)
om0 = D[th0[t], t];
om1 = om0 + D[q1[t], t];
om2 = om1 + D[q2[t], t];

(* Linear momentum *)
pLin = m[[0]] v0 + m[[1]] v1 + m[[2]] v2 // Simplify;

(* System COM *)
Mtot = Total[m];
pG = (m[[0]] c0 + m[[1]] c1 + m[[2]] c2)/Mtot // Simplify;

(* Angular momentum about COM (scalar z-component) *)
cross2D[a_, b_] := a[[1]] b[[2]] - a[[2]] b[[1]];

lG = I[[0]] om0 + I[[1]] om1 + I[[2]] om2
     + cross2D[c0 - pG, m[[0]] v0]
     + cross2D[c1 - pG, m[[1]] v1]
     + cross2D[c2 - pG, m[[2]] v2] // Simplify;

(* Collect momenta in vector form *)
hG = {pLin[[1]], pLin[[2]], lG} // Simplify;

(* Extract coefficient matrix A(q) such that hG == A.qd *)
A = CoefficientArrays[hG, qd][[2]];
      

The resulting matrix A is the planar analogue of the centroidal momentum matrix. Mathematica’s symbolic capabilities allow checking invariance properties, such as showing that the time derivative of angular momentum about the COM is independent of internal actuation torques under free flight.

In this lesson we introduced the centroidal description of floating-base robots. We derived the system COM and centroidal frame, defined the centroidal momentum vector \( \mathbf{h}_G \), and showed how it can be expressed as \( \mathbf{h}_G = \mathbf{A}(\mathbf{q})\dot{\mathbf{q}} \) via the centroidal momentum matrix. Using spatial Newton–Euler equations, we proved that the rate of change \( \dot{\mathbf{h}}_G \) depends only on external wrenches, implying conservation of angular momentum about the COM in free flight. We then illustrated how to compute centroidal momentum numerically in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica. These constructions are foundational for subsequent modeling of contacts and full floating-base dynamics in legged robots.