From 47e35e632fd262a38d76ab9fe47e6d50246148a6 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 17 Dec 2025 08:20:43 -0800 Subject: [PATCH] Add a test for equivalence of different solvers. PiperOrigin-RevId: 845777469 Change-Id: I76a162b71bea349cca3472001360181654a5d571 --- test/engine/engine_solver_test.cc | 120 +++++++++++- test/engine/testdata/solver/humanoid.xml | 217 +++++++++++++++++++++ test/engine/testdata/solver/model.xml | 186 ++++++++++++++++++ test/engine/testdata/solver/perf/mixed.xml | 108 ++++++++++ 4 files changed, 627 insertions(+), 4 deletions(-) create mode 100644 test/engine/testdata/solver/humanoid.xml create mode 100644 test/engine/testdata/solver/model.xml create mode 100644 test/engine/testdata/solver/perf/mixed.xml diff --git a/test/engine/engine_solver_test.cc b/test/engine/engine_solver_test.cc index 04280c0b..41bc4ff0 100644 --- a/test/engine/engine_solver_test.cc +++ b/test/engine/engine_solver_test.cc @@ -33,8 +33,8 @@ using ::std::max; using SolverTest = MujocoTest; -static const char* const kModelPath = - "testdata/model.xml"; +static const char* const kModelPath = "engine/testdata/solver/model.xml"; +static const char* const kHumanoidPath = "engine/testdata/solver/humanoid.xml"; // compare accelerations produced by CG solver with and without islands TEST_F(SolverTest, IslandsEquivalent) { @@ -121,8 +121,9 @@ TEST_F(SolverTest, IslandsEquivalentForward) { mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; int nv = model->nv; - model->opt.tolerance = 0; // set tolerance to 0 - model->opt.ls_tolerance = 0; // set ls_tolerance to 0 + + // set tolerance to 0 so opt.iterations are always run + model->opt.tolerance = 0; mjData* data_island = mj_makeData(model); mjData* data_noisland = mj_makeData(model); @@ -174,5 +175,116 @@ TEST_F(SolverTest, IslandsEquivalentForward) { mj_deleteModel(model); } +TEST_F(SolverTest, SolversEquivalent) { + struct SolverTolerances { + double newton; + double cg; + double pgs_pyramidal; + double 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 + const struct { + const char* path; + SolverTolerances tolerances; + } kConfigs[] = { + {.path = kModelPath, + .tolerances = + { + .newton = 1e-15, + .cg = 1e-7, + .pgs_pyramidal = 1e-14, + .pgs_elliptic = 1e-3, + }}, + {.path = kHumanoidPath, + .tolerances = + { + .newton = 1e-15, + .cg = 1e-7, + .pgs_pyramidal = 1e-6, + .pgs_elliptic = 1e-9, + }}, + }; + + for (const auto& config : kConfigs) { + const std::string xml_path = GetTestDataFilePath(config.path); + char error[1024]; + mjModel* model = + mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + model->opt.tolerance = 0; // set tolerance to 0 + model->opt.iterations = 500; // set iterations to 500 + model->opt.disableflags |= mjDSBL_WARMSTART; // disable warmstart + int nv = model->nv; + + mjData* data = mj_makeData(model); + mjData* data_truth = mj_makeData(model); + + for (mjtCone cone : {mjCONE_PYRAMIDAL, mjCONE_ELLIPTIC}) { + model->opt.cone = cone; + + // use Newton Dense as ground truth + model->opt.solver = mjSOL_NEWTON; + model->opt.jacobian = mjJAC_DENSE; + mj_resetDataKeyframe(model, data_truth, 0); + mj_forward(model, data_truth); + + mjtNum scale = mju_norm(data_truth->qfrc_constraint, nv); + + for (mjtSolver solver : {mjSOL_NEWTON, mjSOL_CG, mjSOL_PGS}) { + double rtol; + switch (solver) { + case mjSOL_NEWTON: + rtol = config.tolerances.newton; + break; + case mjSOL_CG: + rtol = config.tolerances.cg; + break; + case mjSOL_PGS: + rtol = cone == mjCONE_PYRAMIDAL ? config.tolerances.pgs_pyramidal + : config.tolerances.pgs_elliptic; + break; + } + + // increase base tolerance to avoid test flakiness + double tol_multiplier = 1e2; + double tolerance = scale * rtol * tol_multiplier; + + for (mjtJacobian jacobian : {mjJAC_DENSE, mjJAC_SPARSE}) { + model->opt.solver = solver; + model->opt.jacobian = jacobian; + + mj_resetDataKeyframe(model, data, 0); + mj_forward(model, data); + + const char* cone_str = + (cone == mjCONE_PYRAMIDAL ? "pyramidal" : "elliptic"); + const char* solver_str = + (solver == mjSOL_NEWTON ? "Newton" + : (solver == mjSOL_CG ? "CG" : "PGS")); + const char* jacobian_str = + (jacobian == mjJAC_DENSE ? "dense" : "sparse"); + + EXPECT_THAT(AsVector(data->qfrc_constraint, nv), + Pointwise(DoubleNear(tolerance), + AsVector(data_truth->qfrc_constraint, nv))) + << "model: " << config.path << "\n" + << "cone: " << cone_str << "\n" + << "solver: " << solver_str << "\n" + << "jacobian: " << jacobian_str << "\n" + << "tolerance: " << tolerance; + } + } + } + + mj_deleteData(data_truth); + mj_deleteData(data); + mj_deleteModel(model); + } +} + } // namespace } // namespace mujoco diff --git a/test/engine/testdata/solver/humanoid.xml b/test/engine/testdata/solver/humanoid.xml new file mode 100644 index 00000000..6cb3b61e --- /dev/null +++ b/test/engine/testdata/solver/humanoid.xml @@ -0,0 +1,217 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/solver/model.xml b/test/engine/testdata/solver/model.xml new file mode 100644 index 00000000..82bc2906 --- /dev/null +++ b/test/engine/testdata/solver/model.xml @@ -0,0 +1,186 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/solver/perf/mixed.xml b/test/engine/testdata/solver/perf/mixed.xml new file mode 100644 index 00000000..8940d190 --- /dev/null +++ b/test/engine/testdata/solver/perf/mixed.xml @@ -0,0 +1,108 @@ + + + + + + + + + + + + + + + + + + + + + +