Repository: Genesis-Embodied-AI/Genesis
File: genesis/utils/usd/usd_utils.py (Python)

Code before:

def usd_diagonal_inertia_to_numpy(usd_pos: Gf.Vec3f) -> np.ndarray | None:
    """
    Convert USD diagonal inertia to numpy array, handling default and invalid values.

    The USD Physics MassAPI defines diagonalInertia with default value (0, 0, 0),
    which is valid but means the inertia should be ignored/computed from geometry.
    This function returns None for the default ignored value or invalid values (negative or inf/nan).

    Parameters
    ----------
    usd_pos : Gf.Vec3f
        USD diagonal inertia attribute value.

    Returns
    -------
    np.ndarray | None
        Valid diagonal inertia as numpy array, or None if default ignored or invalid.
    """
    inertia = usd_pos_to_numpy(usd_pos)
    # Default is (0, 0, 0) which means ignored - only return if non-zero and valid
    if np.allclose(inertia, 0):
        return None
    # Check for invalid values (negative or inf/nan)
    if not np.all(inertia >= 0):
        return None
    return inertia


def usd_mass_to_float(usd_mass: float) -> float | None:
    """
    Convert USD mass to float, handling default and invalid values.

    The USD Physics MassAPI defines mass with default value 0, which is valid but
    means the mass should be ignored/computed from geometry. This function returns
    None for the default ignored value or invalid values (non-positive, inf, or nan).

    Parameters
    ----------
    usd_mass : float
        USD mass attribute value.

    Returns
    -------
    float | None
        Valid mass value, or None if default ignored or invalid.
    """
    # Default is 0 which means ignored
    if usd_mass <= 0:
        return None
    # Check for invalid values (inf or nan)

Review comment (suggestion):
I would rather check `all(math.isfinite(e) for e in inertia)`. Because we do not want to hind random negative values.

Code after:

def usd_inertia_to_numpy(inertia: Gf.Vec3f) -> np.ndarray | None:
    """
    Convert USD diagonal inertia to numpy diagonal matrix, handling default and invalid values.

    The USD Physics MassAPI defines diagonalInertia with default value (0, 0, 0),
    which is valid but means the inertia should be ignored/computed from geometry.
    This function returns None for the default ignored value or invalid values (non-finite).

    Parameters
    ----------
    inertia : Gf.Vec3f
        USD diagonal inertia attribute value.

    Returns
    -------
    np.ndarray | None
        Valid diagonal inertia matrix (3x3), or None if default ignored or invalid.
    """
    diagonal_inertia = usd_diagonal_inertia_to_numpy(inertia)
    if diagonal_inertia is None:
        return None
    return np.diag(diagonal_inertia)


def usd_diagonal_inertia_to_numpy(usd_pos: Gf.Vec3f) -> np.ndarray | None:
    """
    Convert USD diagonal inertia to numpy array, handling default and invalid values.

    The USD Physics MassAPI defines diagonalInertia with default value (0, 0, 0),
    which is valid but means the inertia should be ignored/computed from geometry.
    This function returns None for the default ignored value or invalid values (negative or inf/nan).

    Parameters
    ----------
    usd_pos : Gf.Vec3f
        USD diagonal inertia attribute value.

    Returns
    -------
    np.ndarray | None
        Valid diagonal inertia as numpy array, or None if default ignored or invalid.
    """
    inertia = usd_pos_to_numpy(usd_pos)
    # Default is (0, 0, 0) which means ignored - only return if non-zero and valid
    if np.allclose(inertia, 0):
        return None
    # Check for invalid values (non-finite)
    if not all(math.isfinite(e) for e in inertia):
        return None
    return inertia