newton/geometry/inertia.py (2)

602-602: Consider making the maximum iterations configurable.

The hardcoded max_iterations = 10 may not be sufficient for severely unbalanced inertia tensors, potentially leading to incomplete corrections.

def verify_and_correct_inertia(
    mass: float,
    inertia: wp.mat33,
    balance_inertia: bool = True,
    bound_mass: float | None = None,
    bound_inertia: float | None = None,
+   max_iterations: int = 10,
) -> tuple[float, wp.mat33, bool]:

Then update the balancing loop:

-        max_iterations = 10
+        # max_iterations is now a parameter

---

613-613: The convergence tolerance may be too strict for some use cases.

The hardcoded tolerance of 1e-10 might be overly strict for floating-point computations, potentially causing unnecessary iterations.

Consider making the tolerance configurable or using a more appropriate value like 1e-8 or even relative to the magnitude of the inertia values:

-                if max_violation <= 1e-10:
+                if max_violation <= max(1e-8, 1e-10 * max(I)):

newton/sim/builder.py (1)

3402-3425: Excellent integration of inertia validation with proper error handling

The inertia verification and correction is well-integrated into the finalization process:

Strengths:

• Conditional execution prevents unnecessary processing when validation is disabled
• Proper handling of corrected mass and inertia values with appropriate inverse calculations
• Maintains consistency with existing inverse mass/inertia computation patterns (matches lines 3187-3190)
• Updates all related properties (mass, inertia, inv_mass, inv_inertia) when corrections occur

Minor optimization opportunity:
The condition check could be simplified since verify_and_correct_inertia handles None values gracefully:

-if self.balance_inertia or self.bound_mass is not None or self.bound_inertia is not None:
+if self.balance_inertia or self.bound_mass is not None or self.bound_inertia is not None:

However, the current approach is clearer about when validation occurs and avoids unnecessary function calls.

Configuration used: .coderabbit.yml
Review profile: CHILL
Plan: Pro

newton/geometry/inertia.py (1)

580-635: Triangle inequality validation needs convergence verification.

The triangle inequality checking and balancing algorithm implementation looks correct, but there's a missing convergence check after the iterative loop as noted in past reviews.

The balancing algorithm may not converge in pathological cases. Consider adding a post-loop verification to ensure the algorithm actually converged within max_iterations.

newton/geometry/inertia.py (3)

582-588: Simplify violation detection logic.

The current implementation uses a boolean flag and multiple if statements to detect violations. This can be simplified for better readability and performance.

Consider simplifying the violation detection:

-    has_violations = False
-    if Ixx + Iyy < Izz:
-        has_violations = True  # Ixx + Iyy < Izz
-    if Iyy + Izz < Ixx:
-        has_violations = True  # Iyy + Izz < Ixx
-    if Izz + Ixx < Iyy:
-        has_violations = True  # Izz + Ixx < Iyy
+    has_violations = (Ixx + Iyy < Izz) or (Iyy + Izz < Ixx) or (Izz + Ixx < Iyy)

---

492-651: Consider performance optimization for batch processing.

Given the PR objectives mention performance concerns about this function being "invasive" during the finalize() step, consider the feasibility of vectorizing operations for batch processing when applied to multiple bodies.

For better performance when processing multiple rigid bodies, consider:

1. Vectorizing the validation operations using NumPy array operations
2. Implementing a Warp kernel version as suggested by eric-heiden in the PR comments
3. Pre-allocating arrays for batch processing scenarios

The current implementation processes one body at a time, which may become a bottleneck for large models with many rigid bodies.

---

638-647: Eigenvalue computation could fail for edge cases.

While the code handles LinAlgError, there are potential edge cases where eigenvalue computation might produce NaN or infinite values that should be handled.

Consider adding additional validation for the eigenvalues:

        eigenvalues = np.linalg.eigvals(corrected_inertia)
-        if np.any(eigenvalues <= 0):
+        if np.any(eigenvalues <= 0) or np.any(~np.isfinite(eigenvalues)):
            warnings.warn("Corrected inertia matrix is not positive definite, this should not happen", stacklevel=2)
            # As a last resort, make it positive definite by adding a small value to diagonal
