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

Support pinned flex vertices with bending

Browse Source 
The flexcomp compiler previously rejected pins on dim-2 flexes with
bending (elastic2d bend/both). Allow them: mj_flexPassiveBend treats a
pinned vertex (body without 3 free slide dofs) as static -- zero
velocity, and no bending force applied to it (the reaction is carried
by the pin) -- while its position still enters every neighbor's
bending force, which is exactly what the pin constrains.

PiperOrigin-RevId: 944410969
Change-Id: Ib0a69c8d5fb6f64d3e2a76af3b6b2c7be1898191
This commit is contained in:
Alessio Quaglino
2026-07-08 04:01:51 -07:00
committed by Copybara-Service
parent c4c2cad503
commit fb6d1cf18f
4 changed files with 107 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_passive.c
+14 -8
View File
@@ -481,10 +481,14 @@ static void mj_flexPassiveBend(const mjModel* m, mjData* d, int f,
frc[0][1] = -(frc[1][1] + frc[2][1] + frc[3][1]);
frc[0][2] = -(frc[1][2] + frc[2][2] + frc[3][2]);
// velocities
mjtNum* vel[4];
// a pinned vertex is welded to a static (jointless) parent body: its bending reaction is
// absorbed by the pin, so its velocity is zero and (below) no force is applied to it.
static const mjtNum zero3[3] = {0, 0, 0};
const mjtNum* vel[4]; int isfree[4];
for (int i = 0; i < 4; i++) {
vel[i] = d->qvel + m->body_dofadr[bodyid[v[i]]];
int bid = bodyid[v[i]];
isfree[i] = (m->body_dofnum[bid] == 3);
vel[i] = isfree[i] ? (d->qvel + m->body_dofadr[bid]) : zero3;
}
// force
@@ -506,12 +510,14 @@ static void mj_flexPassiveBend(const mjModel* m, mjData* d, int f,
}
}
// insert into global force
// insert into global force (free flex vertices only: 3 translational dofs, no moment arm).
// A pinned vertex has no free flex dof -- its bending reaction is carried by the pin -- so it
// is skipped (its POSITION still enters every neighbor's force via the xpos sum above,
// which is what the pin constrains).
for (int i = 0; i < 4; i++) {
int bid = bodyid[v[i]];
int body_dofnum = m->body_dofnum[bid];
int body_dofadr = m->body_dofadr[bid];
for (int x = 0; x < body_dofnum; x++) {
if (!isfree[i]) continue;
int body_dofadr = m->body_dofadr[bodyid[v[i]]];
for (int x = 0; x < 3; x++) {
if (enbl_spring) d->qfrc_spring[body_dofadr+x] -= spring[3*i+x];
if (enbl_damper) d->qfrc_damper[body_dofadr+x] -= damper[3*i+x] * m->flex_damping[f];
}
src/user/user_mesh.cc
+9 -3
View File
@@ -4280,10 +4280,16 @@ void mjCFlex::ResolveReferences(const mjCModel* m) {
mjCBody* pbody = static_cast<mjCBody*>(m->FindObject(mjOBJ_BODY, vertbody));
if (pbody) {
vertbodyid.push_back(pbody->id);
if (pbody->joints.size() != 3 && dim == 2 &&
// pinned vertices with bending are only valid for static (jointless) pin
// bodies: the runtime treats pin velocity as zero, which is only correct
// for static bodies.
if (!pbody->joints.empty() && pbody->joints.size() != 3 && dim == 2 &&
(elastic2d == 1 || elastic2d == 3) && !interpolated) {
// TODO(quaglino): add support for pins
throw mjCError(this, "pins are not supported for bending");
throw mjCError(
this,
"pinned flex vertices with bending require a static (jointless) "
"pin body, body '%s' has joints",
vertbody.c_str());
}
} else {
throw mjCError(this, "unknown body '%s' in flex", vertbody.c_str());
test/engine/engine_passive_test.cc
+41
View File
@@ -965,5 +965,46 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
}
}
// verify that a pinned vertex (on a static body) gets zero bending force
// while its free neighbors get nonzero bending force
TEST_F(ElasticityTest, PinnedVertexBendingForce) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="parent">
<flexcomp name="test" type="grid" count="3 3 1" spacing="1 1 1"
radius="0.01" dim="2">
<elasticity young="1" poisson="0" thickness="1" elastic2d="bend"/>
<pin id="0"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), testing::NotNull()) << error;
MjDataPtr d = MakeData(m);
// displace a free vertex out of plane to create bending
d->qpos[3] = 0.1;
mj_forward(m.get(), d.get());
// vertex 0 is pinned to the world body (body 0), which has 0 dofs,
// so no bending force is written for it — the reaction is absorbed by the pin
// verify that at least some free vertices have nonzero spring force
bool has_nonzero = false;
for (int i = 0; i < m->nv; i++) {
if (d->qfrc_spring[i] != 0) {
has_nonzero = true;
break;
}
}
EXPECT_TRUE(has_nonzero) << "bending should produce nonzero spring forces";
}
} // namespace
} // namespace mujoco
test/user/user_flex_test.cc
+43
View File
@@ -1366,5 +1366,48 @@ TEST_F(UserFlexTest, Load1DFlexFromOBJ) {
mj_deleteModel(m);
}
TEST_F(UserFlexTest, PinBendingRejectsNonStaticBody) {
// pinned vertex inherits the flexcomp's parent body, which here has a joint
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="moving">
<joint type="hinge"/>
<geom size="0.1"/>
<flexcomp name="test" type="grid" count="3 3 1" spacing="1 1 1"
radius="0.01" dim="2">
<elasticity young="1" poisson="0" thickness="1" elastic2d="bend"/>
<pin id="0"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m.get(), IsNull());
EXPECT_THAT(error.data(), HasSubstr("static"));
}
TEST_F(UserFlexTest, PinBendingAcceptsStaticBody) {
// pinned vertex inherits the flexcomp's parent body (world), which is static
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="parent">
<flexcomp name="test" type="grid" count="3 3 1" spacing="1 1 1"
radius="0.01" dim="2">
<elasticity young="1" poisson="0" thickness="1" elastic2d="bend"/>
<pin id="0"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m.get(), NotNull()) << error.data();
}
} // namespace
} // namespace mujoco