Restrict midpoint integration to unconstrained free bodies in implicitfast
PiperOrigin-RevId: 908750768 Change-Id: I9a45a160ac757cc82bfe54871609956769988369
This commit is contained in:
Yuval Tassa
committed by
Copybara-Service
Copybara-Service
parent
8287d9d152
commit
910b3336ed
@@ -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
|
||||
|
||||
@@ -783,6 +783,140 @@ TEST_F(ImplicitIntegratorTest, MidpointFullNewtonConvergence) {
|
||||
EXPECT_LT((mjtNum)total_iter / ncases, 3.0);
|
||||
}
|
||||
|
||||
// verify midpoint eligibility: compare with/without invdiscrete
|
||||
// if trajectories differ, midpoint was applied
|
||||
// if trajectories match, midpoint was skipped
|
||||
TEST_F(ImplicitIntegratorTest, MidpointEligibility) {
|
||||
// free body with asymmetric inertia, optionally near a plane
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option integrator="implicitfast" timestep="0.01">
|
||||
<flag energy="enable"/>
|
||||
</option>
|
||||
<worldbody>
|
||||
<geom type="plane" size="5 5 0.1"/>
|
||||
<body name="free" pos="0 0 2">
|
||||
<freejoint/>
|
||||
<geom type="ellipsoid" size="0.3 0.2 0.1" mass="1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
char error[1024];
|
||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(m, NotNull()) << error;
|
||||
mjData* d1 = mj_makeData(m);
|
||||
mjData* d2 = mj_makeData(m);
|
||||
int nsteps = 50;
|
||||
|
||||
auto spin_and_compare = [&](const char* label,
|
||||
bool expect_midpoint) {
|
||||
mj_resetData(m, d1);
|
||||
mj_resetData(m, d2);
|
||||
d1->qvel[3] = d2->qvel[3] = 5;
|
||||
d1->qvel[4] = d2->qvel[4] = 3;
|
||||
d1->qvel[5] = d2->qvel[5] = 1;
|
||||
|
||||
// d1: midpoint enabled (default)
|
||||
m->opt.enableflags &= ~mjENBL_INVDISCRETE;
|
||||
for (int i = 0; i < nsteps; i++) mj_step(m, d1);
|
||||
|
||||
// d2: midpoint disabled
|
||||
m->opt.enableflags |= mjENBL_INVDISCRETE;
|
||||
mj_resetData(m, d2);
|
||||
d2->qvel[3] = 5; d2->qvel[4] = 3; d2->qvel[5] = 1;
|
||||
for (int i = 0; i < nsteps; i++) mj_step(m, d2);
|
||||
m->opt.enableflags &= ~mjENBL_INVDISCRETE;
|
||||
|
||||
// compare angular velocities
|
||||
mjtNum diff = 0;
|
||||
for (int k = 3; k < 6; k++) {
|
||||
mjtNum d = d1->qvel[k] - d2->qvel[k];
|
||||
diff += d * d;
|
||||
}
|
||||
if (expect_midpoint) {
|
||||
EXPECT_GT(diff, 1e-6)
|
||||
<< label << ": expected midpoint to be applied";
|
||||
} else {
|
||||
EXPECT_LT(diff, 1e-20)
|
||||
<< label << ": expected midpoint to be skipped";
|
||||
}
|
||||
};
|
||||
|
||||
// case 1: free body in vacuum, implicitfast -> midpoint applied
|
||||
m->opt.integrator = mjINT_IMPLICITFAST;
|
||||
m->opt.density = 0;
|
||||
m->opt.viscosity = 0;
|
||||
spin_and_compare("vacuum+implicitfast", true);
|
||||
|
||||
// case 2: implicit integrator -> midpoint NOT applied
|
||||
m->opt.integrator = mjINT_IMPLICIT;
|
||||
spin_and_compare("vacuum+implicit", false);
|
||||
|
||||
// case 3: fluid (nonzero density) -> midpoint NOT applied
|
||||
m->opt.integrator = mjINT_IMPLICITFAST;
|
||||
m->opt.density = 1.2;
|
||||
spin_and_compare("fluid+implicitfast", false);
|
||||
m->opt.density = 0;
|
||||
|
||||
// case 4: fluid (nonzero viscosity) -> midpoint NOT applied
|
||||
m->opt.viscosity = 0.001;
|
||||
spin_and_compare("viscosity+implicitfast", false);
|
||||
m->opt.viscosity = 0;
|
||||
|
||||
// case 5: body with active contacts -> midpoint NOT applied
|
||||
// test both island-enabled and island-disabled branches
|
||||
for (int disable_island = 0; disable_island < 2; disable_island++) {
|
||||
m->opt.integrator = mjINT_IMPLICITFAST;
|
||||
if (disable_island) {
|
||||
m->opt.disableflags |= mjDSBL_ISLAND;
|
||||
} else {
|
||||
m->opt.disableflags &= ~mjDSBL_ISLAND;
|
||||
}
|
||||
|
||||
mj_resetData(m, d1);
|
||||
mj_resetData(m, d2);
|
||||
d1->qpos[2] = d2->qpos[2] = 0.05;
|
||||
d1->qvel[3] = d2->qvel[3] = 5;
|
||||
d1->qvel[4] = d2->qvel[4] = 3;
|
||||
d1->qvel[5] = d2->qvel[5] = 1;
|
||||
|
||||
// verify contacts are active
|
||||
mj_forward(m, d1);
|
||||
ASSERT_GT(d1->ncon, 0) << "body should be in contact with the plane";
|
||||
|
||||
// single step with midpoint enabled
|
||||
mj_resetData(m, d1);
|
||||
d1->qpos[2] = 0.05;
|
||||
d1->qvel[3] = 5; d1->qvel[4] = 3; d1->qvel[5] = 1;
|
||||
m->opt.enableflags &= ~mjENBL_INVDISCRETE;
|
||||
mj_step(m, d1);
|
||||
|
||||
// single step with midpoint disabled
|
||||
mj_resetData(m, d2);
|
||||
d2->qpos[2] = 0.05;
|
||||
d2->qvel[3] = 5; d2->qvel[4] = 3; d2->qvel[5] = 1;
|
||||
m->opt.enableflags |= mjENBL_INVDISCRETE;
|
||||
mj_step(m, d2);
|
||||
m->opt.enableflags &= ~mjENBL_INVDISCRETE;
|
||||
|
||||
mjtNum diff = 0;
|
||||
for (int k = 0; k < m->nv; k++) {
|
||||
mjtNum d = d1->qvel[k] - d2->qvel[k];
|
||||
diff += d * d;
|
||||
}
|
||||
EXPECT_LT(diff, 1e-20)
|
||||
<< "contact (island " << (disable_island ? "disabled" : "enabled")
|
||||
<< "): expected midpoint to be skipped";
|
||||
}
|
||||
m->opt.disableflags &= ~mjDSBL_ISLAND;
|
||||
|
||||
mj_deleteData(d2);
|
||||
mj_deleteData(d1);
|
||||
mj_deleteModel(m);
|
||||
}
|
||||
|
||||
TEST_F(ForwardTest, ControlClamping) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
|
||||
Reference in New Issue
Block a user