chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Remove plugins that are not referenced after detaching a body.

Browse Source 
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
This commit is contained in:
Alessio Quaglino
2025-03-21 04:32:57 -07:00
committed by Copybara-Service
parent 8d92a0fbe6
commit dc96ed6bdb
3 changed files with 157 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_model.cc
+91 -44
View File
@@ -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
+8
View File
@@ -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
+58 -32
View File
@@ -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">