Allow attaching a model to itself.

PiperOrigin-RevId: 623806812
Change-Id: I9b0e36308be754e838790ead8ec717953e4b2350

src/user/user_mesh.cc

@@ -254,11 +254,6 @@ mjCMesh& mjCMesh::operator=(const mjCMesh& other) {
     } else {
       this->graph_ = NULL;
     }
-    if (other.plugin.instance) {
-      mjCPlugin* new_plugin = new mjCPlugin(*static_cast<mjCPlugin*>(other.plugin.instance));
-      plugin = new_plugin->spec;
-      model->plugins.push_back(new_plugin);
-    }
   }
   PointToLocal();
   return *this;

src/user/user_model.cc

@@ -190,95 +190,99 @@ void mjCModel::CopyList(std::vector<T*>& dest,
                         std::map<mjCDef*, int>& def_map,
                         const std::vector<mjCDef*>& defaults) {
   // loop over the elements from the other model
-  for (T* element : source) {
+  int nsource = (int)source.size();
+  for (int i = 0; i < nsource; i++) {
+    T* candidate = new T(*source[i]);
     try {
       // try to find the referenced object in this model
-      element->NameSpace(element->model);
+      candidate->NameSpace(source[i]->model);
-      element->CopyFromSpec();
+      candidate->CopyFromSpec();
-      element->ResolveReferences(this);
+      candidate->ResolveReferences(this);
     } catch (mjCError err) {
       // if not present, skip the element
+      delete candidate;
       continue;
     }
     // copy the element from the other model to this model
-    dest.push_back(new T(*element));
+    dest.push_back(candidate);
     dest.back()->model = this;
-    dest.back()->def = defaults[def_map[element->def]];
+    dest.back()->def = defaults[def_map[candidate->def]];
+    dest.back()->id = -1;
+  }
+  if (!dest.empty()) {
+    processlist(ids, dest, dest[0]->elemtype);
   }
 }
 
 
 
 mjCModel& mjCModel::operator+=(const mjCModel& other) {
-  if (this != &other) {
+  // create global lists
-    // create global lists
+  MakeLists(bodies[0]);
-    MakeLists(bodies[0]);
+  CreateObjectLists();
-    CreateObjectLists();
+  ProcessLists();
-    ProcessLists();
 
-    // copy all elements not in the tree
+  // copy all elements not in the tree
-    std::map<mjCDef*, int> def_map;
+  std::map<mjCDef*, int> def_map;
-    for (int i = 0; i < other.defaults.size(); i++) {
+  int ndefaults = (int)other.defaults.size();
-      defaults.push_back(new mjCDef(*other.defaults[i]));
+  for (int i = 0; i < ndefaults; i++) {
-      def_map[other.defaults[i]] = i;
+    defaults.push_back(new mjCDef(*other.defaults[i]));
-    }
+    def_map[other.defaults[i]] = i;
-    CopyList(flexes, other.flexes, def_map, defaults);
-    CopyList(meshes, other.meshes, def_map, defaults);
-    CopyList(skins, other.skins, def_map, defaults);
-    CopyList(hfields, other.hfields, def_map, defaults);
-    CopyList(textures, other.textures, def_map, defaults);
-    CopyList(materials, other.materials, def_map, defaults);
-    CopyList(pairs, other.pairs, def_map, defaults);
-    CopyList(excludes, other.excludes, def_map, defaults);
-    CopyList(tendons, other.tendons, def_map, defaults);
-    CopyList(equalities, other.equalities, def_map, defaults);
-    CopyList(actuators, other.actuators, def_map, defaults);
-    CopyList(sensors, other.sensors, def_map, defaults);
-    CopyList(numerics, other.numerics, def_map, defaults);
-    CopyList(texts, other.texts, def_map, defaults);
-    CopyList(tuples, other.tuples, def_map, defaults);
-    CopyList(keys, other.keys, def_map, defaults);
-
-    // plugins are global
-    plugins = other.plugins;
-    active_plugins = other.active_plugins;
-
-    // update defaults for the copied objects
-    for (int i = 1; i < other.bodies.size(); i++) {
-      bodies[i]->def = defaults[def_map[other.bodies[i]->def]];
-    }
-    for (int i = 0; i < other.joints.size(); i++) {
-      joints[i]->def = defaults[def_map[other.joints[i]->def]];
-    }
-    for (int i = 0; i < other.geoms.size(); i++) {
-      geoms[i]->def = defaults[def_map[other.geoms[i]->def]];
-    }
-    for (int i = 0; i < other.sites.size(); i++) {
-      sites[i]->def = defaults[def_map[other.sites[i]->def]];
-    }
-    for (int i = 0; i < other.cameras.size(); i++) {
-      cameras[i]->def = defaults[def_map[other.cameras[i]->def]];
-    }
-    for (int i = 0; i < other.lights.size(); i++) {
-      lights[i]->def = defaults[def_map[other.lights[i]->def]];
-    }
-
-    // cast children to mjCBase
-
-
-    // restore to the same state as other
-    if (!compiled) {
-      mjCBody* world = bodies[0];
-      bodies.clear();
-      frames.clear();
-      joints.clear();
-      geoms.clear();
-      sites.clear();
-      cameras.clear();
-      lights.clear();
-      bodies.push_back(world);
-    }
   }
+  CopyList(flexes, other.flexes, def_map, defaults);
+  CopyList(meshes, other.meshes, def_map, defaults);
+  CopyList(skins, other.skins, def_map, defaults);
+  CopyList(hfields, other.hfields, def_map, defaults);
+  CopyList(textures, other.textures, def_map, defaults);
+  CopyList(materials, other.materials, def_map, defaults);
+  CopyList(pairs, other.pairs, def_map, defaults);
+  CopyList(excludes, other.excludes, def_map, defaults);
+  CopyList(tendons, other.tendons, def_map, defaults);
+  CopyList(equalities, other.equalities, def_map, defaults);
+  CopyList(actuators, other.actuators, def_map, defaults);
+  CopyList(sensors, other.sensors, def_map, defaults);
+  CopyList(numerics, other.numerics, def_map, defaults);
+  CopyList(texts, other.texts, def_map, defaults);
+  CopyList(tuples, other.tuples, def_map, defaults);
+  CopyList(keys, other.keys, def_map, defaults);
+
+  // plugins are global
+  plugins = other.plugins;
+  active_plugins = other.active_plugins;
+
+  // update defaults for the copied objects
+  for (int i = 1; i < other.bodies.size(); i++) {
+    bodies[i]->def = defaults[def_map[other.bodies[i]->def]];
+  }
+  for (int i = 0; i < other.joints.size(); i++) {
+    joints[i]->def = defaults[def_map[other.joints[i]->def]];
+  }
+  for (int i = 0; i < other.geoms.size(); i++) {
+    geoms[i]->def = defaults[def_map[other.geoms[i]->def]];
+  }
+  for (int i = 0; i < other.sites.size(); i++) {
+    sites[i]->def = defaults[def_map[other.sites[i]->def]];
+  }
+  for (int i = 0; i < other.cameras.size(); i++) {
+    cameras[i]->def = defaults[def_map[other.cameras[i]->def]];
+  }
+  for (int i = 0; i < other.lights.size(); i++) {
+    lights[i]->def = defaults[def_map[other.lights[i]->def]];
+  }
+
+  // restore to the same state as other
+  if (!compiled) {
+    mjCBody* world = bodies[0];
+    bodies.clear();
+    frames.clear();
+    joints.clear();
+    geoms.clear();
+    sites.clear();
+    cameras.clear();
+    lights.clear();
+    bodies.push_back(world);
+  }
+
   PointToLocal();
   return *this;
 }

src/user/user_objects.cc

@@ -815,15 +815,11 @@ void mjCBody::NameSpace(const mjCModel* m) {
   }
 
   for (auto& camera : cameras) {
-    if (!camera->name.empty()) {
+    camera->NameSpace(m);
-      camera->name = prefix + camera->name + suffix;
-    }
   }
 
   for (auto& light : lights) {
-    if (!light->name.empty()) {
+    light->NameSpace(m);
-      light->name = prefix + light->name + suffix;
-    }
   }
 
   for (auto& body : bodies) {
@@ -1408,6 +1404,10 @@ mjCFrame& mjCFrame::operator+=(const mjCBody& other) {
 
   // TODO: needs to attach only referencing elements
   *model += *other.model;
+
+  // clear suffixes and return
+  other.model->suffix.clear();
+  other.model->prefix.clear();
   return *this;
 }
 
@@ -2501,6 +2501,15 @@ void mjCCamera::PointToLocal() {
 
 
 
+void mjCCamera::NameSpace(const mjCModel* m) {
+  if (!name.empty()) {
+    name = m->prefix + name + m->suffix;
+  }
+  spec_targetbody_ = m->prefix + spec_targetbody_ + m->suffix;
+}
+
+
+
 void mjCCamera::CopyFromSpec() {
   *static_cast<mjsCamera*>(this) = spec;
   userdata_ = spec_userdata_;
@@ -2648,6 +2657,15 @@ void mjCLight::PointToLocal() {
 
 
 
+void mjCLight::NameSpace(const mjCModel* m) {
+  if (!name.empty()) {
+    name = m->prefix + name + m->suffix;
+  }
+  spec_targetbody_ = m->prefix + spec_targetbody_ + m->suffix;
+}
+
+
+
 void mjCLight::CopyFromSpec() {
   *static_cast<mjsLight*>(this) = spec;
   targetbody_ = spec_targetbody_;
@@ -3990,6 +4008,7 @@ void mjCPair::Compile(void) {
 mjCBodyPair::mjCBodyPair(mjCModel* _model) {
   // set model pointer
   model = _model;
+  elemtype = mjOBJ_EXCLUDE;
 
   // set defaults
   spec_bodyname1_.clear();
@@ -4329,7 +4348,7 @@ void mjCTendon::NameSpace(const mjCModel* m) {
     name = m->prefix + name + m->suffix;
   }
   for (int i=0; i<path.size(); i++) {
-    path[i]->NameSpace(model);
+    path[i]->NameSpace(m);
   }
 }
 

src/user/user_objects.h

@@ -584,6 +584,7 @@ class mjCCamera : public mjCCamera_, private mjsCamera {
   void Compile(void);                     // compiler
   void CopyFromSpec(void);
   void PointToLocal(void);
+  void NameSpace(const mjCModel* m);
 };
 
 
@@ -622,6 +623,7 @@ class mjCLight : public mjCLight_, private mjsLight {
   void Compile(void);                     // compiler
   void CopyFromSpec(void);
   void PointToLocal(void);
+  void NameSpace(const mjCModel* m);
 };
 
 

test/user/user_api_test.cc

@@ -196,7 +196,125 @@ TEST_F(PluginTest, RecompileCompareCache) {
 }
 
 // -------------------------------- test attach -------------------------------
-TEST_F(MujocoTest, Attach) {
+TEST_F(MujocoTest, AttachSame) {
+  std::array<char, 1000> er;
+  mjtNum tol = 0;
+  std::string field = "";
+
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body name="body">
+        <joint type="hinge" name="hinge"/>
+        <geom type="cylinder" size=".1 1 0"/>
+        <light mode="targetbody" target="child"/>
+        <body name="child"/>
+      </body>
+      <body name="ignore"/>
+      <frame name="frame" pos=".1 0 0" euler="0 90 0"/>
+    </worldbody>
+    <sensor>
+      <framepos name="sensor" objtype="body" objname="body"/>
+      <framepos name="ignore" objtype="body" objname="ignore"/>
+    </sensor>
+    <tendon>
+      <fixed name="fixed">
+        <joint joint="hinge" coef="2"/>
+      </fixed>
+    </tendon>
+    <actuator>
+      <position name="hinge" joint="hinge"/>
+      <position name="fixed" tendon="fixed"/>
+    </actuator>
+    <contact>
+      <exclude body1="body" body2="child"/>
+    </contact>
+  </mujoco>)";
+
+  static constexpr char xml_result[] = R"(
+  <mujoco>
+    <worldbody>
+      <body name="body">
+        <joint type="hinge" name="hinge"/>
+        <geom type="cylinder" size=".1 1 0"/>
+        <light mode="targetbody" target="child"/>
+        <body name="child"/>
+      </body>
+      <body name="ignore"/>
+      <frame name="frame" pos=".1 0 0" euler="0 90 0">
+        <body name="attached-body-1">
+          <joint type="hinge" name="attached-hinge-1"/>
+          <geom type="cylinder" size=".1 1 0"/>
+          <light mode="targetbody" target="attached-child-1"/>
+          <body name="attached-child-1"/>
+        </body>
+      </frame>
+    </worldbody>
+    <sensor>
+      <framepos name="sensor" objtype="body" objname="body"/>
+      <framepos name="ignore" objtype="body" objname="ignore"/>
+      <framepos name="attached-sensor-1" objtype="body" objname="attached-body-1"/>
+    </sensor>
+    <tendon>
+      <fixed name="fixed">
+        <joint joint="hinge" coef="2"/>
+      </fixed>
+      <fixed name="attached-fixed-1">
+        <joint joint="attached-hinge-1" coef="2"/>
+      </fixed>
+    </tendon>
+    <actuator>
+      <position name="hinge" joint="hinge"/>
+      <position name="fixed" tendon="fixed"/>
+      <position name="attached-hinge-1" joint="attached-hinge-1"/>
+      <position name="attached-fixed-1" tendon="attached-fixed-1"/>
+    </actuator>
+    <contact>
+      <exclude body1="body" body2="child"/>
+      <exclude body1="attached-body-1" body2="attached-child-1"/>
+    </contact>
+  </mujoco>)";
+
+  // create parent
+  mjSpec* parent = ParseSpecFromString(xml, er.data(), er.size());
+  EXPECT_THAT(parent, NotNull()) << er.data();
+
+  // get frame
+  mjsFrame* frame = mjs_findFrame(parent, "frame");
+  EXPECT_THAT(frame, NotNull());
+
+  // get subtree
+  mjsBody* body = mjs_findBody(parent, "body");
+  EXPECT_THAT(body, NotNull());
+
+  // attach child to parent frame
+  EXPECT_THAT(
+      mjs_attachBody(frame, body, /*prefix=*/"attached-", /*suffix=*/"-1"), 0);
+
+  // compile new model
+  mjModel* m_attached = mjs_compile(parent, 0);
+  EXPECT_THAT(m_attached, NotNull());
+
+  // check full name stored in mjModel
+  EXPECT_STREQ(mj_id2name(m_attached, mjOBJ_BODY, 4), "attached-body-1");
+
+  // check body 3 is attached to the world
+  EXPECT_THAT(m_attached->body_parentid[3], 0);
+
+  // compare with expected XML
+  mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size());
+  EXPECT_THAT(m_expected, NotNull()) << er.data();
+  EXPECT_LE(CompareModel(m_attached, m_expected, field), tol)
+            << "Expected and attached models are different!\n"
+            << "Different field: " << field << '\n';;
+
+  // destroy everything
+  mjs_deleteSpec(parent);
+  mj_deleteModel(m_attached);
+  mj_deleteModel(m_expected);
+}
+
+TEST_F(MujocoTest, AttachDifferent) {
   std::array<char, 1000> er;
   mjtNum tol = 0;
   std::string field = "";