From 83b11031fb03b5e5ad9c6f7ac9153338569220ee Mon Sep 17 00:00:00 2001
From: hao <2964491850@qq.com>
Date: Fri, 22 Nov 2024 13:59:12 +0800
Subject: [PATCH] apply transform for mesh_prim of ray caster sensor

---
 .../omni/isaac/lab/sensors/ray_caster/ray_caster.py            | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/ray_caster/ray_caster.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/ray_caster/ray_caster.py
index ec25f88f24e..04e019f6f90 100644
--- a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/ray_caster/ray_caster.py
+++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/ray_caster/ray_caster.py
@@ -175,6 +175,9 @@ def _initialize_warp_meshes(self):
                 mesh_prim = UsdGeom.Mesh(mesh_prim)
                 # read the vertices and faces
                 points = np.asarray(mesh_prim.GetPointsAttr().Get())
+                transform_matrix = np.array(omni.usd.get_world_transform_matrix(mesh_prim)).T
+                points = np.matmul(points, transform_matrix[:3, :3].T)
+                points += transform_matrix[:3, 3]
                 indices = np.asarray(mesh_prim.GetFaceVertexIndicesAttr().Get())
                 wp_mesh = convert_to_warp_mesh(points, indices, device=self.device)
                 # print info