Improve URDF geom name loading API.

PiperOrigin-RevId: 505112774
Change-Id: I866e58e664089256a41908dddab9ad79745fb1da

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_