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
2026-07-11 03:03:53 -07:00
parent 4a03a61734
commit 892d889793
2 changed files with 90 additions and 45 deletions
@@ -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