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

Restrict midpoint integration to unconstrained free bodies in implicitfast

Browse Source 
PiperOrigin-RevId: 908750768
Change-Id: I9a45a160ac757cc82bfe54871609956769988369
This commit is contained in:
Yuval Tassa
2026-05-01 08:40:53 -07:00
committed by Copybara-Service
parent 8287d9d152
commit 910b3336ed
5 changed files with 221 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_derivative_test.cc
-74
View File
@@ -1745,79 +1745,5 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) {
mj_deleteModel(model);
}
TEST_F(DerivativeTest, MidpointFluidAccuracy) {
const std::string xml_path =
GetTestDataFilePath(kTumblingThinObjectEllipsoidPath);
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjtNum dt_small = 1e-4;
mjtNum dt_large = m->opt.timestep; // 2e-3, the default
mjtNum duration = 0.5;
mjData* d_ref = mj_makeData(m);
mjData* d_midpoint = mj_makeData(m);
mjData* d_nomidpoint = mj_makeData(m);
// give initial angular velocity for tumbling
mj_resetData(m, d_ref);
mj_resetData(m, d_midpoint);
mj_resetData(m, d_nomidpoint);
d_ref->qvel[3] = 5;
d_ref->qvel[4] = 3;
d_ref->qvel[5] = 1;
d_midpoint->qvel[3] = 5;
d_midpoint->qvel[4] = 3;
d_midpoint->qvel[5] = 1;
d_nomidpoint->qvel[3] = 5;
d_nomidpoint->qvel[4] = 3;
d_nomidpoint->qvel[5] = 1;
int nsteps_large = static_cast<int>(duration / dt_large);
int substeps = static_cast<int>(dt_large / dt_small);
mjtNum error_midpoint = 0;
mjtNum error_nomidpoint = 0;
for (int i = 0; i < nsteps_large; i++) {
// reference: RK4 at small timestep
m->opt.integrator = mjINT_RK4;
m->opt.timestep = dt_small;
m->opt.enableflags &= ~mjENBL_INVDISCRETE;
for (int j = 0; j < substeps; j++) {
mj_step(m, d_ref);
}
// implicit with midpoint (default)
m->opt.integrator = mjINT_IMPLICIT;
m->opt.timestep = dt_large;
m->opt.enableflags &= ~mjENBL_INVDISCRETE;
mj_step(m, d_midpoint);
// implicit without midpoint
m->opt.enableflags |= mjENBL_INVDISCRETE;
mj_step(m, d_nomidpoint);
// accumulate position errors
for (int k = 0; k < 7; k++) {
mjtNum diff_mid = d_ref->qpos[k] - d_midpoint->qpos[k];
mjtNum diff_nomid = d_ref->qpos[k] - d_nomidpoint->qpos[k];
error_midpoint += diff_mid * diff_mid;
error_nomidpoint += diff_nomid * diff_nomid;
}
}
// expect midpoint to be more accurate
EXPECT_LT(error_midpoint, error_nomidpoint)
<< "implicit midpoint should be more accurate than implicit without "
<< "midpoint for a free body with fluid forces";
mj_deleteData(d_nomidpoint);
mj_deleteData(d_midpoint);
mj_deleteData(d_ref);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco