diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 2d2d4189..ca35a5c5 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -43,7 +43,8 @@ int mju_cholFactor(mjtNum* mat, int n, mjtNum mindiag) { } // correct diagonal values below threshold - if (tmp < mindiag) { + int deficient = tmp < mindiag; + if (deficient) { tmp = mindiag; rank--; } @@ -52,9 +53,16 @@ int mju_cholFactor(mjtNum* mat, int n, mjtNum mindiag) { mat[j*(n+1)] = mju_sqrt(tmp); // process off-diagonal entries - tmp = 1/mat[j*(n+1)]; - for (int i=j+1; i < n; i++) { - mat[i*n+j] = (mat[i*n+j] - mju_dot(mat+i*n, mat+j*n, j)) * tmp; + if (deficient) { + // clear off-diagonals if deficient + for (int i=j+1; i < n; i++) { + mat[i*n+j] = 0; + } + } else { + tmp = 1/mat[j*(n+1)]; + for (int i=j+1; i < n; i++) { + mat[i*n+j] = (mat[i*n+j] - mju_dot(mat+i*n, mat+j*n, j)) * tmp; + } } } diff --git a/test/engine/engine_solver_test.cc b/test/engine/engine_solver_test.cc index 77fc8f3f..3f1ee7f0 100644 --- a/test/engine/engine_solver_test.cc +++ b/test/engine/engine_solver_test.cc @@ -621,5 +621,60 @@ TEST_F(SolverTest, ZeroToleranceDisablesTermination) { mj_deleteModel(model); } +// With condim 6 and the default friction (1, 0.005, 0.0001) the local +// elliptic-cone Hessian spans the friction ratios squared, a condition number +// around 1e9, which exhausts the single-precision mantissa. Newton factorizes +// it per contact and folds the factor into the full Hessian with rank-1 +// updates, so a Cholesky that responds to a vanishing pivot by clamping the +// diagonal and then dividing the rest of the column by it -- scaling that +// column by 1/sqrt(mindiag) -- injects enormous coupling where there is no +// curvature. This pose reached rank 4 of 6 one step before qacc went to NaN. +TEST_F(SolverTest, EllipticConeHessianSurvivesFrictionRatios) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + )"; + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + model->opt.cone = mjCONE_ELLIPTIC; + model->opt.solver = mjSOL_NEWTON; + + // both factorizations reach the same pivot, at different steps + for (mjtJacobian jacobian : {mjJAC_DENSE, mjJAC_SPARSE}) { + model->opt.jacobian = jacobian; + MjDataPtr data = MakeData(model); + + // bounded by step count, not by data->time: a divergence resets mjData and + // rewinds the clock, so a time-based loop would never terminate + for (int step = 0; step < 200; step++) { + mj_step(model.get(), data.get()); + for (int i = 0; i < mjNWARNING; i++) { + ASSERT_EQ(data->warning[i].number, 0) + << "warning " << i << " at step " << step << ", jacobian " + << jacobian; + } + } + } +} + } // namespace } // namespace mujoco