-            min_eigenvalue = np.min(eigenvalues)
+            min_eigenvalue = np.min(eigenvalues[np.isfinite(eigenvalues)]) if np.any(np.isfinite(eigenvalues)) else -1e-6

Configuration used: .coderabbit.yml
Review profile: CHILL
Plan: Pro

newton/geometry/inertia.py (1)

597-636: Add convergence verification after the balancing loop.

The iterative balancing algorithm should verify convergence after the loop completes. If max_violation > tol after all iterations, a warning should be issued to alert users that the algorithm didn't fully converge.

newton/tests/test_inertia_validation.py (1)

161-195: Consider adding triangle inequality verification to the fast validation test.

While the test verifies that bounds are applied, it doesn't check whether the triangle inequalities are satisfied after correction. Add assertions similar to lines 232-234 from the detailed test to ensure complete validation.

             Ixx, Iyy, Izz = body_inertia[0, 0], body_inertia[1, 1], body_inertia[2, 2]
             self.assertGreaterEqual(Ixx, builder.bound_inertia)
             self.assertGreaterEqual(Iyy, builder.bound_inertia)
             self.assertGreaterEqual(Izz, builder.bound_inertia)
+
+            # Check triangle inequalities
+            self.assertGreaterEqual(Ixx + Iyy, Izz - 1e-10)
+            self.assertGreaterEqual(Iyy + Izz, Ixx - 1e-10)
+            self.assertGreaterEqual(Izz + Ixx, Iyy - 1e-10)

Configuration used: .coderabbit.yml
Review profile: CHILL
Plan: Pro

newton/geometry/inertia.py (1)

492-649: The function logic is comprehensive but has a potential convergence issue.

The verify_and_correct_inertia function provides thorough validation and correction of mass and inertia properties. The implementation correctly handles negative mass, asymmetric matrices, negative eigenvalues, and triangle inequality violations. However, there's a concern about the balancing algorithm's convergence guarantee.

The balancing approach using scalar addition to all eigenvalues (lines 615-618) is mathematically sound and preserves eigenvectors, which is better than more complex iterative approaches. The method ensures the triangle inequality is satisfied by adding the minimum necessary adjustment.

newton/geometry/inertia.py (1)

652-735: Warp kernel implementation addresses performance concerns effectively.

The parallel validation kernel provides a performance-optimized alternative to the CPU-based validation, directly addressing the performance concerns raised in the PR objectives. The kernel correctly handles:

• Mass validation and bounds checking
• Basic inertia validation with diagonal element bounds
• Proper updating of both forward and inverse quantities
• Zero-mass edge cases

The limitation to diagonal-only bounds checking (mentioned in lines 690-707) is reasonable for a parallel implementation, as eigenvalue decomposition would be computationally expensive in a kernel.

However, consider adding a small performance optimization:

    # Update inverse inertia
    if mass > 0.0:
-       inv_inertia = wp.inverse(inertia)
-       body_inv_inertia[tid] = inv_inertia
+       body_inv_inertia[tid] = wp.inverse(inertia)
    else:
        body_inv_inertia[tid] = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)

Configuration used: .coderabbit.yml
Review profile: CHILL
Plan: Pro

newton/geometry/inertia.py (1)

492-649: Comprehensive inertia validation with room for optimization.

The function correctly implements the core validation logic similar to MuJoCo's balanceinertia. However, there are several areas for improvement:

1. Multiple eigenvalue computations: The function computes eigenvalues multiple times (lines 555, 566, 577, 621, 632), which is computationally expensive.

2. Triangle inequality balancing: The current approach (lines 605-629) uses a simple scalar addition method which may not be globally optimal, as noted in the PR description.

Consider caching eigenvalue computations and eigenvectors to reduce redundant calculations:

# Compute eigenvalues (principal moments) for validation
try:
-   eigenvalues = np.linalg.eigvals(corrected_inertia)
+   eigenvalues, eigenvectors = np.linalg.eigh(corrected_inertia)
    
    # Check for negative eigenvalues
    if np.any(eigenvalues < 0):
        warnings.warn(
            f"Negative eigenvalues detected{body_id}: {eigenvalues}, making positive definite",
            stacklevel=2,
        )
        # Make positive definite by reconstructing from eigendecomposition
