chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Improve test comparing forward and inverse dynamics mismatch

Browse Source 
PiperOrigin-RevId: 916864072
Change-Id: I9952c7dd1bde2033dfe1380552f2c4b6b7344b26
This commit is contained in:
Yuval Tassa
2026-05-17 12:02:52 -07:00
committed by Copybara-Service
parent 04042d8bf3
commit a897805683
1 changed files with 29 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_inverse_test.cc
+21 -4
View File
@@ -41,6 +41,17 @@ TEST_F(InverseTest, ForwardInverseMatch) {
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model); mjData* data = mj_makeData(model);
// set small tolerance and enough iterations for all solvers to converge
model->opt.iterations = 500;
model->opt.tolerance = 0;
// solver names for diagnostics
const char* solver_name[] = {"PGS", "CG", "Newton"};
for (mjtSolver solver : {mjSOL_PGS, mjSOL_CG, mjSOL_NEWTON}) {
model->opt.solver = solver;
mj_resetData(model, data);
// simulate, call mj_forward // simulate, call mj_forward
for (int i = 0; i < kSteps; ++i) { for (int i = 0; i < kSteps; ++i) {
mj_step(model, data); mj_step(model, data);
@@ -50,10 +61,16 @@ TEST_F(InverseTest, ForwardInverseMatch) {
// call built-in testing function // call built-in testing function
mj_compareFwdInv(model, data); mj_compareFwdInv(model, data);
// expect mismatch to be small // per-solver tolerances
mjtNum epsilon = MjTol(1e-10, 0.05); mjtNum epsilon;
EXPECT_LT(data->solver_fwdinv[0], epsilon); switch (solver) {
EXPECT_LT(data->solver_fwdinv[1], epsilon); case mjSOL_PGS: epsilon = MjTol(1e-6, 1e-2); break;
case mjSOL_CG: epsilon = MjTol(1e-3, 1e-1); break;
case mjSOL_NEWTON: epsilon = MjTol(1e-10, 5e-3); break;
}
EXPECT_LT(data->solver_fwdinv[0], epsilon) << solver_name[solver];
EXPECT_LT(data->solver_fwdinv[1], epsilon) << solver_name[solver];
}
mj_deleteData(data); mj_deleteData(data);
mj_deleteModel(model); mj_deleteModel(model);