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

Implement midpoint integrator for free bodies.

Browse Source 
PiperOrigin-RevId: 899043541
Change-Id: I0bb38f6ad94e189b45ab16777a04ad6fefc6adf7
This commit is contained in:
Yuval Tassa
2026-04-13 09:37:51 -07:00
committed by Copybara-Service
parent d9b5d8babb
commit 0c337799bd
12 changed files with 913 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_inverse_test.cc
+8 -9
View File
@@ -73,9 +73,16 @@ TEST_F(InverseTest, DiscreteInverseMatch) {
mjtNum* qvel_next = (mjtNum*)mju_malloc(nv * sizeof(mjtNum));
mjtNum* qacc_fd = (mjtNum*)mju_malloc(nv * sizeof(mjtNum));
for (auto integrator : {mjINT_EULER, mjINT_IMPLICIT, mjINT_IMPLICITFAST}) {
for (auto integrator : {mjINT_EULER, mjINT_IMPLICIT}) {
model->opt.integrator = integrator;
for (bool invdiscrete : {false, true}) {
// set/unset mjENBL_INVDISCRETE flag (affects both forward and inverse)
if (invdiscrete) {
model->opt.enableflags |= mjENBL_INVDISCRETE;
} else {
model->opt.enableflags &= ~mjENBL_INVDISCRETE;
}
// simulate
mj_resetData(model, data);
for (int i = 0; i < kSteps; ++i) {
@@ -98,17 +105,9 @@ TEST_F(InverseTest, DiscreteInverseMatch) {
mj_forward(model, data);
mju_copy(data->qacc, qacc_fd, nv);
// set/unset mjENBL_INVDISCRETE flag
if (invdiscrete) {
model->opt.enableflags |= mjENBL_INVDISCRETE;
} else {
model->opt.enableflags &= ~mjENBL_INVDISCRETE;
}
// call built-in testing function
mj_compareFwdInv(model, data);
// depending on mjENBL_INVDISCRETE flag, expect mismatch to be small/large
if (invdiscrete) {
mjtNum epsilon = MjTol(1e-9, 0.05);
EXPECT_LT(data->solver_fwdinv[0], epsilon);