From 703b33c064454c9e10b506a8709b19e3d7af2f32 Mon Sep 17 00:00:00 2001 From: DeepMind Date: Mon, 9 Jan 2023 10:33:40 -0800 Subject: [PATCH] Add geom names to URDF parsing in MuJoCo. Fixes #566 PiperOrigin-RevId: 500750288 Change-Id: I00f94ee37bb2f15be59df3933d0d4dba583f0921 --- doc/XMLreference.rst | 4 +++ src/user/user_model.cc | 1 + src/user/user_model.h | 1 + src/xml/xml_native_reader.cc | 3 ++ src/xml/xml_urdf.cc | 10 ++++++ test/xml/xml_urdf_test.cc | 68 ++++++++++++++++++++++++++++++++++++ 6 files changed, 87 insertions(+) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index bc3168d9..1b12c7de 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -247,6 +247,10 @@ 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/src/user/user_model.cc b/src/user/user_model.cc index e6309eeb..8cc799cb 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -104,6 +104,7 @@ 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 6eb6c2a5..ebce716a 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -135,6 +135,7 @@ 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 9fc37eaa..45852259 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -902,6 +902,9 @@ 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 4548cc00..4d168a04 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -276,6 +276,11 @@ void mjXURDF::Body(XMLElement* body_elem) { if (rgba[0]>=0) { std::memcpy(pgeom->rgba, rgba, 4*sizeof(float)); } + + // save name + if (model->geomnamesfromurdf) { + mjXUtil::ReadAttrTxt(elem, "name", pgeom->name); + } } } @@ -287,6 +292,11 @@ void mjXURDF::Body(XMLElement* body_elem) { if (rgba[0]>=0) { std::memcpy(pgeom->rgba, rgba, 4*sizeof(float)); } + + // save name + if (model->geomnamesfromurdf) { + mjXUtil::ReadAttrTxt(elem, "name", pgeom->name); + } } // advance diff --git a/test/xml/xml_urdf_test.cc b/test/xml/xml_urdf_test.cc index dc729947..ea779b91 100644 --- a/test/xml/xml_urdf_test.cc +++ b/test/xml/xml_urdf_test.cc @@ -49,5 +49,73 @@ TEST_F(MujocoTest, ReadsCapsule) { mj_deleteModel(model); } +TEST_F(MujocoTest, ReadsGeomNamesWithCompilerOption) { + static constexpr char urdf[] = R"( + + + + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(urdf, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + + // Check the geoms have been loaded with the right names + int collision_box_id = mj_name2id(model, mjtObj::mjOBJ_GEOM, "collision_box"); + ASSERT_GE(collision_box_id, 0); + EXPECT_EQ(model->geom_type[collision_box_id], mjtGeom::mjGEOM_BOX); + + int visual_sphere_id = mj_name2id(model, mjtObj::mjOBJ_GEOM, "visual_sphere"); + ASSERT_GE(visual_sphere_id, 0); + EXPECT_EQ(model->geom_type[visual_sphere_id], mjtGeom::mjGEOM_SPHERE); + + mj_deleteModel(model); +} + +TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesByDefault) { + static constexpr char urdf[] = R"( + + + + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(urdf, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco