add heterogeneous object tests

ooctipus · ooctipus · commit 14d3e5b28de1 · 2026-05-07T15:14:44.000-07:00
diff --git a/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py b/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py
@@ -16,6 +16,7 @@
 """Rest everything follows."""
 
 import copy
+from collections.abc import Callable
 
 import numpy as np
 import pytest
@@ -24,13 +25,18 @@
 import omni.replicator.core as rep
 from pxr import Gf
 
+import isaaclab.cloner as lab_cloner
 import isaaclab.sim as sim_utils
+from isaaclab.cloner import ClonePlan
 from isaaclab.sensors.camera import Camera, CameraCfg
 from isaaclab.sensors.ray_caster import MultiMeshRayCasterCamera, MultiMeshRayCasterCameraCfg, patterns
 from isaaclab.sim import PinholeCameraCfg
 from isaaclab.terrains.trimesh.utils import make_plane
 from isaaclab.terrains.utils import create_prim_from_mesh
 
+from isaaclab_assets.robots.anymal import ANYMAL_C_CFG
+from isaaclab_assets.robots.spot import SPOT_CFG
+
 # sample camera poses (quaternions in xyzw format)
 POSITION = [2.5, 2.5, 2.5]
 QUAT_ROS = [0.33985114, 0.82047325, -0.42470819, -0.17591989]
@@ -433,6 +439,174 @@ def test_output_equal_to_usdcamera(setup_simulation, data_types):
     del camera_usd, camera_warp
 
 
