[MISC] Fix gltf Loading for URDF (#1857)

Author: ACMLCZH

genesis/engine/mesh.py

@@ -343,10 +343,21 @@ def from_morph_surface(cls, morph, surface=None):
             if morph.is_format(gs.options.morphs.MESH_FORMATS):
                 meshes = mu.parse_mesh_trimesh(morph.file, morph.group_by_material, morph.scale, surface)
             elif morph.is_format(gs.options.morphs.GLTF_FORMATS):
+                if not morph.parse_glb_with_zup:
+                    gs.logger.warning(
+                        "GLTF is using y-up while Genesis uses z-up. Please set parse_glb_with_zup=True"
+                        " in morph options if you find the mesh is 90-degree rotated. We will set parse_glb_with_zup=True"
+                        " and rotate glb mesh by default later and gradually enforce this option."
+                    )
                 if morph.parse_glb_with_trimesh:
                     meshes = mu.parse_mesh_trimesh(morph.file, morph.group_by_material, morph.scale, surface)
+                    if morph.parse_glb_with_zup:
+                        for mesh in meshes:
+                            mesh.apply_transform(mu.Y_UP_TRANSFORM.T)
                 else:
-                    meshes = gltf_utils.parse_mesh_glb(morph.file, morph.group_by_material, morph.scale, surface)
+                    meshes = gltf_utils.parse_mesh_glb(
+                        morph.file, morph.group_by_material, morph.scale, surface, morph.parse_glb_with_zup
+                    )
             elif morph.is_format(gs.options.morphs.USD_FORMATS):
                 import genesis.utils.usda as usda_utils
 
@@ -395,7 +406,7 @@ def update_trimesh_visual(self):
 
     def apply_transform(self, T):
         """
-        Apply a 4x4 transformation matrix to the mesh.
+        Apply a 4x4 transformation matrix (translation on the right column) to the mesh.
         """
         self._mesh.apply_transform(T)
 

genesis/ext/urdfpy/utils.py

@@ -244,10 +244,29 @@ def load_meshes(filename):
     """
     meshes = trimesh.load(filename, process=False)
 
-    # If we got a scene, dump the meshes
     if isinstance(meshes, trimesh.Scene):
-        meshes = list(meshes.dump())
-        meshes = [g for g in meshes if isinstance(g, trimesh.Trimesh)]
+        T = np.array([
+            [ 1.,  0.,  0.,  0.],
+            [ 0.,  0., -1.,  0.],
+            [ 0.,  1.,  0.,  0.],
+            [ 0.,  0.,  0.,  1.]], dtype=np.float32,
+        )
+        # FIXME: Scene.dump() has bug that uses copy without include_cache=True,
+        #  it will lose the vertex normals.
+        results = []
+        is_glb = filename.endswith((".gltf", ".glb"))
+        for node_name in meshes.graph.nodes_geometry:
+            transform, geometry_name = meshes.graph[node_name]
+            current = meshes.geometry[geometry_name].copy(include_cache=True)
+            if isinstance(current, trimesh.Trimesh):
+                if is_glb:
+                    current.apply_transform(T @ transform)
+                else:
+                    current.apply_transform(transform)
+                current.metadata["name"] = geometry_name
+                current.metadata["node"] = node_name
+                results.append(current)
+        meshes = results
 
     if isinstance(meshes, (list, tuple, set)):
         meshes = list(meshes)

genesis/options/morphs.py

@@ -642,6 +642,8 @@ class Mesh(FileMorph, TetGenMixin):
         **This is only used for RigidEntity.**
     parse_glb_with_trimesh : bool, optional
         Whether to use trimesh to load glb files. Defaults to False, in which case pygltflib will be used.
+    parse_glb_with_zup : bool, optional
+        Whether to use zup to load glb files. Defaults to False.
     fixed : bool, optional
         Whether the baselink of the entity should be fixed. Defaults to False. **This is only used for RigidEntity.**
     contype : int, optional
@@ -679,6 +681,7 @@ class Mesh(FileMorph, TetGenMixin):
     """
 
     parse_glb_with_trimesh: bool = False
+    parse_glb_with_zup: bool = False
 
     # Rigid specific
     fixed: bool = False

genesis/utils/gltf.py

@@ -290,7 +290,7 @@ def parse_glb_tree(glb, node_index):
     return mesh_list
 
 
-def parse_mesh_glb(path, group_by_material, scale, surface):
+def parse_mesh_glb(path, group_by_material, scale, surface, convert_zup=False):
     glb = pygltflib.GLTF2().load(path)
     assert glb is not None
     glb.convert_images(pygltflib.ImageFormat.DATAURI)
@@ -383,6 +383,8 @@ def parse_mesh_glb(path, group_by_material, scale, surface):
                     if primitive.attributes.TEXCOORD_1:
                         uvs = get_glb_data_from_accessor(glb, primitive.attributes.TEXCOORD_1).astype(np.float32)
 
+            if convert_zup:
+                mesh_transform @= mu.Y_UP_TRANSFORM
             points, normals = mu.apply_transform(mesh_transform, points, normals)
             if normals is None:
                 normals = trimesh.Trimesh(points, triangles, process=False).vertex_normals

genesis/utils/mesh.py

@@ -35,6 +35,9 @@
 
 MESH_REPAIR_ERROR_THRESHOLD = 0.01
 CVX_PATH_QUANTIZE_FACTOR = 1e-6
+Y_UP_TRANSFORM = np.asarray(  # translation on the bottom row
+    [[1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, -1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]], dtype=np.float32
+)
 
 
 class MeshInfo:
@@ -505,6 +508,7 @@ def create_texture(image, factor, encoding):
 
 
 def apply_transform(transform, positions, normals=None):
+    # Note that here transform's translation is on the bottom row, different from that in Genesis and trimesh.
     transformed_positions = (np.column_stack([positions, np.ones(len(positions))]) @ transform)[:, :3]
 
     transformed_normals = normals

genesis/utils/usda.py

@@ -29,8 +29,6 @@
     "": None,
 }
 
-yup_rotation = ((1.0, 0.0, 0.0), (0.0, 0.0, 1.0), (0.0, -1.0, 0.0))
-
 
 def get_input_attribute_value(shader, input_name, input_type=None):
     shader_input = shader.GetInput(input_name)
@@ -362,7 +360,7 @@ def parse_mesh_usd(path, group_by_material, scale, surface, bake_cache=True):
         if prim.IsA(UsdGeom.Mesh):
             matrix = np.asarray(xform_cache.GetLocalToWorldTransform(prim), dtype=np.float32)
             if yup:
-                matrix[:3, :3] @= np.asarray(yup_rotation, dtype=np.float32)
+                matrix @= mu.Y_UP_TRANSFORM
             mesh_usd = UsdGeom.Mesh(prim)
             mesh_spec = prim.GetPrimStack()[-1]
             mesh_id = mesh_spec.layer.identifier + mesh_spec.path.pathString

tests/test_mesh.py

@@ -353,13 +353,14 @@ def test_usd_bake(usd_file, show_viewer):
 def test_urdf_with_existing_glb(tmp_path, show_viewer):
     glb_file = "usd/sneaker_airforce.glb"
     asset_path = get_hf_dataset(pattern=glb_file)
+    glb_path = os.path.join(asset_path, glb_file)
 
     urdf_path = tmp_path / "model.urdf"
     urdf_path.write_text(
         f"""<robot name="shoe">
               <link name="base">
                 <visual>
-                  <geometry><mesh filename="{os.path.join(asset_path, glb_file)}"/></geometry>
+                  <geometry><mesh filename="{glb_path}"/></geometry>
                 </visual>
               </link>
             </robot>
@@ -370,11 +371,18 @@ def test_urdf_with_existing_glb(tmp_path, show_viewer):
         show_viewer=show_viewer,
         show_FPS=False,
     )
-    robot = scene.add_entity(
+    robot_urdf = scene.add_entity(
         gs.morphs.URDF(
             file=urdf_path,
         ),
     )
+    robot_mesh = scene.add_entity(
+        gs.morphs.Mesh(
+            file=glb_path,
+            parse_glb_with_zup=True,
+        ),
+    )
+    check_gs_meshes(robot_urdf.vgeoms[0].vmesh, robot_mesh.vgeoms[0].vmesh, "robot")
 
 
 @pytest.mark.required