Prevent duplicated pairs in active_pugins vector.

Fixes #2150 PiperOrigin-RevId: 688164615 Change-Id: I9a5434a9dda28a399ffccfe0c4b31fa64400b85f
google-deepmind · Oct 21, 2024 · a70aea9 · a70aea9
1 parent 90bc4b5
commit a70aea9
Show file tree

Hide file tree

Showing 2 changed files with 54 additions and 2 deletions.
diff --git a/src/user/user_model.cc b/src/user/user_model.cc
@@ -304,6 +304,8 @@ void mjCModel::SaveDofOffsets(bool computesize) {
   }
 }
 
+
+
 template <class T>
 void mjCModel::CopyPlugin(std::vector<mjCPlugin*>& dest,
                           const std::vector<mjCPlugin*>& source,
@@ -336,6 +338,21 @@ void mjCModel::CopyPlugin(std::vector<mjCPlugin*>& dest,
   }
 }
 
+
+
+// return true if the plugin is already in the list of active plugins
+static bool IsPluginActive(
+    const mjpPlugin* plugin,
+    const std::vector<std::pair<const mjpPlugin*, int>>& active_plugins) {
+  return std::find_if(
+             active_plugins.begin(), active_plugins.end(),
+             [&plugin](const std::pair<const mjpPlugin*, int>& element) {
+               return element.first == plugin;
+             }) != active_plugins.end();
+}
+
+
+
 mjCModel& mjCModel::operator+=(const mjCModel& other) {
   // TODO: use compiler settings stored in specs_ during compilation
   std::string msg = "cannot attach mjSpecs with incompatible compiler/";
@@ -387,8 +404,10 @@ mjCModel& mjCModel::operator+=(const mjCModel& other) {
   CopyPlugin(plugins_, other.plugins_, meshes_);
   CopyPlugin(plugins_, other.plugins_, actuators_);
   CopyPlugin(plugins_, other.plugins_, sensors_);
-  for (const auto& active_plugin : other.active_plugins_) {
-    active_plugins_.emplace_back(active_plugin);
+  for (const auto& [plugin, slot] : other.active_plugins_) {
+    if (!IsPluginActive(plugin, active_plugins_)) {
+      active_plugins_.emplace_back(std::make_pair(plugin, slot));
+    }
   }
 
   // restore to the original state

diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc
@@ -293,6 +293,39 @@ TEST_F(PluginTest, AttachPlugin) {
   mj_deleteSpec(spec_2);
 }
 
+TEST_F(PluginTest, ReplicatePlugin) {
+  static constexpr char xml[] = R"(
+    <mujoco>
+      <extension>
+        <plugin plugin="mujoco.sdf.torus">
+          <instance name="torus" />
+        </plugin>
+      </extension>
+      <asset>
+        <mesh name="torus">
+          <plugin instance="torus" />
+        </mesh>
+      </asset>
+      <worldbody>
+        <geom type="sdf" mesh="torus">
+          <plugin instance="torus" />
+        </geom>
+        <replicate count="100">
+          <body />
+        </replicate>
+      </worldbody>
+    </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);