Improve URDF geom name loading API.

PiperOrigin-RevId: 505112774
Change-Id: I866e58e664089256a41908dddab9ad79745fb1da

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:
 

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
 ^^^^^^^^^
 

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

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)

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");

src/xml/xml_urdf.cc

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <cstring>
+#include <iostream>
 #include <string>
 #include <vector>
 
@@ -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;
       }
     }
 

src/xml/xml_urdf.h

@@ -16,6 +16,7 @@
 #define THIRD_PARTY_MUJOCO_SRC_XML_XML_URDF_
 
 #include <string>
+#include <unordered_set>
 #include <vector>
 
 #include "user/user_model.h"
@@ -50,11 +51,12 @@ class mjXURDF : public mjXBase {
   void Clear(void);                                   // clear local objects
 
   // URDF parser variables
-  std::vector<std::string> urName;             // body name
-  std::vector<int> urParent;                   // body parent (index in name vector)
-  std::vector<std::vector<int> > urChildren;   // body children (index in name vector)
-  std::vector<std::string> urMat;              // material name
-  std::vector<mjRGBA> urRGBA;                  // material RBG value
+  std::vector<std::string> urName;              // body name
+  std::vector<int> urParent;                    // body parent (index in name vector)
+  std::vector<std::vector<int> > urChildren;    // body children (index in name vector)
+  std::vector<std::string> urMat;               // material name
+  std::vector<mjRGBA> urRGBA;                   // material RBG value
+  std::unordered_set<std::string> urGeomNames;  // geom name
 };
 
 #endif  // THIRD_PARTY_MUJOCO_SRC_XML_XML_URDF_

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"(
   <robot name="">
   <mujoco>
-    <compiler geomnamesfromurdf="true" discardvisual="false"/>
+    <compiler discardvisual="false"/>
   </mujoco>
 
   <link name="torso">
@@ -88,7 +88,7 @@ TEST_F(MujocoTest, ReadsGeomNamesWithCompilerOption) {
   mj_deleteModel(model);
 }
 
-TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesByDefault) {
+TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesCollisionBeforeVisual) {
   static constexpr char urdf[] = R"(
   <robot name="">
   <mujoco>
@@ -114,6 +114,49 @@ TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesByDefault) {
   std::array<char, 1000> 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"(
+  <robot name="">
+  <mujoco>
+    <compiler discardvisual="false"/>
+  </mujoco>
+
+  <link name="torso">
+    <visual name="shared_name">
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.5"/>
+      </geometry>
+    </visual>
+    <collision name="shared_name">
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.1 0.2 0.3"/>
+      </geometry>
+    </collision>
+  </link>
+  </robot>
+  )";
+  std::array<char, 1000> 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);
 }