Fully-physical consistency for all robots

[cmastalli]

I noticed that most of the Talos' URDFs define rotational inertias that are not fully physically consistent. In other words, it means that these inertias are nonsense.

Typically, when designing a robot, we extract the rotational inertia from the CAD. This is a normal practice, but often people do it incorrectly. For instance, the reported values are expressed in the CAD's reference system, rather than in the CAD's COM point. In any case, we're unable to retrieve the right value as our URDFs come from robot manufacturers.

What I proposed in this PR to solve this issue is as follows:

1. Introduce a unit test that checks physical consistency: positive mass, positive principal components of inertia, and the satisfaction of triangular inequalities (i.e., positive moments of inertia). This unit test relies on Pinocchio, but part of the code could move to Pinocchio in the future.
2. Fix the rotational inertia of robots that weren't failing. The criteria used were to stay as close to the original rotational inertia. As you can see, the changes are minimal.

With this PR, new contributors will be forced to integrate URDFs that meet the physical laws of inertia. I will encourage to not merge any robot before processing this PR.

[nim65s]

Great, thanks !

I guess you did use some script to "stay as close to the original rotational inertia", could you share it ? No need to include that in the repo, but a link to a gist or something would be a nice reference for later

[cmastalli]

Great, thanks !

I guess you did use some script to "stay as close to the original rotational inertia", could you share it ? No need to include that in the repo, but a link to a gist or something would be a nice reference for later

The approach is as simple as a projection to the fully-physical-consistent set. As you can see below, we compute a delta of the triangular inequalities violation to correct the principal component of inertia. Moreover, we include a tiny slacks to accommodate potential rounding errors. Finally, we revert this back to the original coordinates of the rotational inertia via the eigen vectors.

import numpy as np


def project_to_rigid_body_inertia(I: np.ndarray) -> np.ndarray:
    """Return the closest rigid-body-valid rotational inertia matrix.

    Assumes I is symmetric or nearly symmetric. The projection preserves the
    current eigenvectors and minimally adjusts the principal moments.
    """
    I = np.asarray(I, dtype=float)
    I = 0.5 * (I + I.T)

    # Principal moments / principal axes.
    eigvals, eigvecs = np.linalg.eigh(I)

    # For sorted eigvals λ0 <= λ1 <= λ2, rigid-body validity requires:
    # λ0 >= 0 and λ2 <= λ0 + λ1.
    delta = max(0.0, eigvals[2] - eigvals[1] - eigvals[0])

    # Orthogonal projection onto λ2 = λ0 + λ1 in L2 / Frobenius sense.
    projected = eigvals.copy()
    projected[0] += delta / 3.0
    projected[1] += delta / 3.0
    projected[2] -= delta / 3.0

    # Tiny slack so decimal round-tripping does not land back on the boundary.
    slack = max(1e-12, 1e-10 * float(np.trace(I)))
    projected[0] += slack / 3.0
    projected[1] += slack / 3.0
    projected[2] -= slack / 3.0

    J = eigvecs @ np.diag(projected) @ eigvecs.T
    return 0.5 * (J + J.T)

[cmastalli]

Ah, I forgot to mention that I didn't use this script for Talos. In that case, I switched numbers because I suspected it was a typing error. I could run the script over Talos if preferred.