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_derivative_test.cc
+78 -2
View File
@@ -320,17 +320,19 @@ TEST_F(DerivativeTest, PassiveDvel) {
mj_forward(model, data);
// get analytic derivatives
mju_zero(data->qDeriv, model->nD);
mjd_passive_vel(model, data);
mju_copy(qDerivAnalytic, data->qDeriv, nD);
// clear qDeriv, get finite-difference derivatives
mju_zero(data->qDeriv, nD);
mju_zero(qDerivFD, nD);
mjtNum eps = MjTol(1e-6, 1e-3);
mjtNum eps = MjTol(1e-6, 1e-4);
mjd_passive_velFD(model, data, eps);
// expect FD and analytic derivatives to be similar to tol precision
EXPECT_THAT(AsVector(data->qDeriv, nD),
Pointwise(MjNear(1e-4, 1e-3), AsVector(qDerivAnalytic, nD)));
Pointwise(MjNear(1e-6, 1e-4), AsVector(qDerivAnalytic, nD)));
}
mju_free(qDerivFD);
@@ -1733,5 +1735,79 @@ 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