From 70f00b05a1b3e7814f19eceab4f8167eb5e5e56a Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 11 Apr 2024 06:27:44 -0700 Subject: [PATCH] Allow attaching a model to itself. PiperOrigin-RevId: 623806812 Change-Id: I9b0e36308be754e838790ead8ec717953e4b2350 --- src/user/user_mesh.cc | 5 -- src/user/user_model.cc | 150 +++++++++++++++++++------------------ src/user/user_objects.cc | 33 ++++++-- src/user/user_objects.h | 2 + test/user/user_api_test.cc | 120 ++++++++++++++++++++++++++++- 5 files changed, 224 insertions(+), 86 deletions(-) diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 1ffadf6c..ed2f7d72 100644 --- a/src/user/user_mesh.cc +++ b/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(other.plugin.instance)); - plugin = new_plugin->spec; - model->plugins.push_back(new_plugin); - } } PointToLocal(); return *this; diff --git a/src/user/user_model.cc b/src/user/user_model.cc index c80077be..dc106b9d 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -190,95 +190,99 @@ void mjCModel::CopyList(std::vector& dest, std::map& def_map, const std::vector& 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); - element->CopyFromSpec(); - element->ResolveReferences(this); + candidate->NameSpace(source[i]->model); + candidate->CopyFromSpec(); + 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 - MakeLists(bodies[0]); - CreateObjectLists(); - ProcessLists(); + // create global lists + MakeLists(bodies[0]); + CreateObjectLists(); + ProcessLists(); - // copy all elements not in the tree - std::map def_map; - for (int i = 0; i < other.defaults.size(); 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); - } + // copy all elements not in the tree + std::map def_map; + int ndefaults = (int)other.defaults.size(); + for (int i = 0; i < ndefaults; 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]]; + } + + // 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; } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index e6383cc0..5c361a96 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -815,15 +815,11 @@ void mjCBody::NameSpace(const mjCModel* m) { } for (auto& camera : cameras) { - if (!camera->name.empty()) { - camera->name = prefix + camera->name + suffix; - } + camera->NameSpace(m); } for (auto& light : lights) { - if (!light->name.empty()) { - light->name = prefix + light->name + suffix; - } + light->NameSpace(m); } 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(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(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; iNameSpace(model); + path[i]->NameSpace(m); } } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index df1ef8bb..1b9f0f2d 100644 --- a/src/user/user_objects.h +++ b/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); }; diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 74bd22cf..4b01e1a8 100644 --- a/test/user/user_api_test.cc +++ b/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 er; + mjtNum tol = 0; + std::string field = ""; + + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + static constexpr char xml_result[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + // 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 er; mjtNum tol = 0; std::string field = "";