Fix trilinear flexcomp with unused vertices.

Fix a crash and dimension mismatch issue in user_flexcomp when dealing with interpolated="true" meshes that contain unused vertices. Previously, vertbody would correspond only to used vertices while vert included all vertices, leading to assertions or out-of-bounds access. Also enable node pinning for interpolated meshes.

PiperOrigin-RevId: 865881995
Change-Id: Id150f00f22b5f48896ffbffddf03c57b252de8c0

src/user/user_flexcomp.cc

@@ -204,8 +204,9 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
   if (pingridrange.size()%(2*dflex->dim)) {
     return comperr(error, "Pin grid range number of must be multiple of 2*dim", error_sz);
   }
-  if (type != mjFCOMPTYPE_GRID && !(pingrid.empty() && pingridrange.empty())) {
-    return comperr(error, "Pin grid(range) can only be used with grid type", error_sz);
+  if (type != mjFCOMPTYPE_GRID && !(pingrid.empty() && pingridrange.empty()) &&
+      doftype != mjFCOMPDOF_TRILINEAR && doftype != mjFCOMPDOF_QUADRATIC) {
+    return comperr(error, "Pin grid(range) can only be used with grid or interpolated", error_sz);
   }
   if (dflex->dim == 1 && !(pingrid.empty() && pingridrange.empty())) {
     return comperr(error, "Pin grid(range) cannot be used with dim=1", error_sz);
@@ -267,7 +268,13 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
   }
 
   // construct pinned array
-  pinned = vector<bool>(npnt, rigid);
+  int nnode = 0;
+  if (doftype == mjFCOMPDOF_TRILINEAR) {
+    nnode = 8;
+  } else if (doftype == mjFCOMPDOF_QUADRATIC) {
+    nnode = 27;
+  }
+  pinned = vector<bool>(std::max(npnt, nnode), rigid);
 
   // handle pins if user did not specify rigid
   if (!rigid) {
@@ -299,8 +306,14 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
     // process pingrid
     for (int i=0; i < (int)pingrid.size(); i+=dflex->dim) {
       // check range
+      int count_check[3] = {count[0], count[1], count[2]};
+      if (type != mjFCOMPTYPE_GRID && (doftype == mjFCOMPDOF_TRILINEAR ||
+                                       doftype == mjFCOMPDOF_QUADRATIC)) {
+        int dim = (doftype == mjFCOMPDOF_TRILINEAR) ? 2 : 3;
+        count_check[0] = count_check[1] = count_check[2] = dim;
+      }
       for (int k=0; k < dflex->dim; k++) {
-        if (pingrid[i+k] < 0 || pingrid[i+k] >= count[k]) {
+        if (pingrid[i+k] < 0 || pingrid[i+k] >= count_check[k]) {
           return comperr(error, "pingrid out of range", error_sz);
         }
       }
@@ -392,6 +405,35 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
       for (int i=0; i < (int)element.size(); i++) {
         element[i] += reindex[element[i]];
       }
+
+      // compact point, texcoord, pinned arrays
+      int new_npnt = 0;
+      for (int i=0; i < npnt; i++) {
+        if (used[i]) {
+          point[3*new_npnt+0] = point[3*i+0];
+          point[3*new_npnt+1] = point[3*i+1];
+          point[3*new_npnt+2] = point[3*i+2];
+
+          if (!texcoord.empty()) {
+            texcoord[2*new_npnt+0] = texcoord[2*i+0];
+            texcoord[2*new_npnt+1] = texcoord[2*i+1];
+          }
+
+          pinned[new_npnt] = pinned[i];
+          new_npnt++;
+        }
+      }
+
+      // resize arrays
+      point.resize(3*new_npnt);
+      if (!texcoord.empty()) {
+        texcoord.resize(2*new_npnt);
+      }
+      pinned.resize(std::max(new_npnt, nnode));
+      used.assign(new_npnt, true);
+
+      // update count
+      npnt = new_npnt;
     }
   }
 

src/user/user_mesh.cc

@@ -4137,6 +4137,8 @@ void mjCFlex::Compile(const mjVFS* vfs) {
     nvert = (int)vert_.size()/3;
     if (vertbody_.size() == 1) {
       rigid = true;
+    } else if (vertbody_.size() != nvert) {
+      throw mjCError(this, "vertbody size must be 1 or nvert");
     }
   }
   if (nvert < dim+1) {

test/user/user_flex_test.cc

@@ -786,5 +786,55 @@ TEST_F(UserFlexTest, LoadMSHASCII_dim_missing_in_xml) {
   mj_deleteModel(m);
 }
 
+TEST_F(UserFlexTest, TrilinearUnusedVertices_Crash) {
+  // This XML defines 5 points but only uses 4 in the element.
+  // The last point (2 2 2) is unused.
+  static constexpr char xml[] = R"(
+  <mujoco>
+  <worldbody>
+    <flexcomp name="test" type="direct"
+              point="0 0 0  1 0 0  0 1 0  0 0 1  2 2 2"
+              element="0 1 2 3"
+              dof="trilinear" dim="3">
+      <contact selfcollide="none"/>
+    </flexcomp>
+  </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* m = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(m, testing::NotNull()) << error.data();
+  mj_deleteModel(m);
+}
+
+TEST_F(UserFlexTest, MeshNodePinning) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+  <worldbody>
+    <flexcomp name="test" type="direct"
+              point="0 0 0  1 0 0  0 1 0  0 0 1  2 2 2"
+              element="0 1 2 3"
+              dof="trilinear" dim="3">
+      <contact selfcollide="none"/>
+      <pin id="0"/>
+    </flexcomp>
+  </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* m = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(m, testing::NotNull()) << error.data();
+
+  // Verify that node 0 (corner) is pinned
+  // Trilinear 3D has 8 nodes.
+  // If 0 are pinned, we get 8 bodies.
+  // If 1 is pinned, we get 7 bodies (created) + 1 existing (the parent).
+  // m->nbody should reflect this. Flex bodies are added to the model.
+  // Model has 1 world body + flex bodies.
+  EXPECT_EQ(m->nbody, 1 + 7);  // 1 world + 7 flex nodes (1 pinned)
+
+  mj_deleteModel(m);
+}
+
 }  // namespace
 }  // namespace mujoco