From 892d889793a19fdf2ed06d10d0de23a323223394 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sat, 11 Jul 2026 03:03:53 -0700 Subject: [PATCH] Fix numerical instability in elliptic contact line search. 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 --- src/engine/engine_solver.c | 91 ++++++++++++++++--------------- test/engine/engine_solver_test.cc | 44 +++++++++++++++ 2 files changed, 90 insertions(+), 45 deletions(-) diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index b78bead1..0a45fdf3 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -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; } diff --git a/test/engine/engine_solver_test.cc b/test/engine/engine_solver_test.cc index 0684aade..73bf15cf 100644 --- a/test/engine/engine_solver_test.cc +++ b/test/engine/engine_solver_test.cc @@ -347,5 +347,49 @@ TEST_F(SolverTest, SolversEquivalent) { } } +TEST_F(SolverTest, EllipticLineSearchPrecisionDiagnostics) { + std::string xml = R"( + + + )"; + + 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