Enable attaching plugins not instantiated in the extension section.

Fixes #2217.

PiperOrigin-RevId: 695766524
Change-Id: I534b63400810bd6ef888f39f6665213280fd93ac

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);
   }
 }

src/user/user_model.cc

@@ -232,6 +232,7 @@ void mjCModel::CopyList(std::vector<T*>& 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 <class T>
-void mjCModel::CopyPlugin(std::vector<mjCPlugin*>& dest,
-                          const std::vector<mjCPlugin*>& source,
+void mjCModel::CopyExplicitPlugin(T* obj) {
+  if (!obj->plugin.active || !obj->plugin_instance_name.empty() || !obj->spec.plugin.element) {
+    return;
+  }
+  mjCPlugin* origin = static_cast<mjCPlugin*>(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>(mjCBody* obj);
+template void mjCModel::CopyExplicitPlugin<mjCGeom>(mjCGeom* obj);
+template void mjCModel::CopyExplicitPlugin<mjCMesh>(mjCMesh* obj);
+template void mjCModel::CopyExplicitPlugin<mjCActuator>(mjCActuator* obj);
+template void mjCModel::CopyExplicitPlugin<mjCSensor>(mjCSensor* obj);
+
+
+
+template <class T>
+void mjCModel::CopyPlugin(const std::vector<mjCPlugin*>& source,
                           const std::vector<T*>& list) {
   // store elements that reference a plugin instance
   std::unordered_map<std::string, T*> instances;
@@ -330,9 +351,10 @@ void mjCModel::CopyPlugin(std::vector<mjCPlugin*>& 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));

src/user/user_model.h

@@ -366,9 +366,11 @@ class mjCModel : public mjCModel_, private mjSpec {
   template <class T> void CopyList(std::vector<T*>& dest,
                                    const std::vector<T*>& sources);
 
+  // copy plugins that are explicitly instantiated by the argument object to this model
+  template <class T> void CopyExplicitPlugin(T* obj);
+
   // copy vector of plugins to this model
-  template <class T> void CopyPlugin(std::vector<mjCPlugin*>& dest,
-                                     const std::vector<mjCPlugin*>& sources,
+  template <class T> void CopyPlugin(const std::vector<mjCPlugin*>& sources,
                                      const std::vector<T*>& list);
 
   // delete from list the elements that cause an error

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<T*>& dst, const std::vector<T*>& 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;

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 <typename T>
@@ -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

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"(
+    <mujoco model="MuJoCo Model">
+      <worldbody>
+        <body name="body"/>
+      </worldbody>
+    </mujoco>)";
+
+  static constexpr char xml_child[] = R"(
+    <mujoco>
+      <extension>
+        <plugin plugin="mujoco.sensor.touch_grid"/>
+      </extension>
+      <worldbody>
+        <body name="body">
+          <geom type="sphere" size=".1" />
+          <site name="touch2" size="0.001"/>
+        </body>
+      </worldbody>
+      <sensor>
+        <plugin name="touch2" plugin="mujoco.sensor.touch_grid" objtype="site" objname="touch2">
+          <config key="size" value="8 12"/>
+          <config key="fov" value="10 13"/>
+          <config key="gamma" value="0"/>
+          <config key="nchannel" value="1"/>
+        </plugin>
+      </sensor>
+    </mujoco>)";
+
+  std::array<char, 1000> 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"(
     <mujoco>
@@ -326,6 +376,40 @@ TEST_F(PluginTest, ReplicatePlugin) {
   mj_deleteModel(model);
 }
 
+TEST_F(PluginTest, ReplicateExplicitPlugin) {
+  static constexpr char xml[] = R"(
+    <mujoco>
+      <extension>
+        <plugin plugin="mujoco.sensor.touch_grid"/>
+      </extension>
+      <worldbody>
+        <body name="tactile_sensor_2">
+          <replicate count="2" offset="0 0.1 0">
+            <geom type="sphere" size=".0008" />
+          </replicate>
+          <site name="touch2" size="0.001"/>
+        </body>
+      </worldbody>
+      <sensor>
+        <plugin name="touch2" plugin="mujoco.sensor.touch_grid" objtype="site" objname="touch2">
+          <config key="size" value="8 12"/>
+          <config key="fov" value="10 13"/>
+          <config key="gamma" value="0"/>
+          <config key="nchannel" value="1"/>
+        </plugin>
+      </sensor>
+    </mujoco>)";
+
+  std::array<char, 1000> 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);