Chapter 13: Inertia Modeling and Parameterization
Lesson 4: Physical Consistency (Positive Definiteness)
This lesson formalizes what it means for rigid-body inertial parameters and robot inertia matrices to be physically consistent. We connect kinetic energy, spatial inertia tensors, and 3D inertia matrices to positive definiteness, derive necessary and sufficient conditions, and show how to implement consistency checks in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica.
1. Conceptual Overview of Physical Consistency
Inertia modeling assigns mass and its distribution in space to each link of a robot. A set of inertial parameters is physically consistent if it could arise from some nonnegative mass density in Euclidean space. At the level of the manipulator equations, this appears as the positive-definite property of the joint-space inertia matrix \( \mathbf{H}(\mathbf{q}) \), so that kinetic energy is strictly positive for all nonzero generalized velocities:
\[ T(\mathbf{q},\dot{\mathbf{q}}) = \tfrac{1}{2}\,\dot{\mathbf{q}}^{\mathsf{T}} \mathbf{H}(\mathbf{q})\,\dot{\mathbf{q}}, \qquad \dot{\mathbf{q}}\neq \mathbf{0} \;\Longrightarrow\; T(\mathbf{q},\dot{\mathbf{q}}) > 0. \]
For each rigid link, the same idea appears in the spatial inertia matrix \( \boldsymbol{\mathcal{I}}\in\mathbb{R}^{6\times 6} \), which maps spatial twists to spatial momentum and defines kinetic energy:
\[ T = \tfrac{1}{2}\,\mathbf{v}^{\mathsf{T}} \boldsymbol{\mathcal{I}}\mathbf{v}, \qquad \mathbf{v}\neq \mathbf{0} \;\Longrightarrow\; T > 0. \]
The goal of this lesson is to make these statements precise, derive algebraic conditions on inertial parameters, and show how to implement them as robust numerical checks.
flowchart TD
A["Mass distribution in 3D space"] --> B["Rigid-body inertial parameters (m, COM, 3x3 inertia)"]
B --> C["Spatial inertia matrix 6x6"]
C --> D["Link kinetic energy T = 1/2 v^T I v"]
D --> E["Joint-space inertia H(q) from recursive algorithms"]
E --> F["Check: T > 0 for all nonzero velocities?"]
F --> G["Physically consistent model (energy-based test)"]
2. Kinetic Energy and Positive Definiteness of \( \mathbf{H}(\mathbf{q}) \)
For an \(n\)-DOF manipulator with generalized coordinates \( \mathbf{q}\in\mathbb{R}^n \) and velocities \( \dot{\mathbf{q}}\in\mathbb{R}^n \), the kinetic energy has the standard quadratic form
\[ T(\mathbf{q},\dot{\mathbf{q}}) = \tfrac{1}{2}\,\dot{\mathbf{q}}^{\mathsf{T}} \mathbf{H}(\mathbf{q})\,\dot{\mathbf{q}}, \qquad \mathbf{H}(\mathbf{q}) = \mathbf{H}(\mathbf{q})^{\mathsf{T}}. \]
Definition (positive definiteness). A symmetric matrix \( \mathbf{H}(\mathbf{q}) \) is positive definite (denoted \( \mathbf{H}(\mathbf{q}) \succ 0 \)) if
\[ \forall \dot{\mathbf{q}}\neq \mathbf{0}: \quad \dot{\mathbf{q}}^{\mathsf{T}} \mathbf{H}(\mathbf{q})\,\dot{\mathbf{q}} > 0. \]
Equivalently, all eigenvalues of \( \mathbf{H}(\mathbf{q}) \) are strictly positive, or the Cholesky factorization exists with a full-rank upper-triangular factor. Under physically consistent inertial parameters, the recursive algorithms of Chapter 12 produce such a positive-definite \( \mathbf{H}(\mathbf{q}) \) for all \( \mathbf{q} \).
Because \( T \) is the stored kinetic energy, a violation of positive definiteness (e.g. negative eigenvalues of \( \mathbf{H}(\mathbf{q}) \)) would correspond to negative energy along some velocity direction, which is unphysical for a passive mechanical system.
3. Rigid-Body \(3\times 3\) Inertia and Principal Moments
Consider a single rigid body with mass density \( \rho(\mathbf{r})\geq 0 \). Let \( \mathbf{c}\in\mathbb{R}^3 \) be its center of mass and let \( \mathbf{I}_C \) be the inertia tensor about the center-of-mass frame:
\[ \mathbf{I}_C = \int_{\mathcal{B}} \bigl( \|\mathbf{r}-\mathbf{c}\|^2 \mathbf{I}_3 - (\mathbf{r}-\mathbf{c})(\mathbf{r}-\mathbf{c})^{\mathsf{T}} \bigr)\rho(\mathbf{r})\,\mathrm{d}V. \]
In coordinates, \( \mathbf{I}_C \) is a symmetric \(3\times 3\) matrix:
\[ \mathbf{I}_C = \begin{bmatrix} I_{xx} & I_{xy} & I_{xz}\\ I_{xy} & I_{yy} & I_{yz}\\ I_{xz} & I_{yz} & I_{zz} \end{bmatrix}. \]
Because it is obtained from integrating nonnegative quantities, \( \mathbf{I}_C \) is symmetric positive semidefinite. If the body has nonzero mass and nonzero size, \( \mathbf{I}_C \) is positive definite.
Diagonalizing \( \mathbf{I}_C \) gives principal axes \( \mathbf{R}\in\mathrm{SO}(3) \) and principal moments \( J_1,J_2,J_3 \):
\[ \mathbf{I}_C = \mathbf{R} \begin{bmatrix} J_1 & 0 & 0\\ 0 & J_2 & 0\\ 0 & 0 & J_3 \end{bmatrix} \mathbf{R}^{\mathsf{T}}, \qquad J_i \geq 0. \]
A classical representation introduces the second moments of mass \( L_x,L_y,L_z\geq 0 \) so that in a principal frame:
\[ J_1 = L_y + L_z,\quad J_2 = L_x + L_z,\quad J_3 = L_x + L_y. \]
From \( L_x,L_y,L_z\geq 0 \) we obtain the triangle inequalities for the principal moments:
\[ \begin{aligned} J_1 &\leq J_2 + J_3,\\ J_2 &\leq J_1 + J_3,\\ J_3 &\leq J_1 + J_2. \end{aligned} \]
Thus, physical consistency of a \(3\times 3\) inertia tensor requires not only positive definiteness but also these geometric inequalities, expressing that no single principal inertia can exceed the sum of the other two.
4. Spatial Inertia Matrix and Energy-Based Proof of SPD
In spatial vector notation (introduced in Chapter 12), a rigid body with mass \( m \), center of mass \( \mathbf{c} \) expressed in some body frame, and inertia about the center of mass \( \mathbf{I}_C \) has a spatial inertia \( \boldsymbol{\mathcal{I}}\in\mathbb{R}^{6\times 6} \) given by
\[ \boldsymbol{\mathcal{I}} = \begin{bmatrix} \mathbf{I}_C + m[\mathbf{c}]_{\times}[\mathbf{c}]_{\times}^{\mathsf{T}} & m[\mathbf{c}]_{\times}\\[4pt] m[\mathbf{c}]_{\times}^{\mathsf{T}} & m\mathbf{I}_3 \end{bmatrix}, \]
where \( [\mathbf{c}]_{\times} \) is the skew-symmetric matrix representing cross products with \( \mathbf{c} \).
Let \( \mathbf{v} = [\boldsymbol{\omega}^{\mathsf{T}},\;\mathbf{v}_o^{\mathsf{T}}]^{\mathsf{T}} \) be the spatial twist about the body frame origin, with angular velocity \( \boldsymbol{\omega} \) and linear velocity at the origin \( \mathbf{v}_o \). The kinetic energy can be rewritten in terms of the velocity of the center of mass \( \mathbf{v}_C = \mathbf{v}_o + \boldsymbol{\omega}\times\mathbf{c} \):
\[ T = \tfrac{1}{2}\,\mathbf{v}^{\mathsf{T}} \boldsymbol{\mathcal{I}}\mathbf{v} = \tfrac{1}{2}\,\boldsymbol{\omega}^{\mathsf{T}} \mathbf{I}_C\,\boldsymbol{\omega} + \tfrac{1}{2}\,m\|\mathbf{v}_C\|^2. \]
Assume \( m > 0 \) and \( \mathbf{I}_C \succ 0 \). Then
- if \( \boldsymbol{\omega} = \mathbf{0} \) but \( \mathbf{v}_C\neq \mathbf{0} \), we have \( T = \tfrac{1}{2}m\|\mathbf{v}_C\|^2 > 0 \);
- if \( \mathbf{v}_C = \mathbf{0} \) but \( \boldsymbol{\omega}\neq \mathbf{0} \), we have \( T = \tfrac{1}{2}\boldsymbol{\omega}^{\mathsf{T}} \mathbf{I}_C\,\boldsymbol{\omega} > 0 \);
- if both are nonzero, the sum of two positive terms is positive.
Hence \( T > 0 \) for all \( \mathbf{v}\neq \mathbf{0} \), proving that \( \boldsymbol{\mathcal{I}}\succ 0 \).
An alternative proof uses the spatial transformation to the center-of-mass frame \( \mathcal{F}_C \). Let \( \mathbf{X}_{BC}\in\mathbb{R}^{6\times 6} \) be the spatial transformation from \( \mathcal{F}_C \) to the body frame. Then
\[ \boldsymbol{\mathcal{I}} = \mathbf{X}_{BC}^{-\mathsf{T}} \begin{bmatrix} \mathbf{I}_C & 0\\ 0 & m\mathbf{I}_3 \end{bmatrix} \mathbf{X}_{BC}^{-1}. \]
Since congruence with a nonsingular matrix preserves positive definiteness, \( \boldsymbol{\mathcal{I}}\succ 0 \) if and only if \( \mathbf{I}_C\succ 0 \) and \( m > 0 \).
5. Parameter-Space Constraints and Minimal Parameter Sets
For each link, it is common to collect the inertial parameters into a parameter vector
\[ \boldsymbol{\pi} = \begin{bmatrix} m & mc_x & mc_y & mc_z & I_{xx} & I_{yy} & I_{zz} & I_{xy} & I_{xz} & I_{yz} \end{bmatrix}^{\mathsf{T}}, \]
where \( m \) is mass, \( m\mathbf{c} = (mc_x,mc_y,mc_z)^{\mathsf{T}} \) are the first moments, and \( I_{ij} \) are the inertia components about the chosen body frame (often the center of mass).
Physical consistency constraints. A parameter vector \( \boldsymbol{\pi} \) is physically consistent if and only if:
- \( m \geq 0 \);
- \( \mathbf{I}_C \) (built from \( I_{ij} \)) is symmetric positive semidefinite;
- the principal moments of \( \mathbf{I}_C \) satisfy the triangle inequalities;
- when combined into the spatial inertia \( \boldsymbol{\mathcal{I}}(\boldsymbol{\pi}) \), the resulting matrix is positive semidefinite (and positive definite if \( m>0 \) and the body has nonzero size).
Chapter 13, Lesson 3 discussed minimal parameter sets obtained by eliminating dependencies in the full parameter vector using the structure of the dynamic model. Physical consistency must hold in the full parameterization, but constraints can be pushed into the minimal parameter space as convex constraints (e.g., via linear matrix inequalities).
6. Algorithmic Flow for Consistency Checking and Projection
In practice, inertial parameters may come from CAD, identification, or manual specification. Numerical noise and modeling mistakes may yield an inertia that is not positive definite. A simple pipeline is:
- Symmetrize the inertia matrices.
- Check eigenvalues (or Cholesky factors) for positive definiteness.
- If necessary, project the inertia onto the nearest positive-definite matrix (e.g., by eigenvalue clipping).
- Optionally enforce triangle inequalities on the principal moments.
flowchart TD
S["Input inertial parameters (m, c, inertia 3x3)"] --> N["Normalize and symmetrize inertia matrix"]
N --> MCHK["Check m > 0 ?"]
MCHK -->|no| ERR["Report invalid mass / \nask for correction"]
MCHK -->|yes| SPD["Check 3x3 inertia SPD \n(eigenvalues or Cholesky)"]
SPD -->|no| PROJ["Project to SPD (clip negative eigenvalues)"]
PROJ --> TRI["Check triangle inequalities on principal moments"]
SPD -->|yes| TRI
TRI --> HSPD["Assemble 6x6 spatial inertia and test SPD"]
HSPD --> OK["Physically consistent parameters"]
In the following sections we implement this pipeline in several programming languages and highlight relevant robotics libraries.
7. Python Implementation (NumPy + Robotics Libraries)
We implement numerical checks using numpy. In practice, one
can integrate these checks into roboticstoolbox-python or
pinocchio-based workflows to validate URDF/SDF inertial
parameters before simulation or control.
import numpy as np
def is_spd(M, tol=1e-9):
"""
Check if a symmetric matrix M is positive definite via Cholesky.
"""
if M.shape[0] != M.shape[1]:
return False
# Symmetry check
if not np.allclose(M, M.T, atol=tol):
return False
try:
np.linalg.cholesky(M)
return True
except np.linalg.LinAlgError:
return False
def project_to_spd(M, tol=1e-9):
"""
Project a symmetric matrix onto the cone of SPD matrices
by eigenvalue clipping.
"""
# Symmetrize
A = 0.5 * (M + M.T)
w, V = np.linalg.eigh(A)
w_clipped = np.maximum(w, tol)
return (V * w_clipped) @ V.T
def skew(v):
"""
Return the 3x3 skew-symmetric matrix for v in R^3.
"""
x, y, z = v
return np.array([[0.0, -z, y],
[z, 0.0, -x],
[-y, x, 0.0]])
def spatial_inertia(m, c, I_C):
"""
Build 6x6 spatial inertia from:
m : scalar mass
c : 3-vector COM in body frame
I_C: 3x3 inertia about COM
"""
c = np.asarray(c).reshape(3,)
I_C = np.asarray(I_C).reshape(3, 3)
S = skew(c)
upper_left = I_C + m * S @ S.T
upper_right = m * S
lower_left = m * S.T
lower_right = m * np.eye(3)
top = np.hstack((upper_left, upper_right))
bottom = np.hstack((lower_left, lower_right))
return np.vstack((top, bottom))
def check_link_inertia(m, c, I_C, tol=1e-9, project=False):
"""
Check and optionally project a single link's inertial parameters.
Returns a dict with flags and possibly corrected inertia.
"""
result = {
"mass_positive": False,
"I_C_spd": False,
"triangle_ok": False,
"spatial_spd": False,
"I_C_corrected": None,
}
if m <= tol:
return result # invalid mass
# Symmetrize I_C
I_C_sym = 0.5 * (I_C + I_C.T)
# SPD check
if is_spd(I_C_sym, tol=tol):
I_C_spd = I_C_sym
result["I_C_spd"] = True
else:
if not project:
return result
I_C_spd = project_to_spd(I_C_sym, tol=tol)
# Triangle inequalities in principal frame
w, _ = np.linalg.eigh(I_C_spd)
J1, J2, J3 = w # sorted ascending
tri_ok = (J1 <= J2 + J3 + tol) and (J2 <= J1 + J3 + tol) and (J3 <= J1 + J2 + tol)
# Assemble spatial inertia and check SPD
I_spatial = spatial_inertia(m, c, I_C_spd)
spatial_ok = is_spd(I_spatial, tol=tol)
result.update({
"mass_positive": True,
"triangle_ok": tri_ok,
"spatial_spd": spatial_ok,
"I_C_corrected": I_C_spd
})
return result
if __name__ == "__main__":
# Example link parameters
m = 5.0
c = np.array([0.1, -0.05, 0.0])
I_C = np.array([[0.08, 0.0, 0.0],
[0.0, 0.09, 0.0],
[0.0, 0.0, 0.05]])
info = check_link_inertia(m, c, I_C, project=True)
print(info)
# In a Pinocchio-based pipeline, you could create pinocchio.Inertia objects
# from (m, c, I_C_corrected) and rebuild the model before computing H(q).
This code emphasizes energy-based tests (SPD of the spatial inertia). A
manipulator-level test can be added by assembling the model (using
roboticstoolbox or pinocchio) and explicitly
checking that \( \mathbf{H}(\mathbf{q}) \) is SPD at a
grid of configurations.
8. C++ Implementation (Eigen + RBDL / Pinocchio)
In C++, the Eigen library is standard for linear algebra,
and robotics libraries like RBDL and pinocchio
represent spatial inertias directly. Below is a minimal consistency
checker that can be used before constructing a dynamics model.
#include <iostream>
#include <Eigen/Dense>
using Eigen::Matrix3d;
using Eigen::MatrixXd;
using Eigen::Vector3d;
bool isSPD(const MatrixXd& M, double tol = 1e-9) {
if (M.rows() != M.cols()) return false;
Eigen::LLT<MatrixXd> llt(M);
if (llt.info() != Eigen::Success) return false;
// Optional: check diagonal > tol
return (llt.matrixL().diagonal().array() > tol).all();
}
Matrix3d skew(const Vector3d& v) {
Matrix3d S;
S << 0.0, -v.z(), v.y(),
v.z(), 0.0, -v.x(),
-v.y(), v.x(), 0.0;
return S;
}
Eigen::Matrix<double,6,6> spatialInertia(double m,
const Vector3d& c,
const Matrix3d& I_C)
{
Matrix3d S = skew(c);
Matrix3d upper_left = I_C + m * S * S.transpose();
Matrix3d upper_right = m * S;
Matrix3d lower_left = m * S.transpose();
Matrix3d lower_right = m * Matrix3d::Identity();
Eigen::Matrix<double,6,6> I;
I.topLeftCorner<3,3>() = upper_left;
I.topRightCorner<3,3>() = upper_right;
I.bottomLeftCorner<3,3>() = lower_left;
I.bottomRightCorner<3,3>() = lower_right;
return I;
}
int main() {
double m = 3.0;
Vector3d c(0.0, 0.0, 0.1);
Matrix3d I_C;
I_C << 0.02, 0.0, 0.0,
0.0, 0.03, 0.0,
0.0, 0.0, 0.01;
if (m <= 0.0) {
std::cerr << "Non-positive mass!" << std::endl;
return 1;
}
// Symmetrize
I_C = 0.5 * (I_C + I_C.transpose());
if (!isSPD(I_C)) {
std::cerr << "Inertia about COM is not SPD!" << std::endl;
}
Eigen::Matrix<double,6,6> I_spatial = spatialInertia(m, c, I_C);
if (!isSPD(I_spatial)) {
std::cerr << "Spatial inertia is not SPD!" << std::endl;
} else {
std::cout << "Spatial inertia is SPD and physically consistent." << std::endl;
}
// In RBDL or Pinocchio, analogous checks can be performed on
// rbdl::Model::I bodies or pinocchio::Inertia objects.
return 0;
}
Integrating this with RBDL or pinocchio allows
rejecting invalid URDF models before running dynamics algorithms.
9. Java Implementation (EJML / Apache Commons Math)
Java does not have as many robotics-specific libraries, but combining a
numerical library like EJML with a robot model (e.g., read
from URDF) is straightforward. Below is a sketch using EJML-like
operations; in real code, replace placeholders with actual EJML calls.
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;
import org.ejml.dense.row.decomposition.chol.CholeskyDecompositionInner_DDRM;
public class InertiaConsistency {
public static boolean isSPD(DMatrixRMaj M, double tol) {
if (M.numRows != M.numCols) return false;
CholeskyDecompositionInner_DDRM chol = new CholeskyDecompositionInner_DDRM(true);
if (!chol.decompose(M)) return false;
DMatrixRMaj L = chol.getT(null);
for (int i = 0; i < L.numRows; ++i) {
double diag = L.get(i, i);
if (diag <= tol) return false;
}
return true;
}
public static DMatrixRMaj skew(DMatrixRMaj v) {
DMatrixRMaj S = new DMatrixRMaj(3, 3);
double x = v.get(0, 0);
double y = v.get(1, 0);
double z = v.get(2, 0);
S.set(0, 0, 0.0); S.set(0, 1, -z); S.set(0, 2, y);
S.set(1, 0, z); S.set(1, 1, 0.0); S.set(1, 2, -x);
S.set(2, 0, -y); S.set(2, 1, x); S.set(2, 2, 0.0);
return S;
}
public static DMatrixRMaj spatialInertia(double m,
DMatrixRMaj c,
DMatrixRMaj I_C) {
DMatrixRMaj S = skew(c);
DMatrixRMaj SS_T = new DMatrixRMaj(3, 3);
CommonOps_DDRM.mult(S, S, SS_T);
DMatrixRMaj upperLeft = new DMatrixRMaj(3, 3);
CommonOps_DDRM.add(I_C, m, SS_T, upperLeft);
DMatrixRMaj upperRight = new DMatrixRMaj(3, 3);
CommonOps_DDRM.scale(m, S, upperRight);
DMatrixRMaj lowerLeft = new DMatrixRMaj(3, 3);
CommonOps_DDRM.transpose(upperRight, lowerLeft);
DMatrixRMaj lowerRight = CommonOps_DDRM.identity(3);
CommonOps_DDRM.scale(m, lowerRight, lowerRight);
DMatrixRMaj I = new DMatrixRMaj(6, 6);
// assemble 6x6 block matrix
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
I.set(i, j, upperLeft.get(i, j));
I.set(i, j + 3, upperRight.get(i, j));
I.set(i + 3, j, lowerLeft.get(i, j));
I.set(i + 3, j + 3, lowerRight.get(i, j));
}
}
return I;
}
public static void main(String[] args) {
double m = 2.5;
DMatrixRMaj c = new DMatrixRMaj(3, 1, true, new double[]{0.0, 0.05, 0.0});
DMatrixRMaj I_C = new DMatrixRMaj(3, 3, true,
new double[]{
0.01, 0.0, 0.0,
0.0, 0.015, 0.0,
0.0, 0.0, 0.008
});
if (m <= 0.0) {
System.err.println("Non-positive mass");
}
// Symmetrize I_C
DMatrixRMaj I_C_T = new DMatrixRMaj(3, 3);
CommonOps_DDRM.transpose(I_C, I_C_T);
DMatrixRMaj I_C_sym = new DMatrixRMaj(3, 3);
CommonOps_DDRM.add(0.5, I_C, 0.5, I_C_T, I_C_sym);
if (!isSPD(I_C_sym, 1e-9)) {
System.err.println("I_C not SPD");
}
DMatrixRMaj I_spatial = spatialInertia(m, c, I_C_sym);
if (!isSPD(I_spatial, 1e-9)) {
System.err.println("Spatial inertia not SPD");
} else {
System.out.println("Spatial inertia SPD");
}
}
}
A Java-based robot simulator can call this check for each link of a URDF model before building the dynamics equations.
10. MATLAB / Simulink Implementation
MATLAB's robotics.RigidBodyTree and
rigidBody classes already enforce basic constraints on
inertial parameters. We can add explicit SPD checks for robustness, and
use them inside Simulink via MATLAB Function blocks.
function result = checkLinkInertia(m, c, I_C)
%CHECKLINKINERTIA Physical consistency test for a single link.
% m : scalar mass
% c : 3x1 COM position
% I_C : 3x3 inertia about COM
tol = 1e-9;
result.massPositive = (m > tol);
% Symmetrize
I_C = 0.5 * (I_C + I_C.');
% Eigenvalues
[eigVec, eigVal] = eig(I_C);
lambda = diag(eigVal);
result.I_C_SPD = all(lambda > tol);
% Triangle inequalities in principal frame
J1 = lambda(1); J2 = lambda(2); J3 = lambda(3);
result.triangleOK = (J1 <= J2 + J3 + tol) && ...
(J2 <= J1 + J3 + tol) && ...
(J3 <= J1 + J2 + tol);
% Spatial inertia SPD test
S = [ 0, -c(3), c(2);
c(3), 0, -c(1);
-c(2), c(1), 0 ];
upperLeft = I_C + m * (S * S.');
upperRight = m * S;
lowerLeft = m * S.';
lowerRight = m * eye(3);
I_spatial = [upperLeft, upperRight;
lowerLeft, lowerRight];
lambda_spatial = eig(I_spatial);
result.spatialSPD = all(lambda_spatial > tol);
end
In Simulink, this function can be called inside a MATLAB Function block,
with Assert blocks reacting to failed tests during
simulation. This ensures that the inertia used in a
RigidBodyTree-based inverse dynamics block corresponds to a
valid mechanical model.
11. Wolfram Mathematica Implementation
Mathematica is convenient for symbolic and semi-symbolic reasoning about positive definiteness and parameter constraints.
(* Define symbolic inertia about COM *)
Ixx = Symbol["Ixx"]; Iyy = Symbol["Iyy"]; Izz = Symbol["Izz"];
Ixy = Symbol["Ixy"]; Ixz = Symbol["Ixz"]; Iyz = Symbol["Iyz"];
IC = {
{Ixx, Ixy, Ixz},
{Ixy, Iyy, Iyz},
{Ixz, Iyz, Izz}
};
(* Positive definiteness conditions *)
spdConditions = Reduce[
PositiveDefiniteMatrixQ[IC],
{Ixx, Iyy, Izz, Ixy, Ixz, Iyz},
Reals
]
(* Numerical check for a given instance *)
m = 2.0;
c = {0.1, 0.0, -0.05};
ICnum = { {0.04, 0., 0.},
{0., 0.05, 0.},
{0., 0., 0.02} };
posDefIC = PositiveDefiniteMatrixQ[ICnum];
S[v_] := { {0, -v[[3]], v[[2]]},
{v[[3]], 0, -v[[1]]},
{-v[[2]], v[[1]], 0} };
Snum = S[c];
upperLeft = ICnum + m Snum.Transpose[Snum];
upperRight = m Snum;
lowerLeft = m Transpose[Snum];
lowerRight = m IdentityMatrix[3];
Ispatial = ArrayFlatten[{ {upperLeft, upperRight},
{lowerLeft, lowerRight} }];
posDefSpatial = PositiveDefiniteMatrixQ[Ispatial]
The symbolic conditions from Reduce can be used to derive
exact inequalities on the components \( I_{ij} \), while numerical tests
verify specific parameter sets.
12. Problems and Solutions
Problem 1 (Spatial inertia positive definiteness).
Let a rigid body have mass \( m > 0 \), center of mass \( \mathbf{c} \), and inertia about COM \( \mathbf{I}_C\succ 0 \). Prove that the spatial inertia
\[ \boldsymbol{\mathcal{I}} = \begin{bmatrix} \mathbf{I}_C + m[\mathbf{c}]_{\times}[\mathbf{c}]_{\times}^{\mathsf{T}} & m[\mathbf{c}]_{\times}\\ m[\mathbf{c}]_{\times}^{\mathsf{T}} & m\mathbf{I}_3 \end{bmatrix} \]
is positive definite.
Solution.
Consider the spatial transformation \( \mathbf{X}_{BC} \) mapping twists expressed at the COM frame \( \mathcal{F}_C \) to the body frame. The spatial inertia about the COM frame is block diagonal:
\[ \boldsymbol{\mathcal{I}}_C = \begin{bmatrix} \mathbf{I}_C & 0\\ 0 & m\mathbf{I}_3 \end{bmatrix}. \]
The kinetic energy expressed at the body frame is \( T = \tfrac{1}{2}\mathbf{v}_B^{\mathsf{T}} \boldsymbol{\mathcal{I}}\mathbf{v}_B \), with \( \mathbf{v}_B = \mathbf{X}_{BC}\mathbf{v}_C \). Using \( \mathbf{X}_{BC} \) invertible, we have
\[ \boldsymbol{\mathcal{I}} = \mathbf{X}_{BC}^{-\mathsf{T}} \boldsymbol{\mathcal{I}}_C \mathbf{X}_{BC}^{-1}. \]
Since congruence with an invertible matrix preserves positive definiteness, \( \boldsymbol{\mathcal{I}}\succ 0 \) if and only if \( \boldsymbol{\mathcal{I}}_C\succ 0 \). But \( \boldsymbol{\mathcal{I}}_C \) is block diagonal with blocks \( \mathbf{I}_C\succ 0 \) and \( m\mathbf{I}_3\succ 0 \), hence positive definite. Therefore \( \boldsymbol{\mathcal{I}}\succ 0 \).
Problem 2 (Triangle inequalities for a candidate inertia).
A link has principal moments of inertia (in kg·m\(^2\)) \( J_1 = 0.1 \), \( J_2 = 0.2 \), \( J_3 = 0.35 \). Check whether these can arise from a rigid body with nonnegative density.
Solution.
The triangle inequalities require
\[ \begin{aligned} J_1 &\leq J_2 + J_3,\\ J_2 &\leq J_1 + J_3,\\ J_3 &\leq J_1 + J_2. \end{aligned} \]
Substituting:
- \( 0.1 \leq 0.2 + 0.35 = 0.55 \): satisfied;
- \( 0.2 \leq 0.1 + 0.35 = 0.45 \): satisfied;
- \( 0.35 \leq 0.1 + 0.2 = 0.3 \): violated.
The last inequality fails, so there is no choice of nonnegative second moments \( L_x,L_y,L_z \) that produce these \( J_i \). Hence the inertia tensor is not physically realizable, even though it may be positive definite as a matrix.
Problem 3 (Projection of an inconsistent inertia).
Using the parameter set in Problem 2, propose a simple projection that yields a physically consistent set of principal moments, keeping the matrix as close as possible (in Euclidean norm) to the original.
Solution.
A simple approach is to enforce the largest violated inequality by reducing \( J_3 \). The most restrictive inequality is \( J_3 \leq J_1 + J_2 \). Define
\[ J_3^{\star} = J_1 + J_2 - \varepsilon \]
for a small \( \varepsilon>0 \). With \( J_1 = 0.1 \), \( J_2 = 0.2 \), choosing \( \varepsilon = 0.01 \) gives \( J_3^{\star} = 0.29 \). The new set \( (0.1, 0.2, 0.29) \) satisfies all triangle inequalities:
- \( 0.1 \leq 0.2 + 0.29 = 0.49 \);
- \( 0.2 \leq 0.1 + 0.29 = 0.39 \);
- \( 0.29 \leq 0.1 + 0.2 = 0.3 \).
In practice, one would diagonalize the inertia tensor, replace the eigenvalues by their projected values \( J_i^{\star} \), and reconstruct the corrected inertia as \( \mathbf{I}_C^{\star} = \mathbf{R}\operatorname{diag}(J_1,J_2,J_3^{\star}) \mathbf{R}^{\mathsf{T}} \).
Problem 4 (Joint-space inertia as sum of SPD contributions).
For an \(n\)-DOF manipulator with links \( k = 1,\dots,N \), let \( \boldsymbol{\mathcal{I}}_k\succ 0 \) be the spatial inertia of link \( k \). Show that the joint-space inertia matrix \( \mathbf{H}(\mathbf{q}) \) is positive definite for each configuration \( \mathbf{q} \).
Solution.
From spatial dynamics, the joint-space inertia can be expressed as
\[ \mathbf{H}(\mathbf{q}) = \sum_{k=1}^{N} \mathbf{J}_k(\mathbf{q})^{\mathsf{T}} \boldsymbol{\mathcal{I}}_k \mathbf{J}_k(\mathbf{q}), \]
where \( \mathbf{J}_k(\mathbf{q})\in\mathbb{R}^{6\times n} \) is the geometric Jacobian mapping joint velocities to the spatial twist of link \( k \). For any \( \dot{\mathbf{q}}\in\mathbb{R}^n \),
\[ \dot{\mathbf{q}}^{\mathsf{T}}\mathbf{H}(\mathbf{q})\dot{\mathbf{q}} = \sum_{k=1}^{N} \left( \mathbf{J}_k(\mathbf{q})\dot{\mathbf{q}} \right)^{\mathsf{T}} \boldsymbol{\mathcal{I}}_k \left( \mathbf{J}_k(\mathbf{q})\dot{\mathbf{q}} \right). \]
Since each \( \boldsymbol{\mathcal{I}}_k\succ 0 \), every term in the sum is nonnegative and zero only if the corresponding twist \( \mathbf{J}_k(\mathbf{q})\dot{\mathbf{q}} \) is zero. For a free manipulator (no locked joints, no massless links), the only \( \dot{\mathbf{q}} \) that makes all link twists zero is \( \dot{\mathbf{q}} = \mathbf{0} \). Therefore, \( \mathbf{H}(\mathbf{q})\succ 0 \) for all \( \mathbf{q} \).
Problem 5 (Energy interpretation of inconsistency).
Suppose a numerical mistake leads to a spatial inertia \( \boldsymbol{\mathcal{I}} \) with one negative eigenvalue. Explain in energy terms why this is unphysical and how an SPD projection fixes the issue.
Solution.
If \( \boldsymbol{\mathcal{I}} \) has a negative eigenvalue \( \lambda < 0 \) with eigenvector \( \mathbf{u} \), then setting the twist \( \mathbf{v} = \mathbf{u} \) yields \( T = \tfrac{1}{2}\mathbf{v}^{\mathsf{T}}\boldsymbol{\mathcal{I}}\mathbf{v} = \tfrac{1}{2}\lambda\|\mathbf{u}\|^2 < 0 \), implying negative kinetic energy, which contradicts passivity of mechanical systems. Projecting \( \boldsymbol{\mathcal{I}} \) to the nearest SPD matrix (e.g. by clipping eigenvalues to a small positive threshold) removes negative directions in the energy, ensuring that \( T \) is strictly positive for all nonzero twists and restoring physical interpretability.
13. Summary
This lesson formalized physical consistency of rigid-body inertial parameters via positive definiteness of 3D inertia tensors, spatial inertia matrices, and joint-space inertia matrices. We connected energy expressions to algebraic conditions (SPD and triangle inequalities), showed how link-level consistency propagates to the manipulator-level inertia \( \mathbf{H}(\mathbf{q}) \), and provided multi-language implementations for checking and projecting inertial parameters. These tools are essential for robust modeling, simulation, and identification in later chapters.
14. References
- Featherstone, R. (1983). The calculation of robot dynamics using articulated-body inertias. International Journal of Robotics Research, 2(1), 13–30.
- Walker, M. W., & Orin, D. E. (1982). Efficient dynamic computer simulation of robotic mechanisms. Journal of Dynamic Systems, Measurement, and Control, 104(3), 205–211.
- Gautier, M., & Khalil, W. (1992). Exciting trajectories for the identification of base inertial parameters of robots. International Journal of Robotics Research, 11(4), 362–375.
- Traversaro, S., Nori, F., Metta, G., & Sandini, G. (2016). Identification of inertial parameters of rigid bodies with physical consistency. IEEE Robotics and Automation Letters, 1(1), 148–154.
- Orin, D. E., Goswami, A. (2008). Centroidal momentum matrix of a humanoid robot: Structure and properties. Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 653–659.
- Khalil, W., & Kleinfinger, J. F. (1986). A new geometric notation for open and closed-loop robots. IEEE International Conference on Robotics and Automation, 1174–1180.
- Featherstone, R. (2008). Rigid Body Dynamics Algorithms. Springer (monograph with foundational theoretical results on spatial inertias).
- Sastry, S. S. (1999). Nonlinear Systems: Analysis, Stability, and Control. Springer (for general results on positive definiteness and energy-based Lyapunov functions applied to mechanical systems).