Adds Raycaster with tracking for Dynamic Meshes

[renezurbruegg]

Description

This PR introduces MultiMeshRayCaster and MultiMeshRayCasterCamera, an extension of the default RayCaster with the following enhancements:

1. Raycasting against multiple target types : Supports primitive shapes (spheres, cubes, …) as well as arbitrary meshes.
2. Dynamic mesh tracking : Keeps track of specified meshes, enabling raycasting against moving parts (e.g., robot links, articulated bodies, or dynamic obstacles).
3. Memory-efficient caching : Avoids redundant memory usage by caching and reusing duplicate meshes.

This is joint work with @pascal-roth and @Mayankm96.

The default RayCaster was limited to static environments and required manual handling of moving meshes, which restricted its use for robotics scenarios where robots or obstacles move dynamically.

MultiMeshRayCaster addresses these limitations by and now supports raycasting against robot parts and other moving entities.

---

Usage

For a quick demo, run:

python scripts/demos/sensors/multi_mesh_raycaster.py --num_envs 16 --asset_type <allegro_hand|anymal_d|multi>

Drop-in replacement

Example change to migrate from RayCasterCfg to MultiMeshRayCasterCfg:

- ray_caster_cfg = RayCasterCfg(
+ ray_caster_cfg = MultiMeshRayCasterCfg(
      prim_path="{ENV_REGEX_NS}/Robot",
      mesh_prim_paths=[
         "/World/Ground",
+         MultiMeshRayCasterCfg.RaycastTargetCfg(target_prim_expr="{ENV_REGEX_NS}/Robot/LF_.*/visuals"),
+         MultiMeshRayCasterCfg.RaycastTargetCfg(target_prim_expr="{ENV_REGEX_NS}/Robot/RF_.*/visuals"),
+         MultiMeshRayCasterCfg.RaycastTargetCfg(target_prim_expr="{ENV_REGEX_NS}/Robot/LH_.*/visuals"),
+         MultiMeshRayCasterCfg.RaycastTargetCfg(target_prim_expr="{ENV_REGEX_NS}/Robot/RH_.*/visuals"),
+         MultiMeshRayCasterCfg.RaycastTargetCfg(target_prim_expr="{ENV_REGEX_NS}/Robot/base/visuals"),
      ],
      pattern_cfg=patterns.GridPatternCfg(resolution=resolution, size=(5.0, 5.0)),
 )

---

Benchmarking & Validation

To benchmark the new raycaster, run:

python scripts/benchmarks/benchmark_ray_caster.py

Then plot the results with:

python scripts/benchmarks/plot_raycast_results.py

This will generate outputs under:
outputs/benchmarks/raycast_benchmark...

Example plots

---

Type of Change

• New feature (non-breaking change which adds functionality)
• This change requires a documentation update

Checklist

• I have run the pre-commit checks with ./isaaclab.sh --format
• I have made corresponding changes to the documentation
• 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

[pavelacamposp]

Thanks for this feature! I've noticed that the duplicate mesh detection in MultiMeshRayCaster._initialize_warp_meshes() (from multi_mesh_ray_caster.py) doesn't scale well with the number of meshes.

Currently, _registered_points_idx() iterates over all registered meshes and compares their vertices. This operation is roughly O(n * V) for each new mesh (n = number of registered meshes, V = vertex count), leading to an overall O(n² * V) cost during initialization as the list of registered meshes grows (loaded_vertices).

A more efficient alternative could be to use a hash-based lookup table to detect mesh duplicates. We can assign a hash key to each mesh based on its vertices and use it for fast duplicate checks. This would reduce the overall complexity to O(n * V) for the entire duplicate mesh detection process.

Example implementation:

def _get_mesh_key(vertices: np.ndarray) -> tuple[int, int, bytes]:
    """Build a key from the shape and data hash of a mesh vertex array."""
    data = np.ascontiguousarray(vertices).view(np.uint8)  # Ensure array is contiguous
    h = hashlib.blake2b(data, digest_size=16)
    return (vertices.shape[0], vertices.shape[1], h.digest())

And in _initialize_warp_meshes():

def _initialize_warp_meshes(self):
    ...
    for target_cfg in self._raycast_targets_cfg:
        ...
        registered_meshes: dict[tuple[int, int, bytes], int] = {}  # Maps mesh keys to wp_mesh indices 
        wp_mesh_ids = []

        for target_prim in target_prims:
        	...
            if str(target_prim.GetPath()) in MultiMeshRayCaster.meshes:
                wp_mesh_ids.append(MultiMeshRayCaster.meshes[str(target_prim.GetPath())].id)
                continue
            ...
            mesh_key = _get_mesh_key(trimesh_mesh.vertices)
            registered_idx = registered_meshes.get(mesh_key, -1)
            if registered_idx != -1 and self.cfg.reference_meshes:
                omni.log.info("Found a duplicate mesh, only reference the mesh.")
                wp_mesh_ids.append(wp_mesh_ids[registered_idx])
            else:
                wp_mesh = convert_to_warp_mesh(trimesh_mesh.vertices, trimesh_mesh.faces, device=self.device)
                MultiMeshRayCaster.meshes[str(target_prim.GetPath())] = wp_mesh
                wp_mesh_ids.append(wp_mesh.id)
                registered_meshes[mesh_key] = len(wp_mesh_ids) - 1  # Store wp_mesh idx

This approach should reduce initialization time significantly when processing multiple meshes (from O(n² * V) to O(n * V)).

[renezurbruegg]

Thanks a lot for the valuable feedback. Hashing the vertices is a great idea to speed up retrieval. I’ll incorporate this, along with an additional check for potential hash collisions, once I find the time.

Also, note that cache lookups can be further accelerated by setting the is_shared flag in the configuration to true. This assumes that all environments share the same meshes, avoiding redundant checking for each one.

[Mayankm96]

This is definitely useful feedback @pavelacamposp

@renezurbruegg I suggest though that we keep this MR to the current limitation and make a separate one with the hashing implementation. Just to not have this MR hanging around for too long :)

[renezurbruegg]

def create_primitive_mesh(prim) -> trimesh.Trimesh:
    prim_type = prim.GetTypeName()
    if prim_type == "Cube":
        size = UsdGeom.Cube(prim).GetSizeAttr().Get()
        return trimesh.creation.box(extents=(size, size, size))
    elif prim_type == "Sphere":
        r = UsdGeom.Sphere(prim).GetRadiusAttr().Get()
        return trimesh.creation.icosphere(subdivisions=3, radius=r)
    elif prim_type == "Cylinder":
        c = UsdGeom.Cylinder(prim)
        return trimesh.creation.cylinder(radius=c.GetRadiusAttr().Get(), height=c.GetHeightAttr().Get())
    elif prim_type == "Capsule":
        c = UsdGeom.Capsule(prim)
        tri_mesh = trimesh.creation.capsule(radius=c.GetRadiusAttr().Get(), height=c.GetHeightAttr().Get())
        if c.GetAxisAttr().Get() == "X":
            # rotate −90° about Y to point the length along +X
            R = rotation_matrix(np.radians(-90), [0, 1, 0])
            tri_mesh.apply_transform(R)
        elif c.GetAxisAttr().Get() == "Y":
            # rotate +90° about X to point the length along +Y
            R = rotation_matrix(np.radians(90), [1, 0, 0])
            tri_mesh.apply_transform(R)
        return tri_mesh

    elif prim_type == "Cone":
        c = UsdGeom.Cone(prim)
        radius = c.GetRadiusAttr().Get()
        height = c.GetHeightAttr().Get()
        mesh = trimesh.creation.cone(radius=radius, height=height)
        # shift all vertices down by height/2 for usd / trimesh cone primitive definiton discrepancy
        mesh.apply_translation((0.0, 0.0, -height / 2.0))
        return mesh
    else:
        raise KeyError(f"{prim_type} is not a valid primitive mesh type")

@renezurbruegg @pascal-roth I also have something in my other code to do primitive to trimesh conversion, I see currently only Plane, Cube, Sphere for ray-caster, could you please add the Cone, Capsule, Cylinder as well? Be careful with the descrepancy how trimesh cone and capsule convention might be a bit different from that of USD. I have written out the convention conversion in the code snippet above and tested it worked

if you find this single function nice, I'd also just replace all collapse helpfer functions and constant to just single function.

in my dexsuite environments, this code is also used, https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/utils.py

once you guys are done here, I will refactor my dexsuite environment to use your guys utility : )))

Thanks a lot for the input. I added all prim types now and verified that they work correctly:

[renezurbruegg]

I also see that I have a same triangulate face utility https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/utils.py

def _triangulate_faces(prim) -> np.ndarray:
    mesh = UsdGeom.Mesh(prim)
    counts = mesh.GetFaceVertexCountsAttr().Get()
    indices = mesh.GetFaceVertexIndicesAttr().Get()
    faces = []
    it = iter(indices)
    for cnt in counts:
        poly = [next(it) for _ in range(cnt)]
        for k in range(1, cnt - 1):
            faces.append([poly[0], poly[k], poly[k + 1]])
    return np.asarray(faces, dtype=np.int64)

your:

def convert_faces_to_triangles(faces: np.ndarray, point_counts: np.ndarray) -> np.ndarray:
    # check if the mesh is already triangulated
    if (point_counts == 3).all():
        return faces.reshape(-1, 3)  # already triangulated
    all_faces = []

    vertex_counter = 0
    # Iterates over all triangles of the mesh.
    # could be very slow for large meshes
    for num_points in point_counts:
        if num_points == 3:
            # triangle
            all_faces.append(faces[vertex_counter : vertex_counter + 3])
        elif num_points == 4:
            # quads. Subdivide into two triangles
            f = faces[vertex_counter : vertex_counter + 4]
            first_triangle = f[:3]
            second_triangle = np.array([f[0], f[2], f[3]])
            all_faces.append(first_triangle)
            all_faces.append(second_triangle)
        else:
            raise RuntimeError(f"Invalid number of points per face: {num_points}")

        vertex_counter += num_points
    return np.asarray(all_faces)

I wonder maybe you want the code still to work beyond num_points > 4?

Thanks, Updated it with your generic fan triangulation