Support pinned flex vertices with bending

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

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
+    // a pinned vertex is welded to a static (jointless) parent body: its bending reaction is
-    mjtNum* vel[4];
+    // 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]];
+      if (!isfree[i]) continue;
-      int body_dofnum = m->body_dofnum[bid];
+      int body_dofadr = m->body_dofadr[bodyid[v[i]]];
-      int body_dofadr = m->body_dofadr[bid];
+      for (int x = 0; x < 3; x++) {
-      for (int x = 0; x < body_dofnum; 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

@@ -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(
-        throw mjCError(this, "pins are not supported for bending");
+            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

@@ -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

@@ -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