+@pytest.mark.isaacsim_ci
+def test_depth_output_equal_to_usd_camera_heterogeneous_scene(setup_simulation):
+    """Compare ray-caster and USD depth cameras in a heterogeneous cloned scene.
+
+    The scene contains 16 environments with alternating Spot / ANYmal-C robot
+    prototypes and alternating cube / sphere objects.  The ray-caster consumes
+    the same clone plan used to build the USD scene and should match the batched
+    USD camera's stable ``distance_to_image_plane`` pixels for every environment.
+    """
+    sim, dt, _ = setup_simulation
+    num_envs = 16
+    stage = sim_utils.get_current_stage()
+    env_fmt = "/World/envs/env_{}"
+    env_ids = torch.arange(num_envs, dtype=torch.long, device=sim.device)
+    env_origins, _ = lab_cloner.grid_transforms(num_envs, spacing=4.0, device=sim.device)
+
+    sim_utils.create_prim("/World/envs", "Xform", stage=stage)
+    for env_id, origin in enumerate(env_origins.cpu().tolist()):
+        sim_utils.create_prim(env_fmt.format(env_id), "Xform", translation=tuple(origin), stage=stage)
+
+    # Prototype rows: even environments use Spot + cube, odd environments use ANYmal-C + sphere.
+    robot_mask = torch.zeros((2, num_envs), dtype=torch.bool, device=sim.device)
+    robot_mask[0, 0::2] = True
+    robot_mask[1, 1::2] = True
+    object_mask = robot_mask.clone()
+
+    spot_spawn = copy.deepcopy(SPOT_CFG.spawn)
+    anymal_spawn = copy.deepcopy(ANYMAL_C_CFG.spawn)
+    spot_spawn.func(env_fmt.format(0) + "/Robot", spot_spawn, translation=SPOT_CFG.init_state.pos)
+    anymal_spawn.func(env_fmt.format(1) + "/Robot", anymal_spawn, translation=ANYMAL_C_CFG.init_state.pos)
+
+    cube_cfg = sim_utils.CuboidCfg(
+        size=(0.35, 0.25, 0.25),
+        visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.7, 0.2, 0.2)),
+    )
+    sphere_cfg = sim_utils.SphereCfg(
+        radius=0.18,
+        visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.2, 0.2, 0.7)),
+    )
+    cube_spawn = cube_cfg.func
+    sphere_spawn = sphere_cfg.func
+    assert isinstance(cube_spawn, Callable)
+    assert isinstance(sphere_spawn, Callable)
+    cube_spawn(env_fmt.format(0) + "/Object", cube_cfg, translation=(0.45, 0.0, 0.25))
+    sphere_spawn(env_fmt.format(1) + "/Object", sphere_cfg, translation=(0.45, 0.0, 0.25))
+
+    lab_cloner.usd_replicate(
+        stage,
+        [env_fmt.format(i) + f"/{asset_name}" for asset_name in ("Robot", "Object") for i in range(2)],
+        [env_fmt + "/Robot", env_fmt + "/Robot", env_fmt + "/Object", env_fmt + "/Object"],
+        env_ids,
+        mask=torch.cat([robot_mask, object_mask], dim=0),
+    )
+    sim.set_clone_plan(
+        ClonePlan(
+            sources=[
+                env_fmt.format(0) + "/Robot",
+                env_fmt.format(1) + "/Robot",
+                env_fmt.format(0) + "/Object",
+                env_fmt.format(1) + "/Object",
+            ],
+            destinations=[
+                env_fmt + "/Robot",
+                env_fmt + "/Robot",
+                env_fmt + "/Object",
+                env_fmt + "/Object",
+            ],
+            clone_mask=torch.cat([robot_mask, object_mask], dim=0),
+        )
+    )
+    sim_utils.update_stage()
+
+    height, width = 96, 128
+    camera_pattern_cfg = patterns.PinholeCameraPatternCfg(
+        focal_length=24.0,
+        horizontal_aperture=20.955,
+        height=height,
+        width=width,
+    )
+    mesh_prim_paths = [
+        "/World/defaultGroundPlane",
+        MultiMeshRayCasterCameraCfg.RaycastTargetCfg(
+            prim_expr="/World/envs/env_.*/Object",
+            track_mesh_transforms=False,
+        ),
+        MultiMeshRayCasterCameraCfg.RaycastTargetCfg(
+            prim_expr="/World/envs/env_.*/Robot/.+",
+            track_mesh_transforms=True,
+        ),
+    ]
+    camera_cfg_warp = MultiMeshRayCasterCameraCfg(
+        prim_path="/World/envs/env_.*/RayCasterCamera",
+        mesh_prim_paths=mesh_prim_paths,
+        update_period=0,
+        debug_vis=False,
+        pattern_cfg=camera_pattern_cfg,
+        max_distance=25.0,
+        data_types=["distance_to_image_plane"],
+        depth_clipping_behavior="max",
+        update_mesh_ids=True,
+    )
+    camera_warp = MultiMeshRayCasterCamera(camera_cfg_warp)
+
+    camera_cfg_usd = CameraCfg(
+        height=height,
+        width=width,
+        prim_path="/World/envs/env_.*/UsdCamera",
+        update_period=0,
+        data_types=["distance_to_image_plane"],
+        spawn=PinholeCameraCfg(
+            focal_length=24.0,
+            focus_distance=400.0,
+            horizontal_aperture=20.955,
+            clipping_range=(0.01, 25.0),
+        ),
+    )
+    camera_usd = Camera(camera_cfg_usd)
+
+    sim.reset()
+    sim.play()
+
+    eyes = env_origins + torch.tensor((1.8, -2.5, 2.5), dtype=torch.float32, device=sim.device)
+    targets = env_origins + torch.tensor((0.0, 0.0, 0.0), dtype=torch.float32, device=sim.device)
+    camera_warp.set_world_poses_from_view(eyes=eyes, targets=targets)
+    camera_usd.set_world_poses_from_view(eyes=eyes, targets=targets)
+
+    for _ in range(5):
+        sim.render()
+
+    camera_usd.update(dt)
+    camera_warp.update(dt)
+
+    ray_depth = camera_warp.data.output["distance_to_image_plane"]
+    usd_depth = camera_usd.data.output["distance_to_image_plane"]
+    assert ray_depth.shape == (num_envs, height, width, 1)
+    assert usd_depth.shape == ray_depth.shape
+    depth_diff = (ray_depth - usd_depth).abs()
+    mesh_ids = getattr(camera_warp.data, "image_mesh_ids", None)
+    assert mesh_ids is not None
+    assert torch.any(mesh_ids == 1), "Expected object pixels in the heterogeneous scene"
+    assert torch.any(mesh_ids > 1), "Expected robot pixels in the heterogeneous scene"
+
+    # The RTX and ray-cast backends can disagree by a pixel along complex robot
+    # silhouettes.  Compare the stable ground pixels after dilating object/robot
+    # edges and depth discontinuities.
+    target_mask = mesh_ids[..., 0] != 0
+    discontinuity_mask = torch.zeros_like(target_mask)
+    for depth in (ray_depth, usd_depth):
+        depth_image = depth[..., 0]
+        discontinuity_mask[:, 1:, :] |= (depth_image[:, 1:, :] - depth_image[:, :-1, :]).abs() > 0.3
+        discontinuity_mask[:, :, 1:] |= (depth_image[:, :, 1:] - depth_image[:, :, :-1]).abs() > 0.3
+    edge_mask = target_mask | discontinuity_mask
+    silhouette_mask = torch.nn.functional.max_pool2d(
+        edge_mask[:, None, :, :].float(), kernel_size=21, stride=1, padding=10
+    ).to(dtype=torch.bool)
+    stable_mask = ~silhouette_mask[:, 0, :, :, None]
+    assert stable_mask.float().mean() > 0.7
+    stable_ray_depth = ray_depth[stable_mask]
+    stable_usd_depth = usd_depth[stable_mask]
+    stable_depth_diff = depth_diff[stable_mask]
+    stable_close = torch.isclose(stable_ray_depth, stable_usd_depth, atol=5e-5, rtol=5e-6)
+    assert stable_close.float().mean() > 0.999
+    assert torch.quantile(stable_depth_diff, 0.999) < 5.0e-5
+    assert torch.quantile(depth_diff, 0.99) < 5.0e-3
+
+    del camera_usd, camera_warp
+
+
 @pytest.mark.isaacsim_ci
 def test_output_equal_to_usdcamera_offset(setup_simulation):
     """Test that ray caster camera output equals USD camera output with offset."""
diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/ray_caster/ray_caster.py b/source/isaaclab_newton/isaaclab_newton/sensors/ray_caster/ray_caster.py
@@ -55,8 +55,8 @@ def _find_physics_ancestor(prim):
 
 
 def _newton_body_pattern(body_path: str) -> str:
-    """Strip a concrete env prefix so Newton can register a cloned body pattern."""
-    return re.sub(r"^/World/envs/env_\d+/", "", body_path)
+    """Convert a concrete env index to a regex wildcard for prototype body matching."""
+    return re.sub(r"^(/World/envs/)env_\d+/", r"\1env_.*/", body_path)
 
 
 def _xform_from_pose(pos, quat) -> wp.transform:
diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/ray_caster/ray_caster.py b/source/isaaclab_physx/isaaclab_physx/sensors/ray_caster/ray_caster.py
@@ -137,17 +137,19 @@ def _create_tracked_target_view(self: Any, target_prim_path: str):
         prims = sim_utils.find_matching_prims(target_prim_path)
         if len(prims) == 0:
             raise RuntimeError(f"No tracked target prims matched: {target_prim_path}")
-        body = _find_physics_ancestor(prims[0])
-        if body is None:
-            raise RuntimeError(
-                f"Cannot track non-physics ray-cast target '{target_prim_path}' with PhysX. "
-                "Set track_mesh_transforms=False for static targets, or apply RigidBodyAPI to dynamic targets."
-            )
-        body_expr = _body_expr_from_sensor_expr(target_prim_path, prims[0], body)
+        body_paths = []
+        for prim in prims:
+            body = _find_physics_ancestor(prim)
+            if body is None:
+                raise RuntimeError(
+                    f"Cannot track non-physics ray-cast target '{target_prim_path}' with PhysX. "
+                    "Set track_mesh_transforms=False for static targets, or apply RigidBodyAPI to dynamic targets."
+                )
+            body_paths.append(body.GetPath().pathString)
         physics_sim_view = PhysxManager.get_physics_sim_view()
         if physics_sim_view is None:
             raise RuntimeError("PhysX simulation view is not initialized.")
-        return physics_sim_view.create_rigid_body_view(body_expr.replace(".*", "*"))
+        return physics_sim_view.create_rigid_body_view(body_paths)
 
     def _update_mesh_transforms(self: Any) -> None:
         """Refresh dynamic multi-mesh targets directly from PhysX views."""
diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/camera_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/camera_cfg.py
@@ -9,7 +9,7 @@
 from isaaclab.managers import ObservationGroupCfg as ObsGroup
 from isaaclab.managers import ObservationTermCfg as ObsTerm
 from isaaclab.managers import SceneEntityCfg
-from isaaclab.sensors import CameraCfg
+from isaaclab.sensors import CameraCfg, MultiMeshRayCasterCameraCfg, patterns
 from isaaclab.utils import configclass
 from isaaclab.utils.noise import UniformNoiseCfg as Unoise
 
@@ -50,6 +50,54 @@
     renderer_cfg=MultiBackendRendererCfg(),
 )
 
