From 929e09f84a2b843a7dca761ced74ea6b313b03de Mon Sep 17 00:00:00 2001 From: DeepMind Date: Fri, 27 Jan 2023 07:28:54 -0800 Subject: [PATCH] Improve URDF geom name loading API. PiperOrigin-RevId: 505112774 Change-Id: I866e58e664089256a41908dddab9ad79745fb1da --- doc/XMLreference.rst | 4 --- doc/changelog.rst | 2 ++ src/user/user_model.cc | 1 - src/user/user_model.h | 1 - src/xml/xml_native_reader.cc | 3 --- src/xml/xml_urdf.cc | 28 ++++++++++++++++----- src/xml/xml_urdf.h | 12 +++++---- test/xml/xml_urdf_test.cc | 49 +++++++++++++++++++++++++++++++++--- 8 files changed, 77 insertions(+), 23 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 1b12c7de..bc3168d9 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -247,10 +247,6 @@ any effect. The settings here are global and apply to the entire model. participate in the inertial computations, so any geoms attached to it are automatically ignored. Therefore it is not necessary to adjust this attribute and the geom-specific groups so as to exclude world geoms from the inertial computation. -:at:`geomnamesfromurdf`: :at-val:`[false, true], "false"` - This attribute specifies whether the URDF parser should use the names of geoms specified in the URDF in the model. - This is disabled by default as URDF allows multiple geoms in the model to have the same name, if this attribute is set - to true and there are multiple geoms with the same name in a parsed URDF, this will lead to a parse error. .. _compiler-lengthrange: diff --git a/doc/changelog.rst b/doc/changelog.rst index 616adf72..cb156d76 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -19,6 +19,8 @@ General :align: right :width: 300px +- Geom names are now parsed from URDF. Any duplicate names are ignored. + Bug fixes ^^^^^^^^^ diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 8cc799cb..e6309eeb 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -104,7 +104,6 @@ mjCModel::mjCModel() { inertiagrouprange[0] = 0; inertiagrouprange[1] = mjNGROUP-1; exactmeshinertia = false; - geomnamesfromurdf = false; mj_defaultLROpt(&LRopt); //------------------------ statistics override diff --git a/src/user/user_model.h b/src/user/user_model.h index ebce716a..6eb6c2a5 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -135,7 +135,6 @@ class mjCModel { int inertiafromgeom; // use geom inertias (mjtInertiaFromGeom) int inertiagrouprange[2]; // range of geom groups used to compute inertia bool exactmeshinertia; // if false, use old formula - bool geomnamesfromurdf; // parse geom names from URDF mjLROpt LRopt; // options for lengthrange computation //------------------------ statistics override (if defined) diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 45852259..9fc37eaa 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -902,9 +902,6 @@ void mjXReader::Compiler(XMLElement* section, mjCModel* mod) { if (MapValue(section, "exactmeshinertia", &n, bool_map, 2)){ mod->exactmeshinertia = (n==1); } - if (MapValue(section, "geomnamesfromurdf", &n, bool_map, 2)) { - mod->geomnamesfromurdf = (n==1); - } // lengthrange subelement XMLElement* elem = FindSubElem(section, "lengthrange"); diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index 4d168a04..1bde0bb3 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include @@ -64,6 +65,7 @@ void mjXURDF::Clear(void) { urChildren.clear(); urMat.clear(); urRGBA.clear(); + urGeomNames.clear(); } @@ -245,6 +247,8 @@ void mjXURDF::Body(XMLElement* body_elem) { // process all visual and geometry elements in order float rgba[4] = {-1, 0, 0, 0}; + std::string geom_name; + elem = body_elem->FirstChildElement(); while (elem) { name = elem->Value(); @@ -277,9 +281,15 @@ void mjXURDF::Body(XMLElement* body_elem) { std::memcpy(pgeom->rgba, rgba, 4*sizeof(float)); } - // save name - if (model->geomnamesfromurdf) { - mjXUtil::ReadAttrTxt(elem, "name", pgeom->name); + // save name if it doesn't already exist. + mjXUtil::ReadAttrTxt(elem, "name", geom_name); + if (urGeomNames.find(geom_name) == urGeomNames.end()) { + pgeom->name = geom_name; + urGeomNames.insert(geom_name); + } else { + std::cerr << "WARNING: Geom with duplicate name '" << geom_name + << "' encountered in URDF, creating an unnamed geom." + << std::endl; } } } @@ -293,9 +303,15 @@ void mjXURDF::Body(XMLElement* body_elem) { std::memcpy(pgeom->rgba, rgba, 4*sizeof(float)); } - // save name - if (model->geomnamesfromurdf) { - mjXUtil::ReadAttrTxt(elem, "name", pgeom->name); + // save name if it doesn't already exist. + mjXUtil::ReadAttrTxt(elem, "name", geom_name); + if (urGeomNames.find(geom_name) == urGeomNames.end()) { + pgeom->name = geom_name; + urGeomNames.insert(geom_name); + } else { + std::cerr << "WARNING: Geom with duplicate name '" << geom_name + << "' encountered in URDF, creating an unnamed geom." + << std::endl; } } diff --git a/src/xml/xml_urdf.h b/src/xml/xml_urdf.h index 01b09666..040f2d36 100644 --- a/src/xml/xml_urdf.h +++ b/src/xml/xml_urdf.h @@ -16,6 +16,7 @@ #define THIRD_PARTY_MUJOCO_SRC_XML_XML_URDF_ #include +#include #include #include "user/user_model.h" @@ -50,11 +51,12 @@ class mjXURDF : public mjXBase { void Clear(void); // clear local objects // URDF parser variables - std::vector urName; // body name - std::vector urParent; // body parent (index in name vector) - std::vector > urChildren; // body children (index in name vector) - std::vector urMat; // material name - std::vector urRGBA; // material RBG value + std::vector urName; // body name + std::vector urParent; // body parent (index in name vector) + std::vector > urChildren; // body children (index in name vector) + std::vector urMat; // material name + std::vector urRGBA; // material RBG value + std::unordered_set urGeomNames; // geom name }; #endif // THIRD_PARTY_MUJOCO_SRC_XML_XML_URDF_ diff --git a/test/xml/xml_urdf_test.cc b/test/xml/xml_urdf_test.cc index ea779b91..3d798a54 100644 --- a/test/xml/xml_urdf_test.cc +++ b/test/xml/xml_urdf_test.cc @@ -49,11 +49,11 @@ TEST_F(MujocoTest, ReadsCapsule) { mj_deleteModel(model); } -TEST_F(MujocoTest, ReadsGeomNamesWithCompilerOption) { +TEST_F(MujocoTest, ReadsGeomNames) { static constexpr char urdf[] = R"( - + @@ -88,7 +88,7 @@ TEST_F(MujocoTest, ReadsGeomNamesWithCompilerOption) { mj_deleteModel(model); } -TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesByDefault) { +TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesCollisionBeforeVisual) { static constexpr char urdf[] = R"( @@ -114,6 +114,49 @@ TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesByDefault) { std::array error; mjModel* model = LoadModelFromString(urdf, error.data(), error.size()); ASSERT_THAT(model, NotNull()) << error.data(); + + // Check the collision geom gets its name from the URDF. The visual sphere + // should not have a name to avoid duplicates. + int collision_box_id = mj_name2id(model, mjtObj::mjOBJ_GEOM, "shared_name"); + ASSERT_GE(collision_box_id, 0); + EXPECT_EQ(model->geom_type[collision_box_id], mjtGeom::mjGEOM_BOX); + + mj_deleteModel(model); +} + +TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesVisualBeforeCollision) { + static constexpr char urdf[] = R"( + + + + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(urdf, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + + // Check the visual geom gets its name from the URDF. The collision geom + // should not have a name to avoid duplicates. + int visual_sphere_id = mj_name2id(model, mjtObj::mjOBJ_GEOM, "shared_name"); + ASSERT_GE(visual_sphere_id, 0); + EXPECT_EQ(model->geom_type[visual_sphere_id], mjtGeom::mjGEOM_SPHERE); + mj_deleteModel(model); }