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

Merge pull request #3466 from smallquail:flex-implicit-contact

Browse Source 
PiperOrigin-RevId: 960804180
Change-Id: Ic3eb203ca0d9cf19c9043b46127db342678d94f9
This commit is contained in:
Copybara-Service
2026-08-07 02:36:26 -07:00
parent dca295dde2 2a3554c8a3
commit e144090273
12 changed files with 425 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_derivative_test.cc
+4 -4
View File
@@ -2153,13 +2153,13 @@ TEST_F(DerivativeTest, FlexStiffAssemble) {
std::vector<int> rownnz(nv), rowadr(nv);
int nnz = mjd_flexStiff_assemble(model.get(), data.get(), rownnz.data(),
rowadr.data(), NULL, NULL, s1, s2,
/*flg_bend=*/1, /*flg_stretch=*/1, NULL);
/*flg_bend=*/1, /*flg_stretch=*/1, /*flg_contact=*/0, NULL);
ASSERT_GT(nnz, 0);
std::vector<int> colind(nnz);
std::vector<mjtNum> val(nnz);
mjd_flexStiff_assemble(model.get(), data.get(), rownnz.data(), rowadr.data(),
colind.data(), val.data(), s1, s2, /*flg_bend=*/1,
/*flg_stretch=*/1, NULL);
/*flg_stretch=*/1, /*flg_contact=*/0, NULL);
// compare CSR apply vs operators on test vectors
for (int trial = 0; trial < 3; trial++) {
@@ -2220,13 +2220,13 @@ TEST_F(DerivativeTest, FlexStiffAssembleInterp) {
mjtNum s1 = 4e-6, s2 = 2e-3;
std::vector<int> rownnz(nv), rowadr(nv);
int nnz = mjd_flexStiff_assemble(model.get(), data.get(), rownnz.data(), rowadr.data(),
NULL, NULL, s1, s2, /*flg_bend=*/0, /*flg_stretch=*/0,
NULL, NULL, s1, s2, /*flg_bend=*/0, /*flg_stretch=*/0, /*flg_contact=*/0,
krot.data());
ASSERT_GT(nnz, 0);
std::vector<int> colind(nnz);
std::vector<mjtNum> val(nnz);
mjd_flexStiff_assemble(model.get(), data.get(), rownnz.data(), rowadr.data(),
colind.data(), val.data(), s1, s2, /*flg_bend=*/0, /*flg_stretch=*/0,
colind.data(), val.data(), s1, s2, /*flg_bend=*/0, /*flg_stretch=*/0, /*flg_contact=*/0,
krot.data());
// compare CSR apply vs the operator called with negated scales (its convention)
test/engine/engine_forward_test.cc
+53
View File
@@ -3630,6 +3630,59 @@ TEST_F(ActuatorDampingTest, DampingVsKvGearScaling) {
}
// flex sheet dropping on a plane should not gain energy from implicit bending
// Passive flex contact stiffness is far beyond the explicit limit (~50x) because its curvature is
// carried by the metric. Both curvature and shift are needed; without the shift it rings apart.
TEST_F(ImplicitIntegratorTest, PassiveFlexContactIsImplicit) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.002" integrator="implicitfast" solver="CG" iterations="400"/>
<worldbody>
<flexcomp name="lower" type="grid" dim="2" count="9 9 1" spacing=".04 .04 1"
radius=".004" mass=".3" pos="0 0 .2">
<contact selfcollide="auto" passive="true"/>
<elasticity young="1e5" poisson=".2" thickness="2e-3" elastic2d="both" damping="1e-4"/>
<pin id="0 8 72 80"/>
</flexcomp>
<flexcomp name="upper" type="grid" dim="2" count="5 5 1" spacing=".04 .04 1"
radius=".004" mass=".1" pos="0 0 .27">
<contact selfcollide="auto" passive="true"/>
<elasticity young="1e5" poisson=".2" thickness="2e-3" elastic2d="both" damping="1e-4"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
MjDataPtr d = MakeData(m);
const mjModel* model = m.get();
mjData* data = d.get();
// Physical peak speed is ~2 m/s; without the shift this scene reaches 143 m/s.
mjtNum vmax = 0;
for (int i = 0; i < 1000; i++) {
mj_step(model, data);
for (int j = 0; j < model->nv; j++) {
vmax = mju_max(vmax, mju_abs(data->qvel[j]));
}
ASSERT_FALSE(data->warning[mjWARN_BADQACC].number) << "diverged at step " << i;
}
EXPECT_LT(vmax, 4.0) << "peak speed " << vmax;
// Upper sheet must not pass through the lower one: check that its lowest vertex stays above
// the lower sheet's lowest point.
mjtNum lo[2] = {1e30, 1e30};
for (int k = 0; k < 2; k++) {
int f = mj_name2id(model, mjOBJ_FLEX, k ? "upper" : "lower");
for (int i = 0; i < model->flex_vertnum[f]; i++) {
lo[k] = mju_min(lo[k], data->flexvert_xpos[3*(model->flex_vertadr[f] + i) + 2]);
}
}
EXPECT_GT(lo[1], lo[0] - 0.01) << "upper sheet passed through: lowest z " << lo[1]
<< " against the lower sheet's " << lo[0];
}
TEST_F(ImplicitIntegratorTest, FlexContactEnergy) {
static constexpr char xml[] = R"(
<mujoco>