+RAY_PATTERN = patterns.PinholeCameraPatternCfg(focal_length=24.0, horizontal_aperture=20.955)
+
+RAYCASTER_CAMERA_MESH_PRIM_PATHS = [
+    MultiMeshRayCasterCameraCfg.RaycastTargetCfg(
+        prim_expr="/World/envs/env_.*/table",
+        track_mesh_transforms=False,
+    ),
+    MultiMeshRayCasterCameraCfg.RaycastTargetCfg(
+        prim_expr="/World/GroundPlane",
+        track_mesh_transforms=False,
+    ),
+    MultiMeshRayCasterCameraCfg.RaycastTargetCfg(
+        prim_expr="/World/envs/env_.*/Object",
+        track_mesh_transforms=True,
+    ),
+    MultiMeshRayCasterCameraCfg.RaycastTargetCfg(
+        prim_expr="/World/envs/env_.*/Robot/.*/visuals",
+        track_mesh_transforms=True,
+    ),
+]
+
+BASE_RAYCASTER_CAMERA_CFG = MultiMeshRayCasterCameraCfg(
+    prim_path="/World/envs/env_.*/Camera",
+    offset=MultiMeshRayCasterCameraCfg.OffsetCfg(
+        pos=(0.57, -0.8, 0.5),
+        rot=(0.6124, 0.3536, 0.3536, 0.6124),
+        convention="opengl",
+    ),
+    mesh_prim_paths=RAYCASTER_CAMERA_MESH_PRIM_PATHS,
+    max_distance=2.5,
+    data_types=["distance_to_image_plane"],
+    pattern_cfg=MISSING,
+)
+
+WRIST_RAYCASTER_CAMERA_CFG = MultiMeshRayCasterCameraCfg(
+    prim_path="/World/envs/env_.*/Robot/ee_link/palm_link/Camera",
+    offset=MultiMeshRayCasterCameraCfg.OffsetCfg(
+        pos=(0.038, -0.38, -0.18),
+        rot=(0.641, 0.641, -0.299, 0.299),
+        convention="opengl",
+    ),
+    mesh_prim_paths=RAYCASTER_CAMERA_MESH_PRIM_PATHS,
+    max_distance=2.5,
+    data_types=["distance_to_image_plane"],
+    pattern_cfg=MISSING,
+    debug_vis=True,
+)
+
 
 @configclass
 class BaseTiledCameraCfg(PresetCfg):
@@ -88,6 +136,10 @@ class BaseTiledCameraCfg(PresetCfg):
     semantic_segmentation64 = BASE_CAMERA_CFG.replace(data_types=["semantic_segmentation"], width=64, height=64)
     semantic_segmentation128 = BASE_CAMERA_CFG.replace(data_types=["semantic_segmentation"], width=128, height=128)
     semantic_segmentation256 = BASE_CAMERA_CFG.replace(data_types=["semantic_segmentation"], width=256, height=256)
+    # raycaster camera presets
+    raycaster_depth64 = BASE_RAYCASTER_CAMERA_CFG.replace(pattern_cfg=RAY_PATTERN.replace(width=64, height=64))
+    raycaster_depth128 = BASE_RAYCASTER_CAMERA_CFG.replace(pattern_cfg=RAY_PATTERN.replace(width=128, height=128))
+    raycaster_depth256 = BASE_RAYCASTER_CAMERA_CFG.replace(pattern_cfg=RAY_PATTERN.replace(width=256, height=256))
     default = rgb64
 
 
@@ -128,6 +180,10 @@ class WristTiledCameraCfg(PresetCfg):
     semantic_segmentation64 = WRIST_CAMERA_CFG.replace(data_types=["semantic_segmentation"], width=64, height=64)
     semantic_segmentation128 = WRIST_CAMERA_CFG.replace(data_types=["semantic_segmentation"], width=128, height=128)
     semantic_segmentation256 = WRIST_CAMERA_CFG.replace(data_types=["semantic_segmentation"], width=256, height=256)
+    # raycaster camera presets
+    raycaster_depth64 = WRIST_RAYCASTER_CAMERA_CFG.replace(pattern_cfg=RAY_PATTERN.replace(width=64, height=64))
+    raycaster_depth128 = WRIST_RAYCASTER_CAMERA_CFG.replace(pattern_cfg=RAY_PATTERN.replace(width=128, height=128))
+    raycaster_depth256 = WRIST_RAYCASTER_CAMERA_CFG.replace(pattern_cfg=RAY_PATTERN.replace(width=256, height=256))
     default = rgb64
 
 
diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py
@@ -442,7 +442,7 @@ def validate_config(self):
             cam = getattr(self.scene, cam_attr, None)
             if cam is None:
                 continue
-            renderer_type = getattr(cam.renderer_cfg, "renderer_type", None)
+            renderer_type = getattr(getattr(cam, "renderer_cfg", None), "renderer_type", None)
             if renderer_type == "newton_warp":
                 unsupported = set(cam.data_types) - warp_supported
                 if unsupported:
