From 45f908c165cf2cc48d2ccf3e60ae25f191b1e3b7 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 12 Nov 2024 09:33:32 -0800 Subject: [PATCH] Enable attaching plugins not instantiated in the `extension` section. Fixes #2217. PiperOrigin-RevId: 695766524 Change-Id: I534b63400810bd6ef888f39f6665213280fd93ac --- src/user/user_mesh.cc | 8 +++- src/user/user_model.cc | 40 ++++++++++++++---- src/user/user_model.h | 6 ++- src/user/user_objects.cc | 34 +++++++++++++-- src/user/user_objects.h | 8 ++++ test/user/user_api_test.cc | 84 ++++++++++++++++++++++++++++++++++++++ 6 files changed, 164 insertions(+), 16 deletions(-) diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 695de30f..766e4976 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -268,10 +268,16 @@ void mjCMesh::CopyFromSpec() { +void mjCMesh::CopyPlugin() { + model->CopyExplicitPlugin(this); +} + + + mjCMesh::~mjCMesh() { if (center_) mju_free(center_); if (graph_) mju_free(graph_); - if (spec.plugin.active && spec.plugin.name->empty()) { + if (spec.plugin.active && spec.plugin.name->empty() && model) { model->DeleteElement(spec.plugin.element); } } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 94b3ec3d..e01fbc5c 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -232,6 +232,7 @@ void mjCModel::CopyList(std::vector& dest, dest.back()->model = this; dest.back()->compiler = origin ? &origin->compiler : &spec.compiler; dest.back()->id = -1; + dest.back()->CopyPlugin(); } if (!dest.empty()) { processlist(ids, dest, dest[0]->elemtype); @@ -309,8 +310,28 @@ void mjCModel::SaveDofOffsets(bool computesize) { template -void mjCModel::CopyPlugin(std::vector& dest, - const std::vector& source, +void mjCModel::CopyExplicitPlugin(T* obj) { + if (!obj->plugin.active || !obj->plugin_instance_name.empty() || !obj->spec.plugin.element) { + return; + } + mjCPlugin* origin = static_cast(obj->spec.plugin.element); + mjCPlugin* candidate = new mjCPlugin(*origin); + candidate->id = plugins_.size(); + candidate->model = this; + plugins_.push_back(candidate); + obj->spec.plugin.element = candidate; +} + +template void mjCModel::CopyExplicitPlugin(mjCBody* obj); +template void mjCModel::CopyExplicitPlugin(mjCGeom* obj); +template void mjCModel::CopyExplicitPlugin(mjCMesh* obj); +template void mjCModel::CopyExplicitPlugin(mjCActuator* obj); +template void mjCModel::CopyExplicitPlugin(mjCSensor* obj); + + + +template +void mjCModel::CopyPlugin(const std::vector& source, const std::vector& list) { // store elements that reference a plugin instance std::unordered_map instances; @@ -330,9 +351,10 @@ void mjCModel::CopyPlugin(std::vector& dest, candidate->NameSpace(plugin->model); bool referenced = instances.find(candidate->name) != instances.end(); auto same_name = [candidate](const mjCPlugin* dest) { return dest->name == candidate->name; }; - bool instance_exists = std::find_if(dest.begin(), dest.end(), same_name) != dest.end(); + bool instance_exists = std::find_if(plugins_.begin(), plugins_.end(), + same_name) != plugins_.end(); if (referenced && !instance_exists) { - dest.push_back(candidate); + plugins_.push_back(candidate); instances.at(candidate->name)->spec.plugin.element = candidate; } else { delete candidate; @@ -390,11 +412,11 @@ mjCModel& mjCModel::operator+=(const mjCModel& other) { CopyList(tuples_, other.tuples_); // create new plugins and map them - CopyPlugin(plugins_, other.plugins_, bodies_); - CopyPlugin(plugins_, other.plugins_, geoms_); - CopyPlugin(plugins_, other.plugins_, meshes_); - CopyPlugin(plugins_, other.plugins_, actuators_); - CopyPlugin(plugins_, other.plugins_, sensors_); + CopyPlugin(other.plugins_, bodies_); + CopyPlugin(other.plugins_, geoms_); + CopyPlugin(other.plugins_, meshes_); + CopyPlugin(other.plugins_, actuators_); + CopyPlugin(other.plugins_, sensors_); for (const auto& [plugin, slot] : other.active_plugins_) { if (!IsPluginActive(plugin, active_plugins_)) { active_plugins_.emplace_back(std::make_pair(plugin, slot)); diff --git a/src/user/user_model.h b/src/user/user_model.h index d65d93d9..33341228 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -366,9 +366,11 @@ class mjCModel : public mjCModel_, private mjSpec { template void CopyList(std::vector& dest, const std::vector& sources); + // copy plugins that are explicitly instantiated by the argument object to this model + template void CopyExplicitPlugin(T* obj); + // copy vector of plugins to this model - template void CopyPlugin(std::vector& dest, - const std::vector& sources, + template void CopyPlugin(const std::vector& sources, const std::vector& list); // delete from list the elements that cause an error diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index e795f49a..35659e3e 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -789,6 +789,7 @@ mjCBody::mjCBody(const mjCBody& other, mjCModel* _model) { mjSpec* origin = model->FindSpec(mjs_getString(other.model->spec.modelname)); compiler = origin ? &origin->compiler : &model->spec.compiler; *this = other; + CopyPlugin(); } @@ -942,6 +943,7 @@ void mjCBody::CopyList(std::vector& dst, const std::vector& src, dst.back()->model = model; dst.back()->compiler = origin ? &origin->compiler : &model->spec.compiler; dst.back()->id = -1; + dst.back()->CopyPlugin(); dst.back()->classname = src[i]->classname; // assign dst frame to src frame @@ -992,6 +994,12 @@ void mjCBody::CopyFromSpec() { +void mjCBody::CopyPlugin() { + model->CopyExplicitPlugin(this); +} + + + // destructor mjCBody::~mjCBody() { // delete objects allocated here @@ -1011,7 +1019,7 @@ mjCBody::~mjCBody() { cameras.clear(); lights.clear(); - if (spec.plugin.active && spec.plugin.name->empty()) { + if (spec.plugin.active && spec.plugin.name->empty() && model) { model->DeleteElement(spec.plugin.element); } } @@ -2221,7 +2229,7 @@ mjCGeom::mjCGeom(const mjCGeom& other) { mjCGeom::~mjCGeom() { - if (spec.plugin.active && spec.plugin.name->empty()) { + if (spec.plugin.active && spec.plugin.name->empty() && model) { model->DeleteElement(spec.plugin.element); } } @@ -2273,6 +2281,12 @@ void mjCGeom::CopyFromSpec() { +void mjCGeom::CopyPlugin() { + model->CopyExplicitPlugin(this); +} + + + void mjCGeom::NameSpace(const mjCModel* m) { mjCBase::NameSpace(m); if (!spec_material_.empty() && model != m) { @@ -5636,7 +5650,7 @@ mjCActuator::mjCActuator(const mjCActuator& other) { mjCActuator::~mjCActuator() { - if (spec.plugin.active && spec.plugin.name->empty()) { + if (spec.plugin.active && spec.plugin.name->empty() && model) { model->DeleteElement(spec.plugin.element); } } @@ -5731,6 +5745,12 @@ void mjCActuator::CopyFromSpec() { +void mjCActuator::CopyPlugin() { + model->CopyExplicitPlugin(this); +} + + + void mjCActuator::ResolveReferences(const mjCModel* m) { switch (trntype) { case mjTRN_JOINT: @@ -6000,7 +6020,7 @@ mjCSensor::mjCSensor(const mjCSensor& other) { mjCSensor::~mjCSensor() { - if (spec.plugin.active && spec.plugin.name->empty()) { + if (spec.plugin.active && spec.plugin.name->empty() && model) { model->DeleteElement(spec.plugin.element); } } @@ -6063,6 +6083,12 @@ void mjCSensor::CopyFromSpec() { +void mjCSensor::CopyPlugin() { + model->CopyExplicitPlugin(this); +} + + + void mjCSensor::ResolveReferences(const mjCModel* m) { objname_ = prefix + objname_ + suffix; refname_ = prefix + refname_ + suffix; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 1018fb8b..5b031d6b 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -208,6 +208,9 @@ class mjCBase : public mjCBase_ { // Appends prefix and suffix to reference virtual void NameSpace(const mjCModel* m); + // Copy plugins instantiated in this object + virtual void CopyPlugin() {} + // Copy assignment mjCBase& operator=(const mjCBase& other); @@ -360,6 +363,7 @@ class mjCBody : public mjCBody_, private mjsBody { void CopyFromSpec(); // copy spec into attributes void PointToLocal(void); void NameSpace_(const mjCModel* m, bool propagate = true); + void CopyPlugin(); // copy src list of elements into dst; set body, model and frame template @@ -557,6 +561,7 @@ class mjCGeom : public mjCGeom_, private mjsGeom { void CopyFromSpec(void); void PointToLocal(void); void NameSpace(const mjCModel* m); + void CopyPlugin(); // inherited using mjCBase::info; @@ -935,6 +940,7 @@ class mjCMesh: public mjCMesh_, private mjsMesh { void ApplyTransformations(); // apply user transformations void ComputeFaceCentroid(double[3]); // compute centroid of all faces void CheckMesh(mjtGeomInertia type); // check if the mesh is valid + void CopyPlugin(); // mesh data to be copied into mjModel double* center_; // face circumcenter data (3*nface) @@ -1503,6 +1509,7 @@ class mjCActuator : public mjCActuator_, private mjsActuator { void PointToLocal(); void ResolveReferences(const mjCModel* m); void NameSpace(const mjCModel* m); + void CopyPlugin(); // reset keyframe references for allowing self-attach void ForgetKeyframes(); @@ -1556,6 +1563,7 @@ class mjCSensor : public mjCSensor_, private mjsSensor { void PointToLocal(); void ResolveReferences(const mjCModel* m); void NameSpace(const mjCModel* m); + void CopyPlugin(); mjCBase* obj; // sensorized object mjCBase* ref; // sensorized reference diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 93c0c1ab..e376d046 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -293,6 +293,56 @@ TEST_F(PluginTest, AttachPlugin) { mj_deleteSpec(spec_2); } +TEST_F(PluginTest, AttachExplicitPlugin) { + static constexpr char xml_parent[] = R"( + + + + + )"; + + static constexpr char xml_child[] = R"( + + + + + + + + + + + + + + + + + + + )"; + + std::array err; + mjSpec* parent = mj_parseXMLString(xml_parent, 0, err.data(), err.size()); + ASSERT_THAT(parent, NotNull()) << err.data(); + mjSpec* child = mj_parseXMLString(xml_child, 0, err.data(), err.size()); + ASSERT_THAT(child, NotNull()) << err.data(); + + mjsBody* body_parent = mjs_findBody(parent, "body"); + EXPECT_THAT(body_parent, NotNull()); + mjsFrame* attachment_frame = mjs_addFrame(body_parent, 0); + EXPECT_THAT(attachment_frame, NotNull()); + + mjs_attachBody(attachment_frame, mjs_findBody(child, "body"), "child-", ""); + mjModel* model = mj_compile(parent, nullptr); + EXPECT_THAT(model, NotNull()); + EXPECT_THAT(model->nplugin, 1); + + mj_deleteSpec(parent); + mj_deleteSpec(child); + mj_deleteModel(model); +} + TEST_F(PluginTest, ReplicatePlugin) { static constexpr char xml[] = R"( @@ -326,6 +376,40 @@ TEST_F(PluginTest, ReplicatePlugin) { mj_deleteModel(model); } +TEST_F(PluginTest, ReplicateExplicitPlugin) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + )"; + + std::array err; + mjSpec* spec = mj_parseXMLString(xml, 0, err.data(), err.size()); + ASSERT_THAT(spec, NotNull()) << err.data(); + mjModel* model = mj_compile(spec, nullptr); + EXPECT_THAT(model, NotNull()); + EXPECT_THAT(model->nplugin, 1); + mj_deleteSpec(spec); + mj_deleteModel(model); +} + TEST_F(MujocoTest, RecompileFails) { mjSpec* spec = mj_makeSpec(); mjsBody* body = mjs_addBody(mjs_findBody(spec, "world"), 0);