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

Fix numerical instability in elliptic contact line search.

Browse Source 
Reformulate the cost difference calculation (`ellipticCostDif`) to use mathematically equivalent formulas that avoid subtracting large, nearly equal values (cancellation errors) in single precision at high normal forces.

This is a C port of Alain's formulation in MJWarp:
https://github.com/google-deepmind/mujoco_warp/pull/1512

Also adds an integration test (`EllipticLineSearchPrecisionDiagnostics`) that reproduces the precision issue under large normal forces in the sliding regime, and asserts that the solver does not produce large negative improvements in either precision. This test failed before the change.

PiperOrigin-RevId: 946137815
Change-Id: Ia8fc1c4823b5fee770140c8989b9465737d22ad7
This commit is contained in:
Yuval Tassa
2026-07-11 03:03:53 -07:00
committed by Copybara-Service
parent 4a03a61734
commit 892d889793
2 changed files with 90 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_solver.c
+46 -45
View File
@@ -1527,44 +1527,6 @@ static mjtNum frictionCostDif(mjtNum start, mjtNum x, mjtNum f, mjtNum Rf, mjtNu
}
// compute cost of an elliptic cone at a given alpha
static mjtNum ellipticCost(const mjtNum* quad, mjtNum alpha, mjtNum mu, mjtNum Dm) {
mjtNum U0 = quad[3], V0 = quad[4], UU = quad[5];
mjtNum UV = quad[6], VV = quad[7];
mjtNum N = U0 + alpha*V0;
mjtNum Tsqr = UU + alpha*(2*UV + alpha*VV);
// no tangential force : top or bottom zone
if (Tsqr <= 0) {
// bottom zone: quadratic cost
if (N < 0) return alpha*alpha*quad[2] + alpha*quad[1] + quad[0];
// top zone: nothing to do
}
// otherwise regular processing
else {
mjtNum T = mju_sqrt(Tsqr);
// N>=mu*T : top zone
if (N >= mu*T) {
// nothing to do
}
// mu*N+T<=0 : bottom zone
else if (mu*N+T <= 0) {
return alpha*alpha*quad[2] + alpha*quad[1] + quad[0];
}
// otherwise middle zone
else {
return 0.5*Dm*(N-mu*T)*(N-mu*T);
}
}
return 0;
}
// compute cost difference of an elliptic cone at a given alpha: cost(alpha) - cost(0)
static mjtNum ellipticCostDif(const mjtNum* quad, mjtNum alpha, mjtNum mu, mjtNum Dm) {
mjtNum U0 = quad[3], V0 = quad[4], UU = quad[5];
@@ -1586,7 +1548,7 @@ static mjtNum ellipticCostDif(const mjtNum* quad, mjtNum alpha, mjtNum mu, mjtNu
}
}
// determine zone and cost at alpha
// determine zone at alpha
mjtNum N = U0 + alpha*V0;
mjtNum Tsqr = UU + alpha*(2*UV + alpha*VV);
int zone_alpha = 0;
@@ -1615,15 +1577,54 @@ static mjtNum ellipticCostDif(const mjtNum* quad, mjtNum alpha, mjtNum mu, mjtNu
return alpha*alpha*quad[2] + alpha*quad[1];
}
// both middle zone
// both middle zone: apply rationalized formula to avoid cancellation
if (zone0 == 3 && zone_alpha == 3) {
mjtNum diff_alpha = N - mu*T;
mjtNum diff0 = U0 - mu*T0;
return 0.5*Dm*(diff_alpha - diff0)*(diff_alpha + diff0);
mjtNum Tsqr_delta = alpha*(2*UV + alpha*VV);
mjtNum T_delta = Tsqr_delta / (T + T0);
mjtNum r_delta = alpha*V0 - mu*T_delta;
mjtNum r0 = U0 - mu*T0;
return 0.5*Dm*r_delta*(2*r0 + r_delta);
}
// otherwise different zones: compute absolute costs and subtract
return ellipticCost(quad, alpha, mu, Dm) - ellipticCost(quad, 0, mu, Dm);
// CONE -> QUADRATIC (3 -> 2)
if (zone0 == 3 && zone_alpha == 2) {
mjtNum dq = alpha*(alpha*quad[2] + quad[1]);
mjtNum boundary0 = mu*U0 + T0;
mjtNum gap0 = 0.5*Dm*boundary0*boundary0;
return dq + gap0;
}
// QUADRATIC -> CONE (2 -> 3)
if (zone0 == 2 && zone_alpha == 3) {
mjtNum dq = alpha*(alpha*quad[2] + quad[1]);
mjtNum boundary = mu*N + T;
mjtNum gap = 0.5*Dm*boundary*boundary;
return dq - gap;
}
// SATISFIED -> QUADRATIC (1 -> 2)
if (zone0 == 1 && zone_alpha == 2) {
return alpha*alpha*quad[2] + alpha*quad[1] + quad[0];
}
// SATISFIED -> CONE (1 -> 3)
if (zone0 == 1 && zone_alpha == 3) {
mjtNum r = N - mu*T;
return 0.5*Dm*r*r;
}
// CONE -> SATISFIED (3 -> 1)
if (zone0 == 3 && zone_alpha == 1) {
mjtNum r0 = U0 - mu*T0;
return -0.5*Dm*r0*r0;
}
// QUADRATIC -> SATISFIED (2 -> 1)
if (zone0 == 2 && zone_alpha == 1) {
return -quad[0];
}
return 0;
}
test/engine/engine_solver_test.cc
+44
View File
@@ -347,5 +347,49 @@ TEST_F(SolverTest, SolversEquivalent) {
}
}
TEST_F(SolverTest, EllipticLineSearchPrecisionDiagnostics) {
std::string xml = R"(
<mujoco>
<option cone="elliptic" solver="Newton"/>
<worldbody>
<geom name="floor" type="plane" size="10 10 1"/>
<body name="box" pos="0 0 0.499">
<joint type="free"/>
<geom type="box" size="0.5 0.5 0.5" mass="1" friction="0.5"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
MjDataPtr data = MakeData(model);
// Set gravity to 0
model->opt.gravity[0] = 0;
model->opt.gravity[1] = 0;
model->opt.gravity[2] = 0;
for (double fn : {1e2, 1e4, 1e6, 1e8}) {
mj_resetData(model.get(), data.get());
// Apply large downward force
data->qfrc_applied[2] = -fn;
// Apply large lateral force (dynamic friction limit is 0.5 * fn)
double ft = fn * 1.5;
data->qfrc_applied[0] = ft;
mj_forward(model.get(), data.get());
int niter = std::min(data->solver_niter[0], mjNSOLVER);
for (int i = 0; i < niter; ++i) {
const mjSolverStat& stat = data->solver[i];
EXPECT_GE(stat.improvement, -MjTol(1e-5, 100.0));
}
}
}
} // namespace
} // namespace mujoco