-       min_eig = np.min(eigenvalues)
-       corrected_inertia += np.eye(3) * (-min_eig + 1e-6)
-       eigenvalues = np.linalg.eigvals(corrected_inertia)
+       eigenvalues = np.maximum(eigenvalues, 1e-6)
+       corrected_inertia = eigenvectors @ np.diag(eigenvalues) @ eigenvectors.T
        was_corrected = True

Configuration used: .coderabbit.yml
Review profile: CHILL
Plan: Pro

newton/geometry/inertia.py (1)

492-649: Review the convergence and error handling in the inertia balancing algorithm.

The verify_and_correct_inertia function implements comprehensive inertia validation and correction logic. While the overall approach is sound, there are several areas that need attention:

1. Convergence verification: The iterative balancing algorithm (lines 605-629) lacks explicit convergence checking, which was flagged in previous reviews.

2. Performance concerns: As noted by eric-heiden, this function performs extensive validation per body during model finalization, which could impact performance for large models.

3. Error handling robustness: The eigenvalue computation fallback (lines 588-598) handles LinAlgError but could be more robust.

Consider these improvements:

        if balance_inertia:
            # For non-diagonal matrices, we need to adjust while preserving the rotation
            deficit = I3 - I1 - I2
            if deficit > 0:
                # Simple approach: add scalar to all eigenvalues to ensure validity
                # This preserves eigenvectors exactly
                # We need: (I1 + a) + (I2 + a) >= I3 + a
                # Which simplifies to: I1 + I2 + a >= I3
                # So: a >= I3 - I1 - I2 = deficit
                adjustment = deficit + 1e-6

                # Add scalar*I to shift all eigenvalues equally
                corrected_inertia = corrected_inertia + np.eye(3) * adjustment
                was_corrected = True

                # Recompute eigenvalues for the warning message
                new_eigenvalues = np.linalg.eigvals(corrected_inertia)
                new_eigenvalues = np.sort(new_eigenvalues)
+
+               # Verify convergence
+               new_I1, new_I2, new_I3 = new_eigenvalues
+               if new_I1 + new_I2 < new_I3 - 1e-10:
+                   warnings.warn(
+                       f"Inertia balancing did not fully converge{body_id}; "
+                       f"remaining violation: {new_I3 - new_I1 - new_I2:.3e}",
+                       stacklevel=2,
+                   )

                warnings.warn(
                    f"Balanced principal moments{body_id} from ({I1:.6f}, {I2:.6f}, {I3:.6f}) to "
                    f"({new_eigenvalues[0]:.6f}, {new_eigenvalues[1]:.6f}, {new_eigenvalues[2]:.6f})",
                    stacklevel=2,
                )

newton/geometry/inertia.py (1)

652-734: The Warp kernel implementation addresses performance concerns effectively.

The validate_and_correct_inertia_kernel provides a parallelized alternative for batch processing, which directly addresses eric-heiden's performance concerns. However, there are some areas for improvement:

1. Limited validation scope: The kernel only performs basic validation (negative mass, bounds, diagonal elements) compared to the comprehensive validation in verify_and_correct_inertia.

2. Inverse inertia calculation: The kernel uses wp.inverse(inertia) which assumes the matrix is invertible, but this could fail for singular matrices.

Consider adding safeguards for the inverse calculation:

    # Update inverse inertia
    if mass > 0.0:
-       body_inv_inertia[tid] = wp.inverse(inertia)
+       # Check if inertia is invertible before computing inverse
+       det = wp.determinant(inertia)
+       if wp.abs(det) > 1e-12:
+           body_inv_inertia[tid] = wp.inverse(inertia)
+       else:
+           # Fallback for near-singular matrices
+           body_inv_inertia[tid] = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
    else:
        body_inv_inertia[tid] = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)

This addresses the concerns about matrix invertibility while maintaining the performance benefits of parallel processing.

Configuration used: .coderabbit.yml
Review profile: CHILL
Plan: Pro