diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index a6c6f3c3..8e3c31c8 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -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]; } diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index ece381b4..b75fef48 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -4280,10 +4280,16 @@ void mjCFlex::ResolveReferences(const mjCModel* m) { mjCBody* pbody = static_cast(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()); diff --git a/test/engine/engine_passive_test.cc b/test/engine/engine_passive_test.cc index 4655961f..ab7de3be 100644 --- a/test/engine/engine_passive_test.cc +++ b/test/engine/engine_passive_test.cc @@ -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"( + + + + + + + + + + + )"; + + 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 diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index cecbd4ee..e1e7a9fa 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -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"( + + + + + + + + + + + + + )"; + std::array 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"( + + + + + + + + + + + )"; + std::array error; + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), NotNull()) << error.data(); +} + } // namespace } // namespace mujoco