From fa222925273e10a7973c567516482e20615a881f Mon Sep 17 00:00:00 2001 From: Pascal Roth Date: Fri, 28 Mar 2025 16:29:16 +0100 Subject: [PATCH 1/5] move meshes to classvar --- .../isaaclab/sensors/ray_caster/ray_caster.py | 27 ++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py index 4e39b04e16b..43bfcaad386 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -9,7 +9,7 @@ import re import torch from collections.abc import Sequence -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, ClassVar import omni.log import omni.physics.tensors.impl.api as physx @@ -49,12 +49,25 @@ class RayCaster(SensorBase): cfg: RayCasterCfg """The configuration parameters.""" + # Class variables to share meshes and mesh_views across instances + meshes: ClassVar[dict[str, wp.Mesh]] = {} + """A dictionary to store warp meshes for raycasting, shared across all instances. + + The keys correspond to the prim path for the meshes, and values are the corresponding warp Mesh objects.""" + mesh_views: ClassVar[dict[str, XFormPrim | physx.ArticulationView | physx.RigidBodyView]] = {} + """A dictionary to store mesh views for raycasting, shared across all instances. + + The keys correspond to the prim path for the mesh views, and values are the corresponding view objects.""" + _instance_count: ClassVar[int] = 0 + """A counter to track the number of RayCaster instances, used to manage class variable lifecycle.""" + def __init__(self, cfg: RayCasterCfg): """Initializes the ray-caster object. Args: cfg: The configuration parameters. """ + RayCaster._instance_count += 1 # check if sensor path is valid # note: currently we do not handle environment indices if there is a regex pattern in the leaf # For example, if the prim path is "/World/Sensor_[1,2]". @@ -69,8 +82,6 @@ def __init__(self, cfg: RayCasterCfg): super().__init__(cfg) # Create empty variables for storing output data self._data = RayCasterData() - # the warp meshes used for raycasting. - self.meshes: dict[str, wp.Mesh] = {} def __str__(self) -> str: """Returns: A string containing information about the instance.""" @@ -157,6 +168,10 @@ def _initialize_warp_meshes(self): # read prims to ray-cast for mesh_prim_path in self.cfg.mesh_prim_paths: + # check if mesh already casted into warp mesh + if mesh_prim_path in RayCaster.meshes: + continue + # check if the prim is a plane - handle PhysX plane as a special case # if a plane exists then we need to create an infinite mesh that is a plane mesh_prim = sim_utils.get_first_matching_child_prim( @@ -289,3 +304,9 @@ def _invalidate_initialize_callback(self, event): # set all existing views to None to invalidate them self._physics_sim_view = None self._view = None + + def __del__(self): + RayCaster._instance_count -= 1 + if RayCaster._instance_count == 0: + RayCaster.meshes.clear() + RayCaster.mesh_views.clear() From 58d344e1d381fe81791ff319d069a05d044f0857 Mon Sep 17 00:00:00 2001 From: Pascal Roth <57946385+pascal-roth@users.noreply.github.com> Date: Mon, 31 Mar 2025 17:10:17 +0200 Subject: [PATCH 2/5] Update source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py Co-authored-by: Mayank Mittal <12863862+Mayankm96@users.noreply.github.com> Signed-off-by: Pascal Roth <57946385+pascal-roth@users.noreply.github.com> --- source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py index 43bfcaad386..f6304485da9 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -53,11 +53,15 @@ class RayCaster(SensorBase): meshes: ClassVar[dict[str, wp.Mesh]] = {} """A dictionary to store warp meshes for raycasting, shared across all instances. - The keys correspond to the prim path for the meshes, and values are the corresponding warp Mesh objects.""" + The keys correspond to the prim path for the meshes, and values are the corresponding warp Mesh objects. + """ + mesh_views: ClassVar[dict[str, XFormPrim | physx.ArticulationView | physx.RigidBodyView]] = {} """A dictionary to store mesh views for raycasting, shared across all instances. - The keys correspond to the prim path for the mesh views, and values are the corresponding view objects.""" + The keys correspond to the prim path for the mesh views, and values are the corresponding view objects. + """ + _instance_count: ClassVar[int] = 0 """A counter to track the number of RayCaster instances, used to manage class variable lifecycle.""" From 971f44918234eb8a741e9b68af3fe055bd0c0acd Mon Sep 17 00:00:00 2001 From: Pascal Roth Date: Mon, 31 Mar 2025 18:19:46 +0200 Subject: [PATCH 3/5] fix --- source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py index f6304485da9..252a389be16 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -71,7 +71,6 @@ def __init__(self, cfg: RayCasterCfg): Args: cfg: The configuration parameters. """ - RayCaster._instance_count += 1 # check if sensor path is valid # note: currently we do not handle environment indices if there is a regex pattern in the leaf # For example, if the prim path is "/World/Sensor_[1,2]". @@ -87,6 +86,9 @@ def __init__(self, cfg: RayCasterCfg): # Create empty variables for storing output data self._data = RayCasterData() + # increment the instance count + RayCaster._instance_count += 1 + def __str__(self) -> str: """Returns: A string containing information about the instance.""" return ( From 4e8385074834c86db7eccda502ab695aa5fe395b Mon Sep 17 00:00:00 2001 From: Pascal Roth Date: Mon, 7 Apr 2025 13:20:33 +0200 Subject: [PATCH 4/5] start to add test --- .../isaaclab/sensors/ray_caster/ray_caster.py | 9 +- .../isaaclab/test/sensors/test_ray_caster.py | 323 ++++++++++++++++++ 2 files changed, 324 insertions(+), 8 deletions(-) create mode 100644 source/isaaclab/test/sensors/test_ray_caster.py diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py index 252a389be16..8843ff668c2 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -49,19 +49,13 @@ class RayCaster(SensorBase): cfg: RayCasterCfg """The configuration parameters.""" - # Class variables to share meshes and mesh_views across instances + # Class variables to share meshes across instances meshes: ClassVar[dict[str, wp.Mesh]] = {} """A dictionary to store warp meshes for raycasting, shared across all instances. The keys correspond to the prim path for the meshes, and values are the corresponding warp Mesh objects. """ - mesh_views: ClassVar[dict[str, XFormPrim | physx.ArticulationView | physx.RigidBodyView]] = {} - """A dictionary to store mesh views for raycasting, shared across all instances. - - The keys correspond to the prim path for the mesh views, and values are the corresponding view objects. - """ - _instance_count: ClassVar[int] = 0 """A counter to track the number of RayCaster instances, used to manage class variable lifecycle.""" @@ -315,4 +309,3 @@ def __del__(self): RayCaster._instance_count -= 1 if RayCaster._instance_count == 0: RayCaster.meshes.clear() - RayCaster.mesh_views.clear() diff --git a/source/isaaclab/test/sensors/test_ray_caster.py b/source/isaaclab/test/sensors/test_ray_caster.py new file mode 100644 index 00000000000..25e68b7f447 --- /dev/null +++ b/source/isaaclab/test/sensors/test_ray_caster.py @@ -0,0 +1,323 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +# if not AppLauncher.instance(): +# app_launcher = AppLauncher(headless=True, enable_cameras=True) +# simulation_app = app_launcher.app +# elif AppLauncher.instance() and AppLauncher.instance()._enable_cameras is False: +# # FIXME: workaround as AppLauncher instance can currently not be closed without terminating the test +# raise ValueError("AppLauncher instance exists but enable_cameras is False") +# else: +# app_launcher = AppLauncher.instance() +# simulation_app = app_launcher.app + +app_launcher = AppLauncher(headless=False) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaacsim.core.utils.stage as stage_utils +import pytest +import torch + +import isaaclab.sim as sim_utils +from isaaclab.assets import RigidObjectCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.sensors import RayCaster, RayCasterCfg, patterns +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils import configclass +import isaaclab.terrains as terrain_gen + +## +# Pre-defined configs +## +from isaaclab_assets.robots.anymal import ANYMAL_C_CFG # isort: skip + +NUM_ENVS = 2 +# sample camera poses +POSITION = [2.5, 2.5, 2.5] +QUAT_WORLD = [-0.3647052, -0.27984815, -0.1159169, 0.88047623] + + + +BOX_TERRAIN_CFG = terrain_gen.TerrainGeneratorCfg( + size=(8.0, 8.0), + border_width=10.0, + num_rows=1, + num_cols=1, + horizontal_scale=0.1, + vertical_scale=0.005, + slope_threshold=0.75, + use_cache=True, + sub_terrains={ + "boxes": terrain_gen.MeshBoxTerrainCfg( + box_height_range=(1.0, 1.0) + ), + }, +) +"""Box terrains configuration.""" + + +@configclass +class MySceneCfg(InteractiveSceneCfg): + """Example scene configuration.""" + + # terrain - flat terrain plane + terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="generator", + terrain_generator=BOX_TERRAIN_CFG, + max_init_terrain_level=None, + ) + + # rigid objects - balls + balls = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/ball", + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.5)), + spawn=sim_utils.SphereCfg( + radius=0.25, + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=0.5), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0)), + ), + ) + + # articulations - robot + robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/robot") + + # sensors + ray_caster_articulation: RayCasterCfg = RayCasterCfg( + prim_path="{ENV_REGEX_NS}/robot/base", + mesh_prim_paths=["/World/ground"], + update_period=0, + debug_vis=False, + pattern_cfg=patterns.GridPatternCfg( + resolution=0.1, + size=(1.0, 1.0), + ), + attach_yaw_only=True, + ) + ray_caster_rigid_object: RayCasterCfg = RayCasterCfg( + prim_path="{ENV_REGEX_NS}/ball", + mesh_prim_paths=["/World/ground"], + update_period=0, + debug_vis=False, + pattern_cfg=patterns.GridPatternCfg( + resolution=0.1, + size=(1.0, 1.0), + ), + attach_yaw_only=True, + ) + ray_caster_xform: RayCasterCfg = RayCasterCfg( + prim_path="{ENV_REGEX_NS}/robot", + mesh_prim_paths=["/World/ground"], + update_period=0, + debug_vis=False, + pattern_cfg=patterns.GridPatternCfg( + resolution=0.1, + size=(1.0, 1.0), + ), + attach_yaw_only=True, + ) + +@pytest.fixture +def setup_sim(): + """Create a simulation context and scene.""" + # Create a new stage + stage_utils.create_new_stage() + # Load simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01) + sim = sim_utils.SimulationContext(sim_cfg) + # construct scene + scene_cfg = MySceneCfg(num_envs=NUM_ENVS, env_spacing=5.0, lazy_sensor_update=False) + yield sim, scene_cfg + # Cleanup + sim.clear_all_callbacks() + sim.clear_instance() + # Make sure that all mesh instances are resolved + assert RayCaster.meshes == {} + assert RayCaster._instance_count == 0 + +def test_ray_caster_init_articulation(setup_sim): + sim, scene_cfg = setup_sim + + # pop other than the articulation raycaster + scene_cfg.ray_caster_rigid_object = None + scene_cfg.ray_caster_xform = None + + # create scene and reset sim + scene = InteractiveScene(scene_cfg) + sim.reset() + + # Check that raycasters are initialized + assert scene.sensors["ray_caster_articulation"].is_initialized + + # check correct initialization of meshes, and instances + assert len(RayCaster.meshes) == 1 + assert RayCaster._instance_count == 1 + + # check correct prim paths + assert scene.sensors["ray_caster_articulation"]._view.prim_paths[0] == "/World/envs/env_0/robot/base" + + # check that buffers exists and have the expected shapes + assert scene.sensors["ray_caster_articulation"].data.pos_w.shape == (NUM_ENVS, 3) + assert scene.sensors["ray_caster_articulation"].data.quat_w.shape == (NUM_ENVS, 4) + num_rays = (scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.size[0] / scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.resolution + 1) * (scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.size[1] / scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.resolution + 1) + assert scene.sensors["ray_caster_articulation"].data.ray_hits_w.shape == (NUM_ENVS, num_rays, 3) + + # check placement equal to articulation + torch.testing.assert_close( + scene.articulations["robot"].data.root_pos_w, + scene.sensors["ray_caster_articulation"].data.pos_w, + atol=1e-5, + rtol=1e-3, + ) + torch.testing.assert_close( + scene.articulations["robot"].data.root_quat_w, + scene.sensors["ray_caster_articulation"].data.quat_w, + atol=1e-5, + rtol=1e-3, + ) + +def test_ray_caster_init_rigid_object(setup_sim): + sim, scene_cfg = setup_sim + + # pop other than the rigid object raycaster + scene_cfg.ray_caster_articulation = None + scene_cfg.ray_caster_xform = None + + # create scene and reset sim + scene = InteractiveScene(scene_cfg) + sim.reset() + + # Check that raycasters are initialized + assert scene.sensors["ray_caster_rigid_object"].is_initialized + + # check correct initialization of meshes, and instances + assert len(RayCaster.meshes) == 1 + assert RayCaster._instance_count == 1 + + # check correct prim paths + assert scene.sensors["ray_caster_rigid_object"]._view.prim_paths[0] == "/World/envs/env_0/ball" + + # check that buffers exists and have the expected shapes + assert scene.sensors["ray_caster_rigid_object"].data.pos_w.shape == (NUM_ENVS, 3) + assert scene.sensors["ray_caster_rigid_object"].data.quat_w.shape == (NUM_ENVS, 4) + num_rays = (scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.size[0] / scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.resolution + 1) * (scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.size[1] / scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.resolution + 1) + assert scene.sensors["ray_caster_rigid_object"].data.ray_hits_w.shape == (NUM_ENVS, num_rays, 3) + + # check placement equal to rigid object + torch.testing.assert_close( + scene.rigid_objects["balls"].data.root_pos_w, + scene.sensors["ray_caster_rigid_object"].data.pos_w, + atol=1e-5, + rtol=1e-3, + ) + torch.testing.assert_close( + scene.rigid_objects["balls"].data.root_quat_w, + scene.sensors["ray_caster_rigid_object"].data.quat_w, + atol=1e-5, + rtol=1e-3, + ) + +def test_ray_caster_init_xform(setup_sim): + sim, scene_cfg = setup_sim + + # pop other than the xform raycaster + scene_cfg.ray_caster_rigid_object = None + scene_cfg.ray_caster_articulation = None + + # create scene and reset sim + scene = InteractiveScene(scene_cfg) + sim.reset() + + # Check that raycasters are initialized + assert scene.sensors["ray_caster_xform"].is_initialized + + # check correct initialization of meshes, and instances + assert len(RayCaster.meshes) == 1 + assert RayCaster._instance_count == 1 + + # check correct prim paths + assert scene.sensors["ray_caster_xform"]._view.prim_paths[0] == "/World/envs/env_0/robot" + + # check that buffers exists and have the expected shapes + assert scene.sensors["ray_caster_xform"].data.pos_w.shape == (NUM_ENVS, 3) + assert scene.sensors["ray_caster_xform"].data.quat_w.shape == (NUM_ENVS, 4) + num_rays = (scene.sensors["ray_caster_xform"].cfg.pattern_cfg.size[0] / scene.sensors["ray_caster_xform"].cfg.pattern_cfg.resolution + 1) * (scene.sensors["ray_caster_xform"].cfg.pattern_cfg.size[1] / scene.sensors["ray_caster_xform"].cfg.pattern_cfg.resolution + 1) + assert scene.sensors["ray_caster_xform"].data.ray_hits_w.shape == (NUM_ENVS, num_rays, 3) + +def test_ray_caster_multi_init(setup_sim): + sim, scene_cfg = setup_sim + + # create scene and reset sim + scene = InteractiveScene(scene_cfg) + sim.reset() + + # Check that raycasters are initialized + assert scene.sensors["ray_caster_articulation"].is_initialized + assert scene.sensors["ray_caster_rigid_object"].is_initialized + assert scene.sensors["ray_caster_xform"].is_initialized + + # check correct initialization of meshes, and instances + assert len(RayCaster.meshes) == 1 + assert RayCaster._instance_count == 3 + + # check correct prim paths + assert scene.sensors["ray_caster_articulation"]._view.prim_paths[0] == "/World/envs/env_0/robot/base" + assert scene.sensors["ray_caster_rigid_object"]._view.prim_paths[0] == "/World/envs/env_0/ball" + assert scene.sensors["ray_caster_xform"]._view.prim_paths[0] == "/World/envs/env_0/robot" + + # check that buffers exists and have the expected shapes + assert scene.sensors["ray_caster_articulation"].data.pos_w.shape == (NUM_ENVS, 3) + assert scene.sensors["ray_caster_rigid_object"].data.pos_w.shape == (NUM_ENVS, 3) + assert scene.sensors["ray_caster_xform"].data.pos_w.shape == (NUM_ENVS, 3) + assert scene.sensors["ray_caster_articulation"].data.quat_w.shape == (NUM_ENVS, 4) + assert scene.sensors["ray_caster_rigid_object"].data.quat_w.shape == (NUM_ENVS, 4) + assert scene.sensors["ray_caster_xform"].data.quat_w.shape == (NUM_ENVS, 4) + num_rays = (scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.size[0] / scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.resolution + 1) * (scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.size[1] / scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.resolution + 1) + assert scene.sensors["ray_caster_articulation"].data.ray_hits_w.shape == (NUM_ENVS, num_rays, 3) + num_rays = (scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.size[0] / scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.resolution + 1) * (scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.size[1] / scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.resolution + 1) + assert scene.sensors["ray_caster_rigid_object"].data.ray_hits_w.shape == (NUM_ENVS, num_rays, 3) + num_rays = (scene.sensors["ray_caster_xform"].cfg.pattern_cfg.size[0] / scene.sensors["ray_caster_xform"].cfg.pattern_cfg.resolution + 1) * (scene.sensors["ray_caster_xform"].cfg.pattern_cfg.size[1] / scene.sensors["ray_caster_xform"].cfg.pattern_cfg.resolution + 1) + assert scene.sensors["ray_caster_xform"].data.ray_hits_w.shape == (NUM_ENVS, num_rays, 3) + +def test_ray_hits_w(setup_sim): + sim, scene_cfg = setup_sim + + # create scene and reset sim + scene = InteractiveScene(scene_cfg) + sim.reset() + + # get the ray hits + ray_hits_w = scene.sensors["ray_caster_articulation"].data.ray_hits_w + ray_hits_w_rigid_object = scene.sensors["ray_caster_rigid_object"].data.ray_hits_w + ray_hits_w_xform = scene.sensors["ray_caster_xform"].data.ray_hits_w + + + +def test_ray_caster_offset(setup_sim): + sim, scene_cfg = setup_sim + + scene_cfg.ray_caster_articulation.offset = RayCasterCfg.OffsetCfg(pos=POSITION, rot=QUAT_WORLD) + scene_cfg.ray_caster_rigid_object.offset = RayCasterCfg.OffsetCfg(pos=POSITION, rot=QUAT_WORLD) + + # create scene and reset sim + scene = InteractiveScene(scene_cfg) + sim.reset() + + # check offset is correctly applied to rays + torch.testing.assert_close( + scene.sensors["ray_caster_articulation"].ray_origins, + scene.sensors["ray_caster_rigid_object"].ray_origins, + atol=1e-5, + rtol=1e-3, + ) From d07e430ca7ce070e986ea348a261718554a2428a Mon Sep 17 00:00:00 2001 From: Pascal Roth Date: Mon, 7 Apr 2025 14:48:57 +0200 Subject: [PATCH 5/5] test working --- .../isaaclab/sensors/ray_caster/ray_caster.py | 6 +- .../isaaclab/test/sensors/test_ray_caster.py | 171 ++++++++++++++++-- 2 files changed, 155 insertions(+), 22 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py index 8843ff668c2..3fc45ed9164 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -52,10 +52,10 @@ class RayCaster(SensorBase): # Class variables to share meshes across instances meshes: ClassVar[dict[str, wp.Mesh]] = {} """A dictionary to store warp meshes for raycasting, shared across all instances. - + The keys correspond to the prim path for the meshes, and values are the corresponding warp Mesh objects. """ - + _instance_count: ClassVar[int] = 0 """A counter to track the number of RayCaster instances, used to manage class variable lifecycle.""" @@ -171,7 +171,7 @@ def _initialize_warp_meshes(self): # check if mesh already casted into warp mesh if mesh_prim_path in RayCaster.meshes: continue - + # check if the prim is a plane - handle PhysX plane as a special case # if a plane exists then we need to create an infinite mesh that is a plane mesh_prim = sim_utils.get_first_matching_child_prim( diff --git a/source/isaaclab/test/sensors/test_ray_caster.py b/source/isaaclab/test/sensors/test_ray_caster.py index 25e68b7f447..415dbb59869 100644 --- a/source/isaaclab/test/sensors/test_ray_caster.py +++ b/source/isaaclab/test/sensors/test_ray_caster.py @@ -18,22 +18,24 @@ # app_launcher = AppLauncher.instance() # simulation_app = app_launcher.app -app_launcher = AppLauncher(headless=False) +app_launcher = AppLauncher(headless=True) simulation_app = app_launcher.app """Rest everything follows.""" +import torch + import isaacsim.core.utils.stage as stage_utils import pytest -import torch import isaaclab.sim as sim_utils -from isaaclab.assets import RigidObjectCfg +import isaaclab.terrains as terrain_gen +import isaaclab.utils.math as math_utils +from isaaclab.assets import AssetBaseCfg, RigidObjectCfg from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.sensors import RayCaster, RayCasterCfg, patterns from isaaclab.terrains import TerrainImporterCfg from isaaclab.utils import configclass -import isaaclab.terrains as terrain_gen ## # Pre-defined configs @@ -41,12 +43,13 @@ from isaaclab_assets.robots.anymal import ANYMAL_C_CFG # isort: skip NUM_ENVS = 2 +BOX_HEIGHT = 1.0 +BOX_WIDTH = 0.82 # sample camera poses POSITION = [2.5, 2.5, 2.5] QUAT_WORLD = [-0.3647052, -0.27984815, -0.1159169, 0.88047623] - BOX_TERRAIN_CFG = terrain_gen.TerrainGeneratorCfg( size=(8.0, 8.0), border_width=10.0, @@ -58,7 +61,8 @@ use_cache=True, sub_terrains={ "boxes": terrain_gen.MeshBoxTerrainCfg( - box_height_range=(1.0, 1.0) + box_height_range=(BOX_HEIGHT, BOX_HEIGHT), + platform_width=BOX_WIDTH, ), }, ) @@ -128,6 +132,13 @@ class MySceneCfg(InteractiveSceneCfg): attach_yaw_only=True, ) + # add light + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DistantLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + @pytest.fixture def setup_sim(): """Create a simulation context and scene.""" @@ -146,9 +157,10 @@ def setup_sim(): assert RayCaster.meshes == {} assert RayCaster._instance_count == 0 + def test_ray_caster_init_articulation(setup_sim): sim, scene_cfg = setup_sim - + # pop other than the articulation raycaster scene_cfg.ray_caster_rigid_object = None scene_cfg.ray_caster_xform = None @@ -170,7 +182,15 @@ def test_ray_caster_init_articulation(setup_sim): # check that buffers exists and have the expected shapes assert scene.sensors["ray_caster_articulation"].data.pos_w.shape == (NUM_ENVS, 3) assert scene.sensors["ray_caster_articulation"].data.quat_w.shape == (NUM_ENVS, 4) - num_rays = (scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.size[0] / scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.resolution + 1) * (scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.size[1] / scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.resolution + 1) + num_rays = ( + scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.size[0] + / scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.resolution + + 1 + ) * ( + scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.size[1] + / scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.resolution + + 1 + ) assert scene.sensors["ray_caster_articulation"].data.ray_hits_w.shape == (NUM_ENVS, num_rays, 3) # check placement equal to articulation @@ -187,6 +207,9 @@ def test_ray_caster_init_articulation(setup_sim): rtol=1e-3, ) + del scene + + def test_ray_caster_init_rigid_object(setup_sim): sim, scene_cfg = setup_sim @@ -211,7 +234,15 @@ def test_ray_caster_init_rigid_object(setup_sim): # check that buffers exists and have the expected shapes assert scene.sensors["ray_caster_rigid_object"].data.pos_w.shape == (NUM_ENVS, 3) assert scene.sensors["ray_caster_rigid_object"].data.quat_w.shape == (NUM_ENVS, 4) - num_rays = (scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.size[0] / scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.resolution + 1) * (scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.size[1] / scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.resolution + 1) + num_rays = ( + scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.size[0] + / scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.resolution + + 1 + ) * ( + scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.size[1] + / scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.resolution + + 1 + ) assert scene.sensors["ray_caster_rigid_object"].data.ray_hits_w.shape == (NUM_ENVS, num_rays, 3) # check placement equal to rigid object @@ -226,7 +257,10 @@ def test_ray_caster_init_rigid_object(setup_sim): scene.sensors["ray_caster_rigid_object"].data.quat_w, atol=1e-5, rtol=1e-3, - ) + ) + + del scene + def test_ray_caster_init_xform(setup_sim): sim, scene_cfg = setup_sim @@ -252,9 +286,20 @@ def test_ray_caster_init_xform(setup_sim): # check that buffers exists and have the expected shapes assert scene.sensors["ray_caster_xform"].data.pos_w.shape == (NUM_ENVS, 3) assert scene.sensors["ray_caster_xform"].data.quat_w.shape == (NUM_ENVS, 4) - num_rays = (scene.sensors["ray_caster_xform"].cfg.pattern_cfg.size[0] / scene.sensors["ray_caster_xform"].cfg.pattern_cfg.resolution + 1) * (scene.sensors["ray_caster_xform"].cfg.pattern_cfg.size[1] / scene.sensors["ray_caster_xform"].cfg.pattern_cfg.resolution + 1) + num_rays = ( + scene.sensors["ray_caster_xform"].cfg.pattern_cfg.size[0] + / scene.sensors["ray_caster_xform"].cfg.pattern_cfg.resolution + + 1 + ) * ( + scene.sensors["ray_caster_xform"].cfg.pattern_cfg.size[1] + / scene.sensors["ray_caster_xform"].cfg.pattern_cfg.resolution + + 1 + ) assert scene.sensors["ray_caster_xform"].data.ray_hits_w.shape == (NUM_ENVS, num_rays, 3) + del scene + + def test_ray_caster_multi_init(setup_sim): sim, scene_cfg = setup_sim @@ -283,25 +328,80 @@ def test_ray_caster_multi_init(setup_sim): assert scene.sensors["ray_caster_articulation"].data.quat_w.shape == (NUM_ENVS, 4) assert scene.sensors["ray_caster_rigid_object"].data.quat_w.shape == (NUM_ENVS, 4) assert scene.sensors["ray_caster_xform"].data.quat_w.shape == (NUM_ENVS, 4) - num_rays = (scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.size[0] / scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.resolution + 1) * (scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.size[1] / scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.resolution + 1) + num_rays = ( + scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.size[0] + / scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.resolution + + 1 + ) * ( + scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.size[1] + / scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.resolution + + 1 + ) assert scene.sensors["ray_caster_articulation"].data.ray_hits_w.shape == (NUM_ENVS, num_rays, 3) - num_rays = (scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.size[0] / scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.resolution + 1) * (scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.size[1] / scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.resolution + 1) + num_rays = ( + scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.size[0] + / scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.resolution + + 1 + ) * ( + scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.size[1] + / scene.sensors["ray_caster_rigid_object"].cfg.pattern_cfg.resolution + + 1 + ) assert scene.sensors["ray_caster_rigid_object"].data.ray_hits_w.shape == (NUM_ENVS, num_rays, 3) - num_rays = (scene.sensors["ray_caster_xform"].cfg.pattern_cfg.size[0] / scene.sensors["ray_caster_xform"].cfg.pattern_cfg.resolution + 1) * (scene.sensors["ray_caster_xform"].cfg.pattern_cfg.size[1] / scene.sensors["ray_caster_xform"].cfg.pattern_cfg.resolution + 1) + num_rays = ( + scene.sensors["ray_caster_xform"].cfg.pattern_cfg.size[0] + / scene.sensors["ray_caster_xform"].cfg.pattern_cfg.resolution + + 1 + ) * ( + scene.sensors["ray_caster_xform"].cfg.pattern_cfg.size[1] + / scene.sensors["ray_caster_xform"].cfg.pattern_cfg.resolution + + 1 + ) assert scene.sensors["ray_caster_xform"].data.ray_hits_w.shape == (NUM_ENVS, num_rays, 3) + del scene + + def test_ray_hits_w(setup_sim): sim, scene_cfg = setup_sim + # reduce number of environments to 1, s.t. they are on top of the box + scene_cfg.num_envs = 1 + scene_cfg.robot.init_state.pos = (0.0, 0.0, BOX_HEIGHT + 0.5) + scene_cfg.balls.init_state.pos = (0.0, 0.0, BOX_HEIGHT + 0.5) # create scene and reset sim scene = InteractiveScene(scene_cfg) sim.reset() # get the ray hits - ray_hits_w = scene.sensors["ray_caster_articulation"].data.ray_hits_w + ray_hits_w_articulation = scene.sensors["ray_caster_articulation"].data.ray_hits_w ray_hits_w_rigid_object = scene.sensors["ray_caster_rigid_object"].data.ray_hits_w - ray_hits_w_xform = scene.sensors["ray_caster_xform"].data.ray_hits_w + # check that all rays within +- BOX_WDITH / 2 are BOX_HEIGHT, else 0 + on_box_idx = torch.all(torch.abs(ray_hits_w_articulation[..., :2]) <= BOX_WIDTH / 2, dim=-1)[0] + torch.testing.assert_close( + ray_hits_w_articulation[0, on_box_idx, 2], + torch.full_like(ray_hits_w_articulation[0, on_box_idx, 2], BOX_HEIGHT), + rtol=1e-3, + atol=1e-5, + ) + torch.testing.assert_close( + ray_hits_w_articulation[0, ~on_box_idx, 2], + torch.zeros_like(ray_hits_w_articulation[0, ~on_box_idx, 2]), + rtol=1e-3, + atol=1e-5, + ) + + # check that the other two follow the same behavior + torch.testing.assert_close( + scene.sensors["ray_caster_rigid_object"].ray_starts, + scene.sensors["ray_caster_articulation"].ray_starts, + rtol=1e-3, + atol=1e-5, + ) + torch.testing.assert_close(ray_hits_w_articulation[..., 2], ray_hits_w_rigid_object[..., 2], rtol=1e-3, atol=1e-5) + + del scene def test_ray_caster_offset(setup_sim): @@ -313,11 +413,44 @@ def test_ray_caster_offset(setup_sim): # create scene and reset sim scene = InteractiveScene(scene_cfg) sim.reset() - + + # get original pattern + ray_starts, ray_directions = scene.sensors["ray_caster_articulation"].cfg.pattern_cfg.func( + scene.sensors["ray_caster_articulation"].cfg.pattern_cfg, scene.sensors["ray_caster_articulation"]._device + ) + # check offset is correctly applied to rays torch.testing.assert_close( - scene.sensors["ray_caster_articulation"].ray_origins, - scene.sensors["ray_caster_rigid_object"].ray_origins, + scene.sensors["ray_caster_articulation"].ray_starts, + (ray_starts + torch.tensor(POSITION, device=scene.sensors["ray_caster_articulation"]._device))[ + None, ... + ].repeat(NUM_ENVS, 1, 1), atol=1e-5, rtol=1e-3, ) + torch.testing.assert_close( + scene.sensors["ray_caster_articulation"].ray_directions, + ( + math_utils.quat_apply( + torch.tensor(QUAT_WORLD, device=scene.sensors["ray_caster_articulation"]._device), ray_directions + ) + )[None, ...].repeat(NUM_ENVS, 1, 1), + atol=1e-5, + rtol=1e-3, + ) + + # test that is the same between the two raycasters + torch.testing.assert_close( + scene.sensors["ray_caster_articulation"].ray_starts, + scene.sensors["ray_caster_rigid_object"].ray_starts, + atol=1e-5, + rtol=1e-3, + ) + torch.testing.assert_close( + scene.sensors["ray_caster_articulation"].ray_directions, + scene.sensors["ray_caster_rigid_object"].ray_directions, + atol=1e-5, + rtol=1e-3, + ) + + del scene