From dc96ed6bdb67dfb9cc277977c99340dd71ea8d2d Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 21 Mar 2025 04:32:57 -0700 Subject: [PATCH] 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 | 135 +++++++++++++++++++++++++------------ src/user/user_model.h | 8 +++ test/user/user_api_test.cc | 90 ++++++++++++++++--------- 3 files changed, 157 insertions(+), 76 deletions(-) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 3fa8c9f8..bc12a17e 100644 --- a/src/user/user_model.cc +++ b/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 +static void processlist(mjListKeyMap& ids, vector& 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 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& 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(std::vector& elements) { +template +void mjCModel::MarkPluginInstance(std::unordered_map& instances, + const std::vector& 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 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& list) { } -// set ids, check for repeated names -template -static void processlist(mjListKeyMap& ids, vector& 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 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) { diff --git a/src/user/user_model.h b/src/user/user_model.h index 19386280..1078080f 100644 --- a/src/user/user_model.h +++ b/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 flexes_; // list of flexes std::vector 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 + void MarkPluginInstance(std::unordered_map& instances, + const std::vector& list); + mjListKeyMap ids; // map from object names to ids mjCError errInfo; // last error info diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 343400dd..84febf8e 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -230,41 +230,41 @@ TEST_F(PluginTest, DeletePlugin) { mj_deleteModel(newmodel); } +static constexpr char xml_plugin_1[] = R"( + + + + + )"; + +static constexpr char xml_plugin_2[] = R"( + + + + + + + + + + + + + + + + + + + + )"; + TEST_F(PluginTest, AttachPlugin) { - static constexpr char xml_1[] = R"( - - - - - )"; - - static constexpr char xml_2[] = R"( - - - - - - - - - - - - - - - - - - - - )"; - std::array 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 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"(