Add geom names to URDF parsing in MuJoCo.

Fixes #566

PiperOrigin-RevId: 500750288
Change-Id: I00f94ee37bb2f15be59df3933d0d4dba583f0921

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:
 

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

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)

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

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

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"(
+  <robot name="">
+  <mujoco>
+    <compiler geomnamesfromurdf="true" discardvisual="false"/>
+  </mujoco>
+
+  <link name="torso">
+    <collision name="collision_box">
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.1 0.2 0.3"/>
+      </geometry>
+    </collision>
+    <visual name="visual_sphere">
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.5"/>
+      </geometry>
+    </visual>
+  </link>
+  </robot>
+  )";
+  std::array<char, 1000> 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"(
+  <robot name="">
+  <mujoco>
+    <compiler discardvisual="false"/>
+  </mujoco>
+
+  <link name="torso">
+    <collision name="shared_name">
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.1 0.2 0.3"/>
+      </geometry>
+    </collision>
+    <visual name="shared_name">
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.5"/>
+      </geometry>
+    </visual>
+  </link>
+  </robot>
+  )";
+  std::array<char, 1000> error;
+  mjModel* model = LoadModelFromString(urdf, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << error.data();
+  mj_deleteModel(model);
+}
+
 }  // namespace
 }  // namespace mujoco