Remove plugins that are not referenced after detaching a body.

Fixes #2497.

Also, use `Release()` instead of `delete` for removing elements when detaching a body in order to preserve correct reference count.

PiperOrigin-RevId: 739133888
Change-Id: I7ccfad84446fb15259bad30c4ba1e6f7fa601518

src/user/user_model.cc

@@ -116,6 +116,49 @@ bool IsNullPose(const T pos[3], const T quat[4]) {
 }
 
 
+// set ids, check for repeated names
+template <class T>
+static void processlist(mjListKeyMap& ids, vector<T*>& list,
+                        mjtObj type, bool checkrepeat = true) {
+  // assign ids for regular elements
+  if (type < mjNOBJECT) {
+    for (size_t i=0; i < list.size(); i++) {
+      // check for incompatible id setting; SHOULD NOT OCCUR
+      if (list[i]->id != -1 && list[i]->id != i) {
+        throw mjCError(list[i], "incompatible id in %s array, position %d", mju_type2Str(type), i);
+      }
+
+      // id equals position in array
+      list[i]->id = i;
+
+      // add to ids map
+      ids[type][list[i]->name] = i;
+    }
+  }
+
+  // check for repeated names
+  if (checkrepeat) {
+    // created vectors with all names
+    vector<string> allnames;
+    for (size_t i=0; i < list.size(); i++) {
+      if (!list[i]->name.empty()) {
+        allnames.push_back(list[i]->name);
+      }
+    }
+
+    // sort and check for duplicates
+    if (allnames.size() > 1) {
+      std::sort(allnames.begin(), allnames.end());
+      auto adjacent = std::adjacent_find(allnames.begin(), allnames.end());
+      if (adjacent != allnames.end()) {
+        string msg = "repeated name '" + *adjacent + "' in " + mju_type2Str(type);
+        throw mjCError(nullptr, "%s", msg.c_str());
+      }
+    }
+  }
+}
+
+
 }  // namespace
 
 //---------------------------------- CONSTRUCTOR AND DESTRUCTOR ------------------------------------
