From 4358a102cd4b386c4702544df6d371b40408f7fe Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sun, 31 May 2026 03:15:06 -0700 Subject: [PATCH] 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 | 374 ++++++++++++++++++++++ test/engine/engine_core_util_test.cc | 4 +- test/engine/engine_passive_test.cc | 8 +- test/engine/engine_solver_test.cc | 144 ++++++--- test/engine/engine_util_misc_test.cc | 4 +- test/engine/engine_util_solve_test.cc | 4 +- test/engine/engine_util_spatial_test.cc | 2 +- test/fixture.h | 40 ++- test/pipeline_test.cc | 2 +- test/user/user_objects_test.cc | 2 +- test/xml/xml_native_reader_test.cc | 2 +- 11 files changed, 512 insertions(+), 74 deletions(-) create mode 100644 test/engine/engine_cg_convergence_test.cc diff --git a/test/engine/engine_cg_convergence_test.cc b/test/engine/engine_cg_convergence_test.cc new file mode 100644 index 00000000..52c9c674 --- /dev/null +++ b/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 // NOLINT +#include +#include // NOLINT +#include +#include + +#include +#include +#include +#include "test/fixture.h" + +namespace mujoco { +namespace { + +using ::testing::NotNull; + +mjtNum gettm(void) { + using Clock = std::chrono::steady_clock; + using Microseconds = std::chrono::duration; + 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 all_qpos(kNumSteps * nq); + std::vector all_qvel(kNumSteps * nv); + std::vector all_warmstart(kNumSteps * nv); + std::vector 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(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(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(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(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(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 diff --git a/test/engine/engine_core_util_test.cc b/test/engine/engine_core_util_test.cc index 8cd73dca..9bd56f7e 100644 --- a/test/engine/engine_core_util_test.cc +++ b/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); diff --git a/test/engine/engine_passive_test.cc b/test/engine/engine_passive_test.cc index be0f2850..e4bd8d47 100644 --- a/test/engine/engine_passive_test.cc +++ b/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::epsilon(), 1e-5); + const mjtNum tol = MjTol(std::numeric_limits::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::epsilon(), 1e-4); + const mjtNum tol = MjTol(std::numeric_limits::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"; diff --git a/test/engine/engine_solver_test.cc b/test/engine/engine_solver_test.cc index 8aaf1d9d..1eb99420 100644 --- a/test/engine/engine_solver_test.cc +++ b/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(6e-3, 7.2e-2), - MjTol(6e-4, 7.2e-3) + MjTol(1e-1, 2), + MjTol(3e-2, 1), + 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(2.0), std::abs(data_noisland->qacc[j]) + - std::abs(data_island->qacc[j])); - EXPECT_NEAR(data_noisland->qacc[j], data_island->qacc[j], - MjTol(scale * rtol[i], 500 * scale * rtol[i])) - << "time: " << time << '\n' - << "dof: " << j << '\n' - << "maxiter: " << maxiter[i] << '\n' - << "rtol: " << scale * rtol[i]; + std::abs(data_island->qacc[j])); + mjtNum ratio = diff / scale; + if (ratio > max_ratio) { + max_ratio = ratio; + worst_diff = diff; + worst_scale = scale; + 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); - EXPECT_THAT(AsVector(data_island->qacc, nv), - Pointwise(MjNear(scale * tol, 500 * scale * tol), - AsVector(data_noisland->qacc, nv))) + mjtNum rtol = solver == mjSOL_CG ? MjTol(1e-6, 1e-2) + : MjTol(1e-13, 1e-3); + mjtNum worst_allowed = scale * rtol; + + 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; - double cg; - double pgs_pyramidal; - double pgs_elliptic; + mjtNum newton; + mjtNum cg; + mjtNum pgs_pyramidal; + mjtNum pgs_elliptic; }; - // Base relative tolerances are factor of 10 above failure thresholds - // on Linux, clang, x86-64 (i.e., test just passes with tol_multiplier = 1) - // TODO: Get float32 tolerances + // Relative tolerances: 10x above failure thresholds on Linux, clang, x86-64. + // MjTol(f64, f32) selects the appropriate tolerance for the current build. const struct { const char* path; SolverTolerances tolerances; @@ -199,18 +239,18 @@ TEST_F(SolverTest, SolversEquivalent) { {.path = kModelPath, .tolerances = { - .newton = 1e-15, - .cg = 1e-7, - .pgs_pyramidal = 1e-14, - .pgs_elliptic = 1e-3, + .newton = MjTol(1e-13, 1e-5), + .cg = MjTol(1e-5, 1e-2), + .pgs_pyramidal = MjTol(1e-12, 1e-5), + .pgs_elliptic = MjTol(1e-3, 1e-2), }}, {.path = kHumanoidPath, .tolerances = { - .newton = 1e-15, - .cg = 1e-7, - .pgs_pyramidal = 1e-6, - .pgs_elliptic = 1e-9, + .newton = MjTol(1e-13, 1e-5), + .cg = MjTol(1e-5, 1e-2), + .pgs_pyramidal = MjTol(1e-5, 1e-5), + .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 - double tol_multiplier = 1e2; - double tolerance = scale * rtol * tol_multiplier; + mjtNum tolerance = scale * rtol; 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), - Pointwise(MjNear(tolerance, - max(1e-1, 1000 * tolerance)), - AsVector(data_truth->qfrc_constraint, nv))) + mjtNum max_diff = 0; + mjtNum worst_expected = 0; + mjtNum worst_actual = 0; + 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 + << ")"; } } } diff --git a/test/engine/engine_util_misc_test.cc b/test/engine/engine_util_misc_test.cc index 053a1e09..57f36d79 100644 --- a/test/engine/engine_util_misc_test.cc +++ b/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); - constexpr mjtNum fd_tol = MjTol(1e-7, 1e-3); + const mjtNum dx = 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; diff --git a/test/engine/engine_util_solve_test.cc b/test/engine/engine_util_solve_test.cc index fa0f87d5..029d840d 100644 --- a/test/engine/engine_util_solve_test.cc +++ b/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 - constexpr mjtNum threshold = MjTol(0, -2e-3); // comparison threshold + const mjtNum eps = MjTol(1e-4, 5e-2); // epsilon used for nudging + const mjtNum threshold = MjTol(0, -2e-3); // comparison threshold bool is_minimum = true; mjtNum* res_nudge = (mjtNum*) mju_malloc(sizeof(mjtNum)*n); diff --git a/test/engine/engine_util_spatial_test.cc b/test/engine/engine_util_spatial_test.cc index 5eda98d1..2377e19c 100644 --- a/test/engine/engine_util_spatial_test.cc +++ b/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); diff --git a/test/fixture.h b/test/fixture.h index 3553b37c..8ed78750 100644 --- a/test/fixture.h +++ b/test/fixture.h @@ -16,7 +16,7 @@ #define MUJOCO_TEST_FIXTURE_H_ #include -#include // IWYU pragma: keep +#include // IWYU pragma: keep #include // IWYU pragma: keep #include #include @@ -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 inline auto MjNear(T1 double_tol, T2 float_tol) { #ifdef mjUSESINGLE - return ::testing::FloatNear(static_cast(float_tol)); + return ::testing::FloatNear(static_cast(float_tol) * MjTolScale()); #else - return ::testing::DoubleNear(static_cast(double_tol)); + return ::testing::DoubleNear(static_cast(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 inline auto MjNear(T1 target, T2 double_tol, T3 float_tol) { #ifdef mjUSESINGLE return ::testing::FloatNear(static_cast(target), - static_cast(float_tol)); + static_cast(float_tol) * MjTolScale()); #else return ::testing::DoubleNear(static_cast(target), - static_cast(double_tol)); + static_cast(double_tol) * MjTolScale()); #endif } - // Precision-aware tolerance for EXPECT_NEAR. +// Scaled by MJTOL_SCALE env var (default 1.0). template -constexpr mjtNum MjTol(T1 double_tol, T2 float_tol) { +inline mjtNum MjTol(T1 double_tol, T2 float_tol) { #ifdef mjUSESINGLE - return static_cast(float_tol); + return static_cast(float_tol) * MjTolScale(); #else - return static_cast(double_tol); + return static_cast(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_ diff --git a/test/pipeline_test.cc b/test/pipeline_test.cc index 7852efd3..fbdb39ac 100644 --- a/test/pipeline_test.cc +++ b/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}; diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 51cd792e..3a5f8cff 100644 --- a/test/user/user_objects_test.cc +++ b/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; diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 574da7da..3897282a 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/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;