chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Zero out off-diagonal entries in Cholesky factorization when diagonal is below threshold.

Browse Source 
PiperOrigin-RevId: 965909091
Change-Id: If13bf8089b16eb3e21bcc22f75d6ac1366407b36
This commit is contained in:
Yuval Tassa
2026-08-17 05:24:04 -07:00
committed by Copybara-Service
parent bbdda5dda5
commit 0fa066237a
2 changed files with 67 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_util_solve.c
+12 -4
View File
@@ -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;
}
}
}
test/engine/engine_solver_test.cc
+55
View File
@@ -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"(
<mujoco>
<default>
<geom condim="6"/>
</default>
<worldbody>
<geom name="floor" type="plane" size=".5 1 .01"/>
<body name="b1" pos="0 0 .4" euler="5 4 3">
<freejoint/>
<geom type="box" pos="0 0 .06" size=".15 .15 .03"/>
<geom type="box" pos="-.05 0 .005" size=".04 .04 .025" euler="1 1 1"/>
<geom size=".05" pos=".1 -.1 .04"/>
</body>
<body name="b2" pos="0 0 .2">
<joint type="ball" springdamper="0.1 1"/>
<geom type="box" size=".2 .2 .05"/>
<geom size=".05" pos=".1 .1 .05"/>
<geom type="box" size=".05 .05 .01" pos=".1 -.1 .06" euler="2 2 2"/>
</body>
</worldbody>
</mujoco>
)";
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