diff --git a/genesis/engine/mesh.py b/genesis/engine/mesh.py index 5efb255dd..7b31a5acc 100644 --- a/genesis/engine/mesh.py +++ b/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) diff --git a/genesis/ext/urdfpy/utils.py b/genesis/ext/urdfpy/utils.py index 8e28accdd..57227be29 100644 --- a/genesis/ext/urdfpy/utils.py +++ b/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) diff --git a/genesis/options/morphs.py b/genesis/options/morphs.py index 0b124c41b..dda93f34e 100644 --- a/genesis/options/morphs.py +++ b/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 diff --git a/genesis/utils/gltf.py b/genesis/utils/gltf.py index d4b9d9ffb..8030927f7 100644 --- a/genesis/utils/gltf.py +++ b/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 diff --git a/genesis/utils/mesh.py b/genesis/utils/mesh.py index d956775e2..11f365908 100644 --- a/genesis/utils/mesh.py +++ b/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 diff --git a/genesis/utils/usda.py b/genesis/utils/usda.py index 716a36428..554ec46e3 100644 --- a/genesis/utils/usda.py +++ b/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 diff --git a/tests/test_mesh.py b/tests/test_mesh.py index 63c13781a..336f42e0f 100644 --- a/tests/test_mesh.py +++ b/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""" - + @@ -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