Clamp and decouple a Hessian pivot lost to rounding instead of aborting Newton.

H = M + J'*D*J >= M is SPD in exact arithmetic, so FactorizeHessian
treated a rank-deficient sparse factor as a programming error. It is
not: the reverse-order sparse factorization can lose a pivot to
cancellation when the inertia is too ill-conditioned for float32 mjtNum.

Both tests fail under float32 before the fix.

PiperOrigin-RevId: 981785664
Change-Id: I382e7cfc2e4e0dfa8e0f8c0bbe3a1063b5bde32c

Author: yuvaltassa

doc/changelog.rst

@@ -22,6 +22,9 @@ Engine
 - The :ref:`mjWARN_INERTIA <mjtWarning>` warning is now also raised by the modified-inertia factorizations of the
   :at:`implicitfast` and damped-:at:`Euler` integrators (previously silent) and of the :at:`implicit` integrator
   (previously a fatal error).
+- The sparse Newton solver no longer aborts with a "rank-deficient sparse Hessian" error when rounding loses a pivot of
+  its Hessian, as can happen in single precision with ill-conditioned inertia. The pivot is now clamped and its row
+  decoupled, as in the dense factorization.
 
 Bug fixes
 ^^^^^^^^^

src/engine/engine_solver.c

@@ -2503,16 +2503,11 @@ static void FactorizeHessian(mjData* d, mjPrimalContext* ctx, int flg_recompute)
     }
 
     // numeric sparse factorization: L = chol(H) using pre-computed sparsity pattern
-    int rank = mju_cholFactorNumeric(
+    mju_cholFactorNumeric(
         ctx->L, nv, mjMINVAL,
         ctx->L_rownnz, ctx->L_rowadr, ctx->L_colind,
         ctx->LT_rownnz, ctx->LT_rowadr, ctx->LT_colind, ctx->LT_map,
         ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, ctx->cholscratch);
-
-    // rank-deficient; SHOULD NOT OCCUR
-    if (rank != nv) {
-      mjERROR("rank-deficient sparse Hessian");
-    }
   }
 
   // dense
@@ -2607,13 +2602,11 @@ static void HessianConeFolded(mjData* d, mjPrimalContext* ctx) {
     mju_addToMatSparse(Hcone, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, nv,
                        ctx->S_val, ctx->S_rownnz, ctx->S_rowadr, ctx->S_colind);
   }
-  if (mju_cholFactorNumeric(ctx->Lcone, nv, mjMINVAL,
-                            ctx->L_rownnz, ctx->L_rowadr, ctx->L_colind,
-                            ctx->LT_rownnz, ctx->LT_rowadr, ctx->LT_colind, ctx->LT_map,
-                            Hcone, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind,
-                            ctx->cholscratch) != nv) {
-    mjERROR("rank-deficient cone Hessian");
-  }
+  mju_cholFactorNumeric(ctx->Lcone, nv, mjMINVAL,
+                        ctx->L_rownnz, ctx->L_rowadr, ctx->L_colind,
+                        ctx->LT_rownnz, ctx->LT_rowadr, ctx->LT_colind, ctx->LT_map,
+                        Hcone, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind,
+                        ctx->cholscratch);
 }
 
 

src/engine/engine_util_solve.c

@@ -351,20 +351,25 @@ int mju_cholFactorNumeric(mjtNum* restrict L, int n, mjtNum mindiag,
 
     // factor row r diagonal, handle rank-deficient case
     mjtNum diag = dense[r];
-    if (diag < mindiag) {
+    int deficient = diag < mindiag;
+    if (deficient) {
       diag = mindiag;
       rank--;
     }
 
-    // scale off-diagonals
+    // scale off-diagonals; if deficient clear them, decoupling the row as mju_cholFactor does
     mjtNum L_rr = mju_sqrt(diag);
     mjtNum L_rr_inv = 1.0 / L_rr;
     int L_adr = L_rowadr[r];
     int L_nnz = L_rownnz[r];
     const int* colptr = L_colind + L_adr;
     mjtNum* Lptr = L + L_adr;
-    for (int i = 0; i < L_nnz - 1; i++) {
-      Lptr[i] = dense[colptr[i]] * L_rr_inv;
+    if (deficient) {
+      mju_zero(Lptr, L_nnz - 1);
+    } else {
+      for (int i = 0; i < L_nnz - 1; i++) {
+        Lptr[i] = dense[colptr[i]] * L_rr_inv;
+      }
     }
 
     // store diagonal

test/engine/engine_solver_test.cc

@@ -765,5 +765,50 @@ TEST_F(SolverTest, ConeFoldEquivalent) {
   }
 }
 
+// A chain of thin capsules whose tip is connected to a slider: the connect
+// fills H = M + J'*D*J, and once the chain sags, the twist pivots of the
+// reverse factorization (the capsules' tiny axial inertia) are five orders of
+// magnitude below their diagonals. In single precision rounding loses one:
+// Newton must clamp and decouple it and continue, not abort or diverge.
+TEST_F(SolverTest, NewtonHessianSurvivesLostPivot) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <composite type="cable" curve="s" count="41 1 1" size="1" initial="none">
+        <joint kind="main" damping=".015"/>
+        <geom type="capsule" size=".005" contype="0" conaffinity="0"/>
+      </composite>
+      <body name="slider" pos="1 0 0">
+        <joint type="slide" axis="1 0 0" damping=".1"/>
+        <geom size=".01" contype="0" conaffinity="0"/>
+      </body>
+    </worldbody>
+    <equality>
+      <connect body1="B_last" body2="slider" anchor=".025 0 0"/>
+    </equality>
+  </mujoco>
+  )";
+
+  char error[1024];
+  MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(model.get(), NotNull()) << error;
+  model->opt.solver = mjSOL_NEWTON;
+
+  for (mjtJacobian jacobian : {mjJAC_DENSE, mjJAC_SPARSE}) {
+    for (int disableflags : {0, (int)mjDSBL_ISLAND}) {
+      model->opt.jacobian = jacobian;
+      model->opt.disableflags = disableflags;
+      MjDataPtr data = MakeData(model);
+
+      // bounded by step count: a divergence resets mjData and rewinds the clock
+      for (int step = 0; step < 200; step++) {
+        mj_step(model.get(), data.get());
+      }
+      EXPECT_EQ(data->warning[mjWARN_BADQACC].number, 0)
+          << "jacobian " << jacobian << ", disableflags " << disableflags;
+    }
+  }
+}
+
 }  // namespace
 }  // namespace mujoco

test/engine/engine_util_solve_test.cc

@@ -1101,6 +1101,52 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
   }
 }
 
+// A pivot lost to rounding leaves its row's coupling in place. The clamp must
+// decouple the row, as mju_cholFactor does: dividing the coupling by
+// sqrt(mindiag) would drive every later pivot of the factorization negative.
+TEST_F(EngineUtilSolveTest, CholFactorNumericClampDecouples) {
+  MjModelPtr model = LoadModelFromString("<mujoco/>");
+  MjDataPtr d = MakeData(model);
+
+  // reverse elimination: row 2 has pivot 1, leaving row 1 with pivot 0 and
+  // coupling 0.5 to row 0, whose pivot is 1 once row 1 is decoupled
+  constexpr int n = 3;
+  mjtNum H[n * n] = {1, .5, 0, .5, 1, 1, 0, 1, 1};
+  mjtNum sparseH[n * n];
+  int H_rownnz[n], H_rowadr[n], H_colind[n * n];
+  mju_dense2sparse(sparseH, H, n, n, H_rownnz, H_rowadr, H_colind, n * n);
+
+  // symbolic factorization
+  int L_rownnz[n], L_rowadr[n], LT_rownnz[n], LT_rowadr[n];
+  int nnz = mju_cholFactorSymbolic(nullptr, L_rownnz, L_rowadr, nullptr,
+                                   LT_rownnz, LT_rowadr, nullptr, H_rownnz,
+                                   H_rowadr, H_colind, n, d.get());
+  int L_colind[n * n], LT_colind[n * n], LT_map[n * n];
+  mju_cholFactorSymbolic(L_colind, L_rownnz, L_rowadr, LT_colind, LT_rownnz,
+                         LT_rowadr, LT_map, H_rownnz, H_rowadr, H_colind, n,
+                         d.get());
+
+  // numeric factorization: only row 1 is clamped
+  mjtNum L[n * n], scratch[n];
+  int rank = mju_cholFactorNumeric(
+      L, n, mjMINVAL, L_rownnz, L_rowadr, L_colind, LT_rownnz, LT_rowadr,
+      LT_colind, LT_map, sparseH, H_rownnz, H_rowadr, H_colind, scratch);
+  EXPECT_EQ(rank, n - 1);
+
+  // row 1 is decoupled: clamped diagonal, zero off-diagonals
+  int adr1 = L_rowadr[1], nnz1 = L_rownnz[1];
+  EXPECT_EQ(L[adr1 + nnz1 - 1], mju_sqrt(mjMINVAL));
+  for (int i = 0; i < nnz1 - 1; i++) {
+    EXPECT_EQ(L[adr1 + i], 0);
+  }
+
+  // row 0 keeps its pivot, every entry is bounded
+  EXPECT_EQ(L[L_rowadr[0] + L_rownnz[0] - 1], 1);
+  for (int i = 0; i < nnz; i++) {
+    EXPECT_LE(mju_abs(L[i]), 1) << "entry " << i;
+  }
+}
+
 // ----------------------------- dense LU --------------------------------------
 
 using DenseLUTest = MujocoTest;