@@ -490,7 +533,7 @@ void mjCModel::RemoveFromList(std::vector<T*>& list, const mjCModel& other) {
       element->ResolveReferences(this);
     } catch (mjCError err) {
       ids[element->elemtype].erase(element->name);
-      delete element;
+      element->Release();
       list.erase(list.begin() + i);
       nlist--;
       i--;
@@ -515,6 +558,52 @@ void mjCModel::DeleteAll<mjCKey>(std::vector<mjCKey*>& elements) {
 
 
 
+template <class T>
+void mjCModel::MarkPluginInstance(std::unordered_map<std::string, bool>& instances,
+                                  const std::vector<T*>& list) {
+  for (const auto& element : list) {
+    if (!element->plugin_instance_name.empty()) {
+      instances[element->plugin_instance_name] = true;
+    }
+  }
+}
+
+
+
+void mjCModel::RemovePlugins() {
+  // store elements that reference a plugin instance
+  std::unordered_map<std::string, bool> instances;
+  MarkPluginInstance(instances, bodies_);
+  MarkPluginInstance(instances, geoms_);
+  MarkPluginInstance(instances, meshes_);
+  MarkPluginInstance(instances, actuators_);
+  MarkPluginInstance(instances, sensors_);
+
+  // remove plugins that are not referenced
+  int nlist = (int)plugins_.size();
+  int removed = 0;
+  for (int i = 0; i < nlist; i++) {
+    if (plugins_[i]->name.empty()) {
+      continue;
+    }
+    if (instances.find(plugins_[i]->name) == instances.end()) {
+      ids[plugins_[i]->elemtype].erase(plugins_[i]->name);
+      plugins_[i]->Release();
+      plugins_.erase(plugins_.begin() + i);
+      nlist--;
+      i--;
+      removed++;
+    }
+  }
+
+  // if any elements were removed, update ids using processlist
+  if (removed > 0 && !plugins_.empty()) {
+    processlist(ids, plugins_, plugins_[0]->elemtype, /*checkrepeat=*/false);
+  }
+}
+
+
+
 mjCModel& mjCModel::operator-=(const mjCBody& subtree) {
   mjCModel oldmodel(*this);
 
@@ -550,6 +639,7 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) {
   RemoveFromList(equalities_, oldmodel);
   RemoveFromList(actuators_, oldmodel);
   RemoveFromList(sensors_, oldmodel);
+  RemovePlugins();
 
   // restore to the original state
   if (!compiled) {
@@ -3923,49 +4013,6 @@ static void reassignid(vector<T*>& list) {
 }
 
 
-// set ids, check for repeated names
-template <class T>
-static void processlist(mjListKeyMap& ids, vector<T*>& list,
-                        mjtObj type, bool checkrepeat = true) {
-  // assign ids for regular elements
-  if (type < mjNOBJECT) {
-    for (size_t i=0; i < list.size(); i++) {
-      // check for incompatible id setting; SHOULD NOT OCCUR
-      if (list[i]->id != -1 && list[i]->id != i) {
-        throw mjCError(list[i], "incompatible id in %s array, position %d", mju_type2Str(type), i);
-      }
-
-      // id equals position in array
-      list[i]->id = i;
-
-      // add to ids map
-      ids[type][list[i]->name] = i;
-    }
-  }
-
-  // check for repeated names
-  if (checkrepeat) {
-    // created vectors with all names
-    vector<string> allnames;
-    for (size_t i=0; i < list.size(); i++) {
-      if (!list[i]->name.empty()) {
-        allnames.push_back(list[i]->name);
-      }
-    }
-
-    // sort and check for duplicates
-    if (allnames.size() > 1) {
-      std::sort(allnames.begin(), allnames.end());
-      auto adjacent = std::adjacent_find(allnames.begin(), allnames.end());
-      if (adjacent != allnames.end()) {
-        string msg = "repeated name '" + *adjacent + "' in " + mju_type2Str(type);
-        throw mjCError(nullptr, "%s", msg.c_str());
-      }
-    }
-  }
-}
-
-
 
 // set object ids, check for repeated names
 void mjCModel::ProcessLists(bool checkrepeat) {

src/user/user_model.h

@@ -350,6 +350,9 @@ class mjCModel : public mjCModel_, private mjSpec {
   void CopyPlugins(mjModel*);           // copy plugin data
   int CountNJmom(const mjModel* m);     // compute number of non-zeros in actuator_moment matrix
 
+  // remove plugins that are not referenced by any object
+  void RemovePlugins();
+
   // objects created here
   std::vector<mjCFlex*>     flexes_;      // list of flexes
   std::vector<mjCMesh*>     meshes_;      // list of meshes
@@ -430,6 +433,11 @@ class mjCModel : public mjCModel_, private mjSpec {
   // return true if body has valid mass and inertia
   bool CheckBodyMassInertia(mjCBody* body);
 
+  // Mark plugin instances mentioned in the list
+  template <class T>
+  void MarkPluginInstance(std::unordered_map<std::string, bool>& instances,
+                          const std::vector<T*>& list);
+
 
   mjListKeyMap ids;   // map from object names to ids
   mjCError errInfo;   // last error info

test/user/user_api_test.cc

@@ -230,41 +230,41 @@ TEST_F(PluginTest, DeletePlugin) {
   mj_deleteModel(newmodel);
 }
 
+static constexpr char xml_plugin_1[] = R"(
+  <mujoco model="MuJoCo Model">
+    <worldbody>
+      <body name="body"/>
+    </worldbody>
+  </mujoco>)";
+
+static constexpr char xml_plugin_2[] = R"(
+  <mujoco model="MuJoCo Model">
+    <extension>
+      <plugin plugin="mujoco.pid">
+        <instance name="actuator-1">
+          <config key="ki" value="4.0"/>
+          <config key="slewmax" value="3.14159"/>
+        </instance>
+      </plugin>
+    </extension>
+    <worldbody>
+      <body name="empty"/>
+      <body name="body">
+        <joint name="joint"/>
+        <geom size="0.1"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <plugin name="actuator-1" plugin="mujoco.pid" instance="actuator-1"
+              joint="joint" actdim="2"/>
+    </actuator>
+  </mujoco>)";
+
 TEST_F(PluginTest, AttachPlugin) {
-  static constexpr char xml_1[] = R"(
-    <mujoco model="MuJoCo Model">
-      <worldbody>
-        <body name="body"/>
-      </worldbody>
-    </mujoco>)";
-
-  static constexpr char xml_2[] = R"(
-    <mujoco model="MuJoCo Model">
-      <extension>
-        <plugin plugin="mujoco.pid">
-          <instance name="actuator-1">
-            <config key="ki" value="4.0"/>
-            <config key="slewmax" value="3.14159"/>
-          </instance>
-        </plugin>
-      </extension>
-      <worldbody>
-        <body name="empty"/>
-        <body name="body">
-          <joint name="joint"/>
-          <geom size="0.1"/>
-        </body>
-      </worldbody>
-      <actuator>
-        <plugin name="actuator-1" plugin="mujoco.pid" instance="actuator-1"
-                joint="joint" actdim="2"/>
-      </actuator>
-    </mujoco>)";
-
   std::array<char, 1000> err;
-  mjSpec* parent = mj_parseXMLString(xml_1, 0, err.data(), err.size());
+  mjSpec* parent = mj_parseXMLString(xml_plugin_1, 0, err.data(), err.size());
   ASSERT_THAT(parent, NotNull()) << err.data();
-  mjSpec* spec_1 = mj_parseXMLString(xml_2, 0, err.data(), err.size());
+  mjSpec* spec_1 = mj_parseXMLString(xml_plugin_2, 0, err.data(), err.size());
   ASSERT_THAT(spec_1, NotNull()) << err.data();
 
   // do a copy before attaching
@@ -306,6 +306,32 @@ TEST_F(PluginTest, AttachPlugin) {
   mj_deleteSpec(spec_3);
 }
 
+TEST_F(PluginTest, DetachPlugin) {
+  std::array<char, 1000> err;
+  mjSpec* parent = mj_parseXMLString(xml_plugin_1, 0, err.data(), err.size());
+  ASSERT_THAT(parent, NotNull()) << err.data();
+  mjSpec* child = mj_parseXMLString(xml_plugin_2, 0, err.data(), err.size());
+  ASSERT_THAT(child, NotNull()) << err.data();
+
+  // attach a body referencing the plugin to the frame
+  mjsFrame* frame = mjs_addFrame(mjs_findBody(parent, "world"), 0);
+  mjsBody* body = mjs_findBody(child, "body");
+  EXPECT_THAT(mjs_attachBody(frame, body, "child-", ""), NotNull());
+
+  // detach the body and compile
+  mjsBody* body_to_detach = mjs_findBody(parent, "child-body");
+  EXPECT_THAT(body_to_detach, NotNull());
+  EXPECT_THAT(mjs_detachBody(parent, body_to_detach), 0);
+  mjModel* model = mj_compile(parent, nullptr);
+  EXPECT_THAT(model, NotNull());
+  EXPECT_THAT(model->nbody, 2);
+  EXPECT_THAT(model->nplugin, 0);
+
+  mj_deleteModel(model);
+  mj_deleteSpec(parent);
+  mj_deleteSpec(child);
+}
+
 TEST_F(PluginTest, AttachExplicitPlugin) {
   static constexpr char xml_parent[] = R"(
     <mujoco model="MuJoCo Model">