From 37e0a79829bfdce0be508baf8b67cf65d076f3f9 Mon Sep 17 00:00:00 2001
From: clear sky mio~ <84168988+clearsky-mio@users.noreply.github.com>
Date: Fri, 13 Dec 2024 11:14:20 +0800
Subject: [PATCH] Adds transform for mesh_prim of ray caster sensor (#1448)

# Description

<!--
Thank you for your interest in sending a pull request. Please make sure
to check the contribution guidelines.

Link: https://isaac-sim.github.io/IsaacLab/source/refs/contributing.html
-->

Currently, ray-saster does not apply transform to the mesh that is cast
against by ray.
As a result, no matter where the mesh is, the ray caster get data as
same as when the mesh is in the pos (0,0,0) with quat (1,0,0,0).

<!-- As a practice, it is recommended to open an issue to have
discussions on the proposed pull request.
This makes it easier for the community to keep track of what is being
developed or added, and if a given feature
is demanded by more than one party. -->

## Type of change

<!-- As you go through the list, delete the ones that are not
applicable. -->

- Bug fix (non-breaking change which fixes an issue)

## Screenshots

I use a red cube to mark the position of ray caster sensor, and use
LidarPattern to generate ray for more intuitive visual effects.

Before fixed, when the mesh is in the center with pos=(0.0, 0.0, 0.0),
rot=(1.0, 0.0, 0.0, 0.0), the result will be right.


![screenshot0](https://github.com/user-attachments/assets/b117e12d-ee94-452a-a223-5446f2586c29)

However, if pos=(1.0, -2.0, 0.0), rot=(0.816, 0.577, 0.0, 0.0), it
causes bug as:


![screenshot1](https://github.com/user-attachments/assets/1e3a2d52-201b-43da-ade1-19255b1bcfed)

After fixed, the result will be right:


![screenshot2](https://github.com/user-attachments/assets/76f757c4-fca7-4b2b-89e1-1da21eb4e0cd)

<!--
Example:

| Before | After |
| ------ | ----- |
| _gif/png before_ | _gif/png after_ |

To upload images to a PR -- simply drag and drop an image while in edit
mode and it should upload the image directly. You can then paste that
source into the above before/after sections.
-->

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.sh --format`
- [ ] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [ ] I have added tests that prove my fix is effective or that my
feature works
- [ ] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [ ] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there

<!--
As you go through the checklist above, you can mark something as done by
putting an x character in it

For example,
- [x] I have done this task
- [ ] I have not done this task
-->

Co-authored-by: Kelly Guo <kellyg@nvidia.com>
---
 .../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 ec25f88f24..04e019f6f9 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