Add a test for equivalence of different solvers.

PiperOrigin-RevId: 845777469 Change-Id: I76a162b71bea349cca3472001360181654a5d571
2025-12-17 08:20:43 -08:00
parent e95f46f84a
commit 47e35e632f
4 changed files with 627 additions and 4 deletions
@@ -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