Add manual test for CG converence

Also add MJTOL_SCALE to fixture to allow tests to be run with zero tolerance. This is useful when assesing the impact of code changes (A/B comparison of failure values)

PiperOrigin-RevId: 924219083
Change-Id: Ifdd09ac850904ca8dd79179930ce738a4b37d284

test/engine/engine_cg_convergence_test.cc

@@ -0,0 +1,374 @@
+// Copyright 2026 DeepMind Technologies Limited
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// CG solver convergence benchmark.
+//
+// Rolls out Newton ground truth on 2humanoid100.xml, then evaluates CG qacc
+// error. Pass 1: vary iterations with/without warmstart. Pass 2: vary
+// tolerance. Pass 3: consecutive stepping. No assertions, only data.
+
+#include <chrono>  // NOLINT
+#include <cstdio>
+#include <ratio>  // NOLINT
+#include <string>
+#include <vector>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <mujoco/mujoco.h>
+#include "test/fixture.h"
+
+namespace mujoco {
+namespace {
+
+using ::testing::NotNull;
+
+mjtNum gettm(void) {
+  using Clock = std::chrono::steady_clock;
+  using Microseconds = std::chrono::duration<mjtNum, std::micro>;
+  static const Clock::time_point tm_start = Clock::now();
+  return Microseconds(Clock::now() - tm_start).count();
+}
+
+using CgConvergenceTest = MujocoTest;
+
+TEST_F(CgConvergenceTest, CGConvergence) {
+  static const char* const kPath =
+      "engine/testdata/island/2humanoid100.xml";
+  const std::string xml_path = GetTestDataFilePath(kPath);
+  char error[1024];
+  mjModel* model =
+      mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
+  ASSERT_THAT(model, NotNull()) << error;
+
+  // disable islands: monolithic solver makes statistics simpler
+  model->opt.disableflags |= mjDSBL_ISLAND;
+
+  int nq = model->nq;
+  int nv = model->nv;
+
+  // stronger sideways gravity
+  model->opt.gravity[0] = -2;
+  model->opt.gravity[1] = -2;
+  model->opt.gravity[2] = -10;
+
+  // configure Newton ground truth: tolerance 0, generous iterations, warmstart
+  model->opt.solver = mjSOL_NEWTON;
+  model->opt.jacobian = mjJAC_SPARSE;
+  model->opt.tolerance = 0;
+  model->opt.iterations = 10;
+  model->opt.disableflags &= ~mjDSBL_WARMSTART;
+
+  mjData* data = mj_makeData(model);
+  mjcb_time = gettm;
+
+  // roll out Newton for kNumSteps, save pre-step state and post-step qacc
+  constexpr int kNumSteps = 1000;
+  std::vector<mjtNum> all_qpos(kNumSteps * nq);
+  std::vector<mjtNum> all_qvel(kNumSteps * nv);
+  std::vector<mjtNum> all_warmstart(kNumSteps * nv);
+  std::vector<mjtNum> all_qacc(kNumSteps * nv);
+
+  for (int i = 0; i < kNumSteps; i++) {
+    // save pre-step state
+    mju_copy(all_qpos.data() + i*nq, data->qpos, nq);
+    mju_copy(all_qvel.data() + i*nv, data->qvel, nv);
+    mju_copy(all_warmstart.data() + i*nv, data->qacc_warmstart, nv);
+
+    // step with Newton
+    mj_step(model, data);
+
+    // save qacc (set by mj_forward inside mj_step)
+    mju_copy(all_qacc.data() + i*nv, data->qacc, nv);
+  }
+
+  // evaluation points: 100 evenly spaced
+  constexpr int kNumEval = 100;
+  constexpr int kStride = kNumSteps / kNumEval;
+
+  // iteration counts to test
+  constexpr int kIterCounts[] = {5, 10, 20, 40, 80, 160};
+  constexpr int kNumIter = sizeof(kIterCounts) / sizeof(kIterCounts[0]);
+
+  // print header
+  std::printf("\nCG Convergence: 2humanoid100.xml\n");
+  std::printf("  %d Newton steps, %d evaluation points\n",
+              kNumSteps, kNumEval);
+  std::printf("  nv = %d, nq = %d\n", nv, nq);
+  std::printf("  metric: ||qacc_cg - qacc_newton|| / ||qacc_newton||\n");
+
+  // table header
+  std::printf("\n  Warmstart (tolerance = 0):\n");
+  std::printf("  %6s | %11s | %11s | %10s | %8s\n",
+              "Iters", "Mean Err", "Max Err", "Mean Iters", "LS evals");
+  std::printf("  %s\n",
+              "-------+-------------+-------------+------------+---------");
+
+  // configure CG: full rollout (tolerance 0), warmstart enabled
+  model->opt.solver = mjSOL_CG;
+  model->opt.tolerance = 0;
+  model->opt.disableflags &= ~mjDSBL_WARMSTART;
+
+  for (int c = 0; c < kNumIter; c++) {
+    model->opt.iterations = kIterCounts[c];
+
+    int total_iters = 0;
+    int total_neval = 0;
+    mjtNum sum_rel_err = 0;
+    mjtNum max_rel_err = 0;
+
+    for (int e = 0; e < kNumEval; e++) {
+      int idx = e * kStride;
+
+      // restore state
+      mju_copy(data->qpos, all_qpos.data() + idx*nq, nq);
+      mju_copy(data->qvel, all_qvel.data() + idx*nv, nv);
+      mju_copy(data->qacc_warmstart, all_warmstart.data() + idx*nv, nv);
+
+      // run CG forward
+      mj_forward(model, data);
+      int niter = data->solver_niter[0];
+      total_iters += niter;
+      for (int j = 0; j < niter && j < mjNSOLVER; j++) {
+        total_neval += data->solver[j].neval;
+      }
+
+      // compute relative error
+      mjtNum newton_norm = mju_norm(all_qacc.data() + idx*nv, nv);
+      mjtNum err = 0;
+      for (int j = 0; j < nv; j++) {
+        mjtNum diff = data->qacc[j] - all_qacc[idx*nv + j];
+        err += diff * diff;
+      }
+      mjtNum rel_err = mju_sqrt(err) / mju_max(newton_norm, 1e-10);
+      sum_rel_err += rel_err;
+      if (rel_err > max_rel_err) {
+        max_rel_err = rel_err;
+      }
+    }
+
+    mjtNum mean_iters = static_cast<mjtNum>(total_iters) / kNumEval;
+    std::printf("  %6d | %11.4e | %11.4e | %10.2f | %8d\n",
+                kIterCounts[c], sum_rel_err / kNumEval, max_rel_err,
+                mean_iters, total_neval);
+  }
+
+  std::printf("  %s\n",
+              "-------+-------------+-------------+------------+---------");
+
+  // --- no warmstart (tolerance = 0) ---
+  std::printf("\n  No warmstart (tolerance = 0):\n");
+  std::printf("  %6s | %11s | %11s | %10s | %8s\n",
+              "Iters", "Mean Err", "Max Err", "Mean Iters", "LS evals");
+  std::printf("  %s\n",
+              "-------+-------------+-------------+------------+---------");
+
+  model->opt.disableflags |= mjDSBL_WARMSTART;
+
+  for (int c = 0; c < kNumIter; c++) {
+    model->opt.iterations = kIterCounts[c];
+
+    int total_iters = 0;
+    int total_neval = 0;
+    mjtNum sum_rel_err = 0;
+    mjtNum max_rel_err = 0;
+
+    for (int e = 0; e < kNumEval; e++) {
+      int idx = e * kStride;
+
+      mju_copy(data->qpos, all_qpos.data() + idx*nq, nq);
+      mju_copy(data->qvel, all_qvel.data() + idx*nv, nv);
+
+      mj_forward(model, data);
+      int niter = data->solver_niter[0];
+      total_iters += niter;
+      for (int j = 0; j < niter && j < mjNSOLVER; j++) {
+        total_neval += data->solver[j].neval;
+      }
+
+      mjtNum newton_norm = mju_norm(all_qacc.data() + idx*nv, nv);
+      mjtNum err = 0;
+      for (int j = 0; j < nv; j++) {
+        mjtNum diff = data->qacc[j] - all_qacc[idx*nv + j];
+        err += diff * diff;
+      }
+      mjtNum rel_err = mju_sqrt(err) / mju_max(newton_norm, 1e-10);
+      sum_rel_err += rel_err;
+      if (rel_err > max_rel_err) {
+        max_rel_err = rel_err;
+      }
+    }
+
+    mjtNum mean_iters = static_cast<mjtNum>(total_iters) / kNumEval;
+    std::printf("  %6d | %11.4e | %11.4e | %10.2f | %8d\n",
+                kIterCounts[c], sum_rel_err / kNumEval, max_rel_err,
+                mean_iters, total_neval);
+  }
+
+  std::printf("  %s\n",
+              "-------+-------------+-------------+------------+---------");
+
+  // --- tolerance sweep (iterations = 100, warmstart) ---
+  std::printf("\n  Tolerance sweep (iterations = 100, warmstart):\n");
+  std::printf("  %10s | %11s | %11s | %9s | %10s | %11s | %8s\n",
+              "Tol", "Mean Err", "Max Err", "Mean Iters", "Max Iters",
+              "Solver us", "LS evals");
+  std::printf("  %s\n",
+              "-----------+-------------+-------------+"
+              "------------+------------+-------------+---------");
+
+  model->opt.solver = mjSOL_CG;
+  model->opt.iterations = 100;
+  model->opt.disableflags &= ~mjDSBL_WARMSTART;
+
+  constexpr mjtNum kTolValues[] = {1e-4, 1e-6, 1e-8, 1e-10, 1e-12, 0};
+  constexpr int kNumTol = sizeof(kTolValues) / sizeof(kTolValues[0]);
+
+  mjtNum total_solver_time = 0;
+  int grand_total_iters = 0;
+
+  for (int c = 0; c < kNumTol; c++) {
+    for (int i = 0; i < mjNTIMER; i++) {
+      data->timer[i].duration = 0;
+      data->timer[i].number = 0;
+    }
+
+    model->opt.tolerance = kTolValues[c];
+
+    int total_iters = 0;
+    int max_iters = 0;
+    int total_neval = 0;
+    mjtNum sum_rel_err = 0;
+    mjtNum max_rel_err = 0;
+
+    for (int e = 0; e < kNumEval; e++) {
+      int idx = e * kStride;
+
+      mju_copy(data->qpos, all_qpos.data() + idx*nq, nq);
+      mju_copy(data->qvel, all_qvel.data() + idx*nv, nv);
+      mju_copy(data->qacc_warmstart, all_warmstart.data() + idx*nv, nv);
+
+      mj_forward(model, data);
+      int niter = data->solver_niter[0];
+      total_iters += niter;
+      if (niter > max_iters) {
+        max_iters = niter;
+      }
+      for (int j = 0; j < niter && j < mjNSOLVER; j++) {
+        total_neval += data->solver[j].neval;
+      }
+
+      mjtNum newton_norm = mju_norm(all_qacc.data() + idx*nv, nv);
+      mjtNum err = 0;
+      for (int j = 0; j < nv; j++) {
+        mjtNum diff = data->qacc[j] - all_qacc[idx*nv + j];
+        err += diff * diff;
+      }
+      mjtNum rel_err = mju_sqrt(err) / mju_max(newton_norm, 1e-10);
+      sum_rel_err += rel_err;
+      if (rel_err > max_rel_err) {
+        max_rel_err = rel_err;
+      }
+    }
+
+    mjtNum solver_time = data->timer[mjTIMER_CONSTRAINT].duration;
+    total_solver_time += solver_time;
+    grand_total_iters += total_iters;
+
+    mjtNum mean_iters = static_cast<mjtNum>(total_iters) / kNumEval;
+    if (kTolValues[c] > 0) {
+      std::printf(
+          "  %10.0e | %11.4e | %11.4e | %10.2f | %10d | %11.2f | %8d\n",
+          kTolValues[c], sum_rel_err / kNumEval, max_rel_err,
+          mean_iters, max_iters, solver_time, total_neval);
+    } else {
+      std::printf(
+          "  %10s | %11.4e | %11.4e | %10.2f | %10d | %11.2f | %8d\n",
+          "0", sum_rel_err / kNumEval, max_rel_err,
+          mean_iters, max_iters, solver_time, total_neval);
+    }
+  }
+
+  mjtNum overall_avg = grand_total_iters > 0
+      ? total_solver_time / grand_total_iters : 0.0;
+  std::printf("  %s\n",
+              "-----------+-------------+-------------+"
+              "------------+------------+-------------+---------");
+  std::printf("  Total solver time: %.2f us, avg time per iter: %.4f us\n",
+              total_solver_time, overall_avg);
+
+  // --- third pass: consecutive stepping (mini-testspeed) ---
+  // uses model defaults: tolerance = 1e-8, iterations = 100
+  std::printf("\n  Pipeline mode (consecutive mj_step, tolerance = 1e-8):\n");
+
+  model->opt.solver = mjSOL_CG;
+  model->opt.tolerance = 1e-8;
+  model->opt.iterations = 100;
+  model->opt.disableflags &= ~mjDSBL_WARMSTART;
+
+  // reset data to initial state
+  mj_resetData(model, data);
+
+  // clear timers
+  for (int i = 0; i < mjNTIMER; i++) {
+    data->timer[i].duration = 0;
+    data->timer[i].number = 0;
+  }
+
+  // run consecutive steps
+  constexpr int kPipeSteps = 1000;
+  int pipe_total_iters = 0;
+  int pipe_total_neval = 0;
+  for (int i = 0; i < kPipeSteps; i++) {
+    mj_step(model, data);
+    int niter = data->solver_niter[0];
+    pipe_total_iters += niter;
+    for (int j = 0; j < niter && j < mjNSOLVER; j++) {
+      pipe_total_neval += data->solver[j].neval;
+    }
+  }
+
+  mjtNum pipe_constraint = data->timer[mjTIMER_CONSTRAINT].duration;
+  mjtNum pipe_step = data->timer[mjTIMER_STEP].duration;
+  int pipe_step_count = data->timer[mjTIMER_STEP].number;
+  mjtNum us_per_step = pipe_step_count > 0 ? pipe_step / pipe_step_count : 0;
+  mjtNum constraint_per_step = pipe_step_count > 0
+      ? pipe_constraint / pipe_step_count : 0;
+  mjtNum iters_per_step = pipe_step_count > 0
+      ? static_cast<mjtNum>(pipe_total_iters) / pipe_step_count : 0;
+  mjtNum steps_per_sec = us_per_step > 0 ? 1e6 / us_per_step : 0;
+
+  std::printf("  %d steps, nv = %d\n", kPipeSteps, nv);
+  std::printf("  Steps/s          : %.0f\n", steps_per_sec);
+  std::printf("  us/step (total)  : %.1f\n", us_per_step);
+  std::printf("  us/step (constr) : %.1f  (%.1f%%)\n",
+              constraint_per_step,
+              us_per_step > 0 ? 100*constraint_per_step/us_per_step : 0.0);
+  std::printf("  CG iters/step    : %.2f\n", iters_per_step);
+  std::printf("  LS evals/step    : %.2f\n",
+              pipe_step_count > 0
+                  ? static_cast<mjtNum>(pipe_total_neval) / pipe_step_count
+                  : 0.0);
+  std::printf("  us/iter          : %.2f\n",
+              pipe_total_iters > 0
+                  ? pipe_constraint / pipe_total_iters : 0.0);
+
+  std::printf("\n");
+  mjcb_time = nullptr;
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+}  // namespace
+}  // namespace mujoco

test/engine/engine_core_util_test.cc

@@ -176,7 +176,7 @@ TEST_F(AngMomMatTest, CompareAngMomMats) {
   mj_angmomMat(model, data, angmom_mat, bodyid);
 
   // compute the angular momentum matrix using finite differences
-  static constexpr mjtNum eps = MjTol(1e-6, 1e-3);
+  static const mjtNum eps = MjTol(1e-6, 1e-3);
   for (int i = 0; i < nv; i++) {
     // reset vel, forward nudge i-th dof, get angmom
     mju_copy(data->qvel, model->key_qvel, model->nv);
@@ -505,7 +505,7 @@ TEST_F(JacobianTest, JacDot) {
     mj_jacDot(model, data, jacp_dot.data(), jacr_dot.data(), point, bodyid);
 
     // jac_h: jacobian after integrating qpos with a timestep of h
-    constexpr mjtNum h = MjTol(1e-7, 5e-4);
+    const mjtNum h = MjTol(1e-7, 5e-4);
     mj_integratePos(model, data->qpos, data->qvel, h);
     mj_kinematics(model, data);
     mj_comPos(model, data);

test/engine/engine_passive_test.cc

@@ -297,7 +297,7 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
   d1->qpos[6] = 0.5;
 
   // tolerance for floating point numbers
-  constexpr mjtNum tol = MjTol(1e-14, 1e-5);
+  const mjtNum tol = MjTol(1e-14, 1e-5);
 
   EXPECT_EQ(m1->nv, m2->nv);
 
@@ -552,7 +552,7 @@ TEST_F(ElasticityTest, ElasticEnergyMembrane) {
           energy += metric[21*t+idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
         }
       }
-      constexpr mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-5);
+      const mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-5);
       EXPECT_NEAR(4*energy/volume, 2*scale*scale, tol);
     }
   }
@@ -603,7 +603,7 @@ TEST_F(ElasticityTest, ElasticEnergySolid) {
           energy += metric[21*t+idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
         }
       }
-      constexpr mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-4);
+      const mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-4);
       EXPECT_NEAR(energy/volume, 3*scale*scale, tol);
     }
   }
@@ -1019,7 +1019,7 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
   mj_forward(m, d);
 
   // spring forces should still be zero after rigid rotation
-  constexpr mjtNum tol = MjTol(1e-6, 1e-3);
+  const mjtNum tol = MjTol(1e-6, 1e-3);
   for (int i = 0; i < m->nv; i++) {
     EXPECT_NEAR(d->qfrc_spring[i], 0, tol)
         << "nonzero spring force at DOF " << i << " after rigid rotation";

test/engine/engine_solver_test.cc

@@ -26,9 +26,8 @@
 namespace mujoco {
 namespace {
 
-using ::testing::NotNull;
-using ::testing::Pointwise;
 using ::std::max;
+using ::testing::NotNull;
 
 using SolverTest = MujocoTest;
 
@@ -60,16 +59,14 @@ TEST_F(SolverTest, IslandsEquivalent) {
   mjtNum maxiter[kNumTol] = {30,   40,   60};
   // Below are 3 tolerances associated with 3 different iteration counts.
   // Tolerances are set to be ~12x higher than failure thresholds.
-  // For float32, failure thresholds are ~6000x larger than for float64.
-  // Line 99 adds a 500x factor for float32, so we need another ~12x in rtol.
   // The point of this test is to show that CG convergence is actually not very
   // precise, simply changing whether islands are used changes the solution by
   // quite a lot, even at high iteration count and zero {ls_}tolerance.
   // Increasing the iteration count higher than 60 does not improve convergence.
   mjtNum rtol[kNumTol] = {
-      MjTol(6e-2, 7.2e-1),
+      MjTol(1e-1, 2),
-      MjTol(6e-3, 7.2e-2),
+      MjTol(3e-2, 1),
-      MjTol(6e-4, 7.2e-3)
+      MjTol(1.5e-4, 3.6)
   };
 
   for (int i = 0; i < kNumTol; ++i) {
@@ -85,6 +82,14 @@ TEST_F(SolverTest, IslandsEquivalent) {
         model->opt.disableflags &= ~mjDSBL_WARMSTART;
       }
 
+      mjtNum max_ratio = 0;
+      mjtNum worst_diff = 0;
+      mjtNum worst_scale = 1.0;
+      mjtNum worst_expected = 0;
+      mjtNum worst_actual = 0;
+      std::string worst_time = "";
+      int worst_dof = -1;
+
       while (data_noisland->time < .1) {
         mj_getState(model, data_noisland, state, mjSTATE_INTEGRATION);
         mj_setState(model, data_island, state, mjSTATE_INTEGRATION);
@@ -95,22 +100,38 @@ TEST_F(SolverTest, IslandsEquivalent) {
         model->opt.disableflags |= mjDSBL_ISLAND;  // disable islands
         mj_forward(model, data_noisland);
 
-        auto time = std::to_string(data_noisland->time);
         for (int j = 0; j < nv; j++) {
-          // increase tolerance for large elements
+          mjtNum diff = std::abs(data_noisland->qacc[j] - data_island->qacc[j]);
           mjtNum scale = 0.5 * max(static_cast<mjtNum>(2.0),
                                    std::abs(data_noisland->qacc[j]) +
-                                   std::abs(data_island->qacc[j]));
+                                       std::abs(data_island->qacc[j]));
-          EXPECT_NEAR(data_noisland->qacc[j], data_island->qacc[j],
+          mjtNum ratio = diff / scale;
-                      MjTol(scale * rtol[i], 500 * scale * rtol[i]))
+          if (ratio > max_ratio) {
-              << "time: " << time << '\n'
+            max_ratio = ratio;
-              << "dof: " << j << '\n'
+            worst_diff = diff;
-              << "maxiter: " << maxiter[i] << '\n'
+            worst_scale = scale;
-              << "rtol: " << scale * rtol[i];
+            worst_expected = data_island->qacc[j];
+            worst_actual = data_noisland->qacc[j];
+            worst_time = std::to_string(data_noisland->time);
+            worst_dof = j;
+          }
         }
 
         mj_step(model, data_noisland);
       }
+
+      // Assert once per condition with the worst offender.
+      // rtol[i] is already scaled by MjTolScale() at initialization.
+      mjtNum allowed_tol = worst_scale * rtol[i];
+      EXPECT_NEAR(worst_actual, worst_expected, allowed_tol)
+          << "Worst offender info:\n"
+          << "time: " << worst_time << '\n'
+          << "dof: " << worst_dof << '\n'
+          << "maxiter: " << maxiter[i] << '\n'
+          << "coldstart: " << coldstart << '\n'
+          << "rtol: " << worst_scale * rtol[i] << '\n'
+          << "actual diff: " << worst_diff << " (allowed: " << allowed_tol
+          << ")";
     }
   }
 
@@ -161,16 +182,36 @@ TEST_F(SolverTest, IslandsEquivalentForward) {
           model->opt.disableflags &= ~mjDSBL_ISLAND;   // enable islands
           mj_forward(model, data_island);
 
+          mjtNum max_diff = 0;
+          mjtNum worst_expected = 0;
+          mjtNum worst_actual = 0;
+          int worst_idx = -1;
           mjtNum scale = 0.5 * (mju_norm(data_noisland->qacc, nv) +
                                 mju_norm(data_island->qacc, nv));
-          mjtNum tol = scale * (solver == mjSOL_CG ? 1e-6 : 1e-8);
+          mjtNum rtol = solver == mjSOL_CG ? MjTol(1e-6, 1e-2)
-          EXPECT_THAT(AsVector(data_island->qacc, nv),
+                                           : MjTol(1e-13, 1e-3);
-                      Pointwise(MjNear(scale * tol, 500 * scale * tol),
+          mjtNum worst_allowed = scale * rtol;
-                                AsVector(data_noisland->qacc, nv)))
+
+          for (int j = 0; j < nv; j++) {
+            mjtNum diff =
+                std::abs(data_island->qacc[j] - data_noisland->qacc[j]);
+            if (diff > max_diff) {
+              max_diff = diff;
+              worst_expected = data_noisland->qacc[j];
+              worst_actual = data_island->qacc[j];
+              worst_idx = j;
+            }
+          }
+
+          EXPECT_NEAR(worst_actual, worst_expected, worst_allowed)
+              << "Worst offender in IslandsEquivalentForward:\n"
+              << "idx: " << worst_idx << '\n'
               << "warmstart: " << warmstart << '\n'
               << "jacobian: " << (jacobian ? "sparse" : "dense") << '\n'
               << "solver: " << (solver == mjSOL_CG ? "CG" : "Newton") << '\n'
-              << "cone: " << (cone == 1 ? "elliptic" : "pyramidal");
+              << "cone: " << (cone == 1 ? "elliptic" : "pyramidal") << '\n'
+              << "actual diff: " << max_diff << " (allowed: " << worst_allowed
+              << ")";
         }
       }
     }
@@ -183,15 +224,14 @@ TEST_F(SolverTest, IslandsEquivalentForward) {
 
 TEST_F(SolverTest, SolversEquivalent) {
   struct SolverTolerances {
-    double newton;
+    mjtNum newton;
-    double cg;
+    mjtNum cg;
-    double pgs_pyramidal;
+    mjtNum pgs_pyramidal;
-    double pgs_elliptic;
+    mjtNum pgs_elliptic;
   };
 
-  // Base relative tolerances are factor of 10 above failure thresholds
+  // Relative tolerances: 10x above failure thresholds on Linux, clang, x86-64.
-  // on Linux, clang, x86-64 (i.e., test just passes with tol_multiplier = 1)
+  // MjTol(f64, f32) selects the appropriate tolerance for the current build.
-  // TODO: Get float32 tolerances
   const struct {
     const char* path;
     SolverTolerances tolerances;
@@ -199,18 +239,18 @@ TEST_F(SolverTest, SolversEquivalent) {
       {.path = kModelPath,
        .tolerances =
            {
-               .newton        = 1e-15,
+               .newton        = MjTol(1e-13, 1e-5),
-               .cg            = 1e-7,
+               .cg            = MjTol(1e-5,  1e-2),
-               .pgs_pyramidal = 1e-14,
+               .pgs_pyramidal = MjTol(1e-12, 1e-5),
-               .pgs_elliptic  = 1e-3,
+               .pgs_elliptic  = MjTol(1e-3,  1e-2),
            }},
       {.path = kHumanoidPath,
        .tolerances =
            {
-               .newton        = 1e-15,
+               .newton        = MjTol(1e-13, 1e-5),
-               .cg            = 1e-7,
+               .cg            = MjTol(1e-5,  1e-2),
-               .pgs_pyramidal = 1e-6,
+               .pgs_pyramidal = MjTol(1e-5,  1e-5),
-               .pgs_elliptic  = 1e-9,
+               .pgs_elliptic  = MjTol(1e-8,  1e-4),
            }},
   };
 
@@ -241,7 +281,7 @@ TEST_F(SolverTest, SolversEquivalent) {
       mjtNum scale = mju_norm(data_truth->qfrc_constraint, nv);
 
       for (mjtSolver solver : {mjSOL_NEWTON, mjSOL_CG, mjSOL_PGS}) {
-        double rtol;
+        mjtNum rtol;
         switch (solver) {
           case mjSOL_NEWTON:
             rtol = config.tolerances.newton;
@@ -255,9 +295,7 @@ TEST_F(SolverTest, SolversEquivalent) {
             break;
         }
 
-        // increase base tolerance to avoid test flakiness
+        mjtNum tolerance = scale * rtol;
-        double tol_multiplier = 1e2;
-        double tolerance = scale * rtol * tol_multiplier;
 
         for (mjtJacobian jacobian : {mjJAC_DENSE, mjJAC_SPARSE}) {
           model->opt.solver = solver;
@@ -274,15 +312,31 @@ TEST_F(SolverTest, SolversEquivalent) {
           const char* jacobian_str =
               (jacobian == mjJAC_DENSE ? "dense" : "sparse");
 
-          EXPECT_THAT(AsVector(data->qfrc_constraint, nv),
+          mjtNum max_diff = 0;
-                      Pointwise(MjNear(tolerance,
+          mjtNum worst_expected = 0;
-                                       max(1e-1, 1000 * tolerance)),
+          mjtNum worst_actual = 0;
-                                AsVector(data_truth->qfrc_constraint, nv)))
+          int worst_idx = -1;
+
+          for (int j = 0; j < nv; j++) {
+            mjtNum diff = std::abs(data->qfrc_constraint[j] -
+                                   data_truth->qfrc_constraint[j]);
+            if (diff > max_diff) {
+              max_diff = diff;
+              worst_expected = data_truth->qfrc_constraint[j];
+              worst_actual = data->qfrc_constraint[j];
+              worst_idx = j;
+            }
+          }
+
+          EXPECT_NEAR(worst_actual, worst_expected, tolerance)
+              << "Worst offender in SolversEquivalent:\n"
+              << "idx: " << worst_idx << '\n'
               << "model: " << config.path << "\n"
               << "cone: " << cone_str << "\n"
               << "solver: " << solver_str << "\n"
               << "jacobian: " << jacobian_str << "\n"
-              << "tolerance: " << tolerance;
+              << "actual diff: " << max_diff << " (allowed: " << tolerance
+              << ")";
         }
       }
     }

test/engine/engine_util_misc_test.cc

@@ -67,8 +67,8 @@ TEST_F(UtilMiscTest, Sigmoid) {
   EXPECT_EQ(mju_sigmoid(2),   1);
 
   // epsilon for finite-differencing
-  constexpr mjtNum dx = MjTol(1e-7, 1e-3);
+  const mjtNum dx = MjTol(1e-7, 1e-3);
-  constexpr mjtNum fd_tol = MjTol(1e-7, 1e-3);
+  const mjtNum fd_tol = MjTol(1e-7, 1e-3);
 
   // derivative at 0
   mjtNum dy_dx_0 = (mju_sigmoid(0 + dx) - mju_sigmoid(0)) / dx;

test/engine/engine_util_solve_test.cc

@@ -93,8 +93,8 @@ mjtNum objective(const mjtNum* x, const mjtNum* H, const mjtNum* g, int n) {
 // utility: test if res is the minimum of a given box-QP problem
 bool isQPminimum(const mjtNum* res, const mjtNum* H, const mjtNum* g, int n,
                  const mjtNum* lower, const mjtNum* upper) {
-  constexpr mjtNum eps = MjTol(1e-4, 5e-2);  // epsilon used for nudging
+  const mjtNum eps = MjTol(1e-4, 5e-2);  // epsilon used for nudging
-  constexpr mjtNum threshold = MjTol(0, -2e-3);  // comparison threshold
+  const mjtNum threshold = MjTol(0, -2e-3);  // comparison threshold
   bool is_minimum = true;
   mjtNum* res_nudge = (mjtNum*) mju_malloc(sizeof(mjtNum)*n);
 

test/engine/engine_util_spatial_test.cc

@@ -121,7 +121,7 @@ TEST_F(RotVecQuatTest, TestEquivalence) {
       {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {-0.5, 1, -0.5}, {1.22, -2.33, 3.44}};
   // List of angles to rotate by, in degrees
   mjtNum angles[6] = {0.0, 1e-8, 31, 47, 181, 271};
-  static constexpr mjtNum eps = MjTol(1e-15, 1e-5);
+  static const mjtNum eps = MjTol(1e-15, 1e-5);
   for (auto vec : vecs) {
     // Unit-normalize the vector
     mju_normalize3(vec);

test/fixture.h

@@ -16,7 +16,7 @@
 #define MUJOCO_TEST_FIXTURE_H_
 
 #include <csetjmp>
-#include <cstdio>  // IWYU pragma: keep
+#include <cstdio>   // IWYU pragma: keep
 #include <cstdlib>  // IWYU pragma: keep
 #include <cstring>
 #include <iomanip>
@@ -35,51 +35,63 @@
 
 extern "C" {
 MJAPI void _mjPRIVATE__set_tls_error_fn(decltype(mju_user_error));
-MJAPI decltype(mju_user_error)  _mjPRIVATE__get_tls_error_fn();
+MJAPI decltype(mju_user_error) _mjPRIVATE__get_tls_error_fn();
 }
 
 namespace mujoco {
 
+// Runtime scale factor for test tolerances, controlled by MJTOL_SCALE env var.
+// Set MJTOL_SCALE=0 to run tests with zero tolerance and see actual residuals.
+inline mjtNum MjTolScale() {
+  static const mjtNum scale = []() {
+    const char* env = std::getenv("MJTOL_SCALE");
+    return env ? std::atof(env) : 1.0;
+  }();
+  return scale;
+}
+
 // Precision-aware GMock matcher. Use instead of DoubleNear/FloatNear.
 // Under double builds, uses double_tol. Under float builds, uses float_tol.
+// Scaled by MJTOL_SCALE env var (default 1.0).
 template <typename T1, typename T2>
 inline auto MjNear(T1 double_tol, T2 float_tol) {
 #ifdef mjUSESINGLE
-  return ::testing::FloatNear(static_cast<float>(float_tol));
+  return ::testing::FloatNear(static_cast<float>(float_tol) * MjTolScale());
 #else
-  return ::testing::DoubleNear(static_cast<double>(double_tol));
+  return ::testing::DoubleNear(static_cast<double>(double_tol) * MjTolScale());
 #endif
 }
 
 // Precision-aware GMock matcher (3-arg version).
 // Under double builds, matches near target with double_tol.
 // Under float builds, matches near target with float_tol.
+// Scaled by MJTOL_SCALE env var (default 1.0).
 template <typename T1, typename T2, typename T3>
 inline auto MjNear(T1 target, T2 double_tol, T3 float_tol) {
 #ifdef mjUSESINGLE
   return ::testing::FloatNear(static_cast<float>(target),
-                              static_cast<float>(float_tol));
+                              static_cast<float>(float_tol) * MjTolScale());
 #else
   return ::testing::DoubleNear(static_cast<double>(target),
-                               static_cast<double>(double_tol));
+                               static_cast<double>(double_tol) * MjTolScale());
 #endif
 }
-
 // Precision-aware tolerance for EXPECT_NEAR.
+// Scaled by MJTOL_SCALE env var (default 1.0).
 template <typename T1, typename T2>
-constexpr mjtNum MjTol(T1 double_tol, T2 float_tol) {
+inline mjtNum MjTol(T1 double_tol, T2 float_tol) {
 #ifdef mjUSESINGLE
-  return static_cast<mjtNum>(float_tol);
+  return static_cast<mjtNum>(float_tol) * MjTolScale();
 #else
-  return static_cast<mjtNum>(double_tol);
+  return static_cast<mjtNum>(double_tol) * MjTolScale();
 #endif
 }
 
 // Precision-aware equality assertion: 4 ULPs in either precision.
 #ifdef mjUSESINGLE
-  #define EXPECT_MJTNUM_EQ(a, b) EXPECT_FLOAT_EQ(a, b)
+#define EXPECT_MJTNUM_EQ(a, b) EXPECT_FLOAT_EQ(a, b)
 #else
-  #define EXPECT_MJTNUM_EQ(a, b) EXPECT_DOUBLE_EQ(a, b)
+#define EXPECT_MJTNUM_EQ(a, b) EXPECT_DOUBLE_EQ(a, b)
 #endif
 
 // Installs and uninstalls error callbacks on MuJoCo that fail the currently
@@ -187,7 +199,7 @@ inline void PrintMatrix(const mjtNum* mat, int nrow, int ncol, int p = 5,
   std::cerr << name << "\n";
   for (int r = 0; r < nrow; r++) {
     for (int c = 0; c < ncol; c++) {
-      mjtNum val = mat[c + r*ncol];
+      mjtNum val = mat[c + r * ncol];
       if (val) {
         std::cerr << std::fixed << std::setw(5 + p) << val << " ";
       } else {
@@ -232,7 +244,6 @@ class MockFilesystem {
                       const unsigned char** buffer) const;
   std::string FullPath(const std::string& path) const;
 
-
  private:
   std::string StripPrefix(const char* path) const;
   static std::string PathReduce(const std::string& current_dir,
@@ -244,6 +255,5 @@ class MockFilesystem {
   std::string dir_;  // current directory
 };
 
-
 }  // namespace mujoco
 #endif  // MUJOCO_TEST_FIXTURE_H_

test/pipeline_test.cc

@@ -42,7 +42,7 @@ TEST_F(PipelineTest, SparseDenseEquivalent) {
   ASSERT_THAT(model, NotNull())  << error;
   mjData* data = mj_makeData(model);
 
-  constexpr mjtNum tol = MjTol(1e-11, 1e-4);
+  const mjtNum tol = MjTol(1e-11, 1e-4);
 
   const char* sname[4] = {"NEWTON", "PGS", "CG", "NOSLIP"};
   mjtSolver solver[4] = {mjSOL_NEWTON, mjSOL_PGS, mjSOL_CG, mjSOL_NEWTON};

test/user/user_objects_test.cc

@@ -31,7 +31,7 @@
 namespace mujoco {
 namespace {
 
-constexpr double kInertiaTol = MjTol(1e-6, 1e-6);
+const double kInertiaTol = MjTol(1e-6, 1e-6);
 
 using std::string;
 using ::testing::ElementsAre;

test/xml/xml_native_reader_test.cc

@@ -1378,7 +1378,7 @@ TEST_F(XMLReaderTest, ParseReplicate) {
 
   // check body positions
   mjtNum pos[2] = {0, 0};
-  constexpr mjtNum tol = MjTol(1e-8, 1e-3);
+  const mjtNum tol = MjTol(1e-8, 1e-3);
   for (int i = 1; i < 102; ++i) {
     mjtNum theta = (i-1) * 1.8 * mjPI / 180;
     EXPECT_NEAR(m->body_pos[3*i+0], pos[0] + sin(theta), tol) << i;