From 417c993a3a799599c6aef8d93dee986e5e198821 Mon Sep 17 00:00:00 2001 From: James Smith Date: Fri, 23 May 2025 13:51:58 -0400 Subject: [PATCH 1/4] Updates to ray caster * Adds a new ray_alignment parameter - either world, yaw, or base * Fixes drift height sampling --- .../03_envs/create_quadruped_base_env.py | 2 +- .../04_sensors/add_sensors_on_robot.py | 2 +- .../tutorials/04_sensors/run_ray_caster.py | 2 +- .../isaaclab/scene/interactive_scene_cfg.py | 2 +- .../isaaclab/sensors/ray_caster/ray_caster.py | 32 ++++++++++++++++--- .../ray_caster/ray_caster_camera_cfg.py | 2 +- .../sensors/ray_caster/ray_caster_cfg.py | 18 ++++++++--- ...eck_manager_based_env_anymal_locomotion.py | 2 +- .../test/markers/check_markers_visibility.py | 2 +- .../test/scene/check_interactive_scene.py | 2 +- .../isaaclab/test/sensors/check_ray_caster.py | 2 +- .../isaaclab_assets/sensors/velodyne.py | 2 +- .../locomotion/velocity/velocity_env_cfg.py | 2 +- 13 files changed, 52 insertions(+), 20 deletions(-) diff --git a/scripts/tutorials/03_envs/create_quadruped_base_env.py b/scripts/tutorials/03_envs/create_quadruped_base_env.py index cce2394cf4d..93e2640c426 100644 --- a/scripts/tutorials/03_envs/create_quadruped_base_env.py +++ b/scripts/tutorials/03_envs/create_quadruped_base_env.py @@ -105,7 +105,7 @@ class MySceneCfg(InteractiveSceneCfg): height_scanner = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot/base", offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), - attach_yaw_only=True, + ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), debug_vis=True, mesh_prim_paths=["/World/ground"], diff --git a/scripts/tutorials/04_sensors/add_sensors_on_robot.py b/scripts/tutorials/04_sensors/add_sensors_on_robot.py index 0cf57447d58..9ebed5c182c 100644 --- a/scripts/tutorials/04_sensors/add_sensors_on_robot.py +++ b/scripts/tutorials/04_sensors/add_sensors_on_robot.py @@ -84,7 +84,7 @@ class SensorsSceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/Robot/base", update_period=0.02, offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), - attach_yaw_only=True, + ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), debug_vis=True, mesh_prim_paths=["/World/defaultGroundPlane"], diff --git a/scripts/tutorials/04_sensors/run_ray_caster.py b/scripts/tutorials/04_sensors/run_ray_caster.py index 867d88a37d0..21342d3f745 100644 --- a/scripts/tutorials/04_sensors/run_ray_caster.py +++ b/scripts/tutorials/04_sensors/run_ray_caster.py @@ -49,7 +49,7 @@ def define_sensor() -> RayCaster: prim_path="/World/Origin.*/ball", mesh_prim_paths=["/World/ground"], pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=(2.0, 2.0)), - attach_yaw_only=True, + ray_alignment="yaw", debug_vis=not args_cli.headless, ) ray_caster = RayCaster(cfg=ray_caster_cfg) diff --git a/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py b/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py index 8afdec4b46f..9fcefa27d84 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py @@ -52,7 +52,7 @@ class MySceneCfg(InteractiveSceneCfg): height_scanner = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot_1/base", offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), - attach_yaw_only=True, + ray_alignment="yaw", pattern_cfg=GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), debug_vis=True, mesh_prim_paths=["/World/ground"], diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py index 123d26cc158..6b2885b4143 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -111,7 +111,13 @@ def reset(self, env_ids: Sequence[int] | None = None): if env_ids is None: env_ids = slice(None) # resample the drift - self.drift[env_ids] = self.drift[env_ids].uniform_(*self.cfg.drift_range) + r = torch.empty(len(env_ids), 3, device=self.device) + self.drift[env_ids] = r.uniform_(*self.cfg.drift_range) + # resample the height drift + r = torch.empty(len(env_ids), device=self.device) + self.ray_cast_drift[env_ids, 0] = r.uniform_(*self.cfg.ray_cast_drift_range["x"]) + self.ray_cast_drift[env_ids, 1] = r.uniform_(*self.cfg.ray_cast_drift_range["y"]) + self.ray_cast_drift[env_ids, 2] = r.uniform_(*self.cfg.ray_cast_drift_range["z"]) """ Implementation. @@ -212,6 +218,7 @@ def _initialize_rays_impl(self): self.ray_directions = self.ray_directions.repeat(self._view.count, 1, 1) # prepare drift self.drift = torch.zeros(self._view.count, 3, device=self.device) + self.ray_cast_drift = torch.zeros(self._view.count, 3, device=self.device) # fill the data buffer self._data.pos_w = torch.zeros(self._view.count, 3, device=self._device) self._data.quat_w = torch.zeros(self._view.count, 4, device=self._device) @@ -233,23 +240,37 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): # note: we clone here because we are read-only operations pos_w = pos_w.clone() quat_w = quat_w.clone() - # apply drift + # apply drift to ray starting position in world frame pos_w += self.drift[env_ids] # store the poses self._data.pos_w[env_ids] = pos_w self._data.quat_w[env_ids] = quat_w # ray cast based on the sensor poses - if self.cfg.attach_yaw_only: + if self.cfg.ray_alignment == "world": + # apply horizontal drift to ray starting position in ray caster frame + pos_w[:, 0:2] += self.ray_cast_drift[env_ids, 0:2] + # no rotation is considered and directions are not rotated + ray_starts_w = self.ray_starts[env_ids] + ray_starts_w += pos_w.unsqueeze(1) + ray_directions_w = self.ray_directions[env_ids] + elif self.cfg.ray_alignment == "yaw": + # apply horizontal drift to ray starting position in ray caster frame + pos_w[:, 0:2] += quat_apply_yaw(quat_w, self.ray_cast_drift[env_ids])[:, 0:2] # only yaw orientation is considered and directions are not rotated ray_starts_w = quat_apply_yaw(quat_w.repeat(1, self.num_rays), self.ray_starts[env_ids]) ray_starts_w += pos_w.unsqueeze(1) ray_directions_w = self.ray_directions[env_ids] - else: + elif self.cfg.ray_alignment == "base": + # apply horizontal drift to ray starting position in ray caster frame + pos_w[:, 0:2] += quat_apply(quat_w, self.ray_cast_drift[env_ids])[:, 0:2] # full orientation is considered ray_starts_w = quat_apply(quat_w.repeat(1, self.num_rays), self.ray_starts[env_ids]) ray_starts_w += pos_w.unsqueeze(1) ray_directions_w = quat_apply(quat_w.repeat(1, self.num_rays), self.ray_directions[env_ids]) + else: + raise RuntimeError(f"Unsupported ray_alignment type: {self.cfg.ray_alignment}.") + # ray cast and store the hits # TODO: Make this work for multiple meshes? self._data.ray_hits_w[env_ids] = raycast_mesh( @@ -259,6 +280,9 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): mesh=self.meshes[self.cfg.mesh_prim_paths[0]], )[0] + # apply vertical drift to ray starting position in ray caster frame + self._data.ray_hits_w[env_ids, :, 2] += self.ray_cast_drift[env_ids, 2].unsqueeze(-1) + def _set_debug_vis_impl(self, debug_vis: bool): # set visibility of markers # note: parent only deals with callbacks. not their visibility diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera_cfg.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera_cfg.py index d6fc22e8221..d0a5a84691f 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera_cfg.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera_cfg.py @@ -60,4 +60,4 @@ class OffsetCfg: def __post_init__(self): # for cameras, this quantity should be False always. - self.attach_yaw_only = False + self.ray_alignment = "base" diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py index 5b150b606a0..8e82ee9d274 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py @@ -7,6 +7,7 @@ from dataclasses import MISSING +from typing import Literal from isaaclab.markers import VisualizationMarkersCfg from isaaclab.markers.config import RAY_CASTER_MARKER_CFG @@ -43,10 +44,11 @@ class OffsetCfg: offset: OffsetCfg = OffsetCfg() """The offset pose of the sensor's frame from the sensor's parent frame. Defaults to identity.""" - attach_yaw_only: bool = MISSING - """Whether the rays' starting positions and directions only track the yaw orientation. - - This is useful for ray-casting height maps, where only yaw rotation is needed. + ray_alignment: Literal["base", "yaw", "world"] = "yaw" + """Specify in what frame the rays are projected onto the ground. Default is `world`. + * `base` if the rays' starting positions and directions track the full root orientation. + * `yaw` if the rays' starting positions and directions only track yaw orientation. This is useful for ray-casting height maps, where only yaw rotation is needed. + * `world` if rays' starting positions and directions are not rotated. This is useful in combination with the grid map package. """ pattern_cfg: PatternBaseCfg = MISSING @@ -56,7 +58,13 @@ class OffsetCfg: """Maximum distance (in meters) from the sensor to ray cast to. Defaults to 1e6.""" drift_range: tuple[float, float] = (0.0, 0.0) - """The range of drift (in meters) to add to the ray starting positions (xyz). Defaults to (0.0, 0.0). + """The range of drift (in meters) to add to the ray starting positions (xyz) in world frame. Defaults to (0.0, 0.0). + + For floating base robots, this is useful for simulating drift in the robot's pose estimation. + """ + + ray_cast_drift_range: dict[str, tuple[float, float]] = {"x": (0.0, 0.0), "y": (0.0, 0.0), "z": (0.0, 0.0)} + """The range of drift (in meters) to add to the projected ray points in local projection frame. Defaults to (0.0, 0.0) for x, y, and z drift. For floating base robots, this is useful for simulating drift in the robot's pose estimation. """ diff --git a/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py b/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py index 83d1729f24c..e63c98eef53 100644 --- a/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py +++ b/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py @@ -92,7 +92,7 @@ class MySceneCfg(InteractiveSceneCfg): height_scanner = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot/base", offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), - attach_yaw_only=True, + ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), debug_vis=True, mesh_prim_paths=["/World/ground"], diff --git a/source/isaaclab/test/markers/check_markers_visibility.py b/source/isaaclab/test/markers/check_markers_visibility.py index 735cd9b9e97..23ef93c34ea 100644 --- a/source/isaaclab/test/markers/check_markers_visibility.py +++ b/source/isaaclab/test/markers/check_markers_visibility.py @@ -70,7 +70,7 @@ class SensorsSceneCfg(InteractiveSceneCfg): prim_path="{ENV_REGEX_NS}/Robot/base", update_period=0.02, offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), - attach_yaw_only=True, + ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), debug_vis=True, mesh_prim_paths=["/World/defaultGroundPlane"], diff --git a/source/isaaclab/test/scene/check_interactive_scene.py b/source/isaaclab/test/scene/check_interactive_scene.py index 858d37bf579..d7d4eb9c9bb 100644 --- a/source/isaaclab/test/scene/check_interactive_scene.py +++ b/source/isaaclab/test/scene/check_interactive_scene.py @@ -62,7 +62,7 @@ class MySceneCfg(InteractiveSceneCfg): height_scanner = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot_1/base", offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), - attach_yaw_only=True, + ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), debug_vis=True, mesh_prim_paths=["/World/ground"], diff --git a/source/isaaclab/test/sensors/check_ray_caster.py b/source/isaaclab/test/sensors/check_ray_caster.py index 95013a7ddbd..84db4428253 100644 --- a/source/isaaclab/test/sensors/check_ray_caster.py +++ b/source/isaaclab/test/sensors/check_ray_caster.py @@ -123,7 +123,7 @@ def main(): prim_path="/World/envs/env_.*/ball", mesh_prim_paths=["/World/ground"], pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=(1.6, 1.0)), - attach_yaw_only=True, + ray_alignment="yaw", debug_vis=not args_cli.headless, ) ray_caster = RayCaster(cfg=ray_caster_cfg) diff --git a/source/isaaclab_assets/isaaclab_assets/sensors/velodyne.py b/source/isaaclab_assets/isaaclab_assets/sensors/velodyne.py index d2324e118e7..3353d96c776 100644 --- a/source/isaaclab_assets/isaaclab_assets/sensors/velodyne.py +++ b/source/isaaclab_assets/isaaclab_assets/sensors/velodyne.py @@ -13,7 +13,7 @@ ## VELODYNE_VLP_16_RAYCASTER_CFG = RayCasterCfg( - attach_yaw_only=False, + ray_alignment="base", pattern_cfg=patterns.LidarPatternCfg( channels=16, vertical_fov_range=(-15.0, 15.0), horizontal_fov_range=(-180.0, 180.0), horizontal_res=0.2 ), diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py index 29aebd63e37..0d809a1dc5c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py @@ -66,7 +66,7 @@ class MySceneCfg(InteractiveSceneCfg): height_scanner = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot/base", offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), - attach_yaw_only=True, + ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), debug_vis=False, mesh_prim_paths=["/World/ground"], From fb0490be9e8d0fa1a5aafcdaad2b425784010598 Mon Sep 17 00:00:00 2001 From: James Smith Date: Fri, 23 May 2025 14:06:10 -0400 Subject: [PATCH 2/4] Update contributors with Fabian / Jeonghwan --- CONTRIBUTORS.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index a4ea2da38ab..9eed51c802a 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -53,6 +53,7 @@ Guidelines for modifications: * David Yang * Dhananjay Shendre * Dorsa Rohani +* Fabian Jenelten * Felipe Mohr * Felix Yu * Gary Lvov @@ -65,6 +66,7 @@ Guidelines for modifications: * Jack Zeng * Jan Kerner * Jean Tampon +* Jeonghwan Kim * Jia Lin Yuan * Jiakai Zhang * Jinghuan Shang From a56475d4d0d0d06dd1bec6bb0a033861b38124f1 Mon Sep 17 00:00:00 2001 From: James Smith Date: Tue, 27 May 2025 10:22:03 -0400 Subject: [PATCH 3/4] Add backwards compatibility for attach_yaw_only --- .../isaaclab/sensors/ray_caster/ray_caster.py | 8 +++++++- .../isaaclab/sensors/ray_caster/ray_caster_cfg.py | 11 +++++++++++ 2 files changed, 18 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 6b2885b4143..ebb6aeb26c0 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -254,7 +254,13 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): ray_starts_w = self.ray_starts[env_ids] ray_starts_w += pos_w.unsqueeze(1) ray_directions_w = self.ray_directions[env_ids] - elif self.cfg.ray_alignment == "yaw": + elif self.cfg.ray_alignment == "yaw" or self.cfg.attach_yaw_only: + if self.cfg.attach_yaw_only: + omni.log.warn( + "The `attach_yaw_only` property will be deprecated in a future release. Please use" + " `ray_alignment='yaw'` instead." + ) + # apply horizontal drift to ray starting position in ray caster frame pos_w[:, 0:2] += quat_apply_yaw(quat_w, self.ray_cast_drift[env_ids])[:, 0:2] # only yaw orientation is considered and directions are not rotated diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py index 8e82ee9d274..8b4f8e1fa4c 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py @@ -44,6 +44,17 @@ class OffsetCfg: offset: OffsetCfg = OffsetCfg() """The offset pose of the sensor's frame from the sensor's parent frame. Defaults to identity.""" + attach_yaw_only: bool = MISSING + """Whether the rays' starting positions and directions only track the yaw orientation. + + This is useful for ray-casting height maps, where only yaw rotation is needed. + + .. warning:: + + This attribute is deprecated. Use :attr:`~isaaclab.sensors.ray_caster.ray_caster_cfg.ray_alignment` instead. + To get the same behavior, set `ray_alignment` to `"yaw"`. + """ + ray_alignment: Literal["base", "yaw", "world"] = "yaw" """Specify in what frame the rays are projected onto the ground. Default is `world`. * `base` if the rays' starting positions and directions track the full root orientation. From a786494be1e8c868f5467985f771c39cd4130fb0 Mon Sep 17 00:00:00 2001 From: ooctipus Date: Mon, 7 Jul 2025 10:41:01 -0700 Subject: [PATCH 4/4] Update ray_caster_cfg.py Signed-off-by: ooctipus --- .../isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py index 8b4f8e1fa4c..2632a038ebd 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py @@ -57,9 +57,9 @@ class OffsetCfg: ray_alignment: Literal["base", "yaw", "world"] = "yaw" """Specify in what frame the rays are projected onto the ground. Default is `world`. - * `base` if the rays' starting positions and directions track the full root orientation. - * `yaw` if the rays' starting positions and directions only track yaw orientation. This is useful for ray-casting height maps, where only yaw rotation is needed. - * `world` if rays' starting positions and directions are not rotated. This is useful in combination with the grid map package. + * `base` if the rays' starting positions and directions track the full root position and orientation. + * `yaw` if the rays' starting positions and directions track root position and only yaw component of orientation. This is useful for ray-casting height maps. + * `world` if rays' starting positions and directions are always fixed. This is useful in combination with the grid map package. """ pattern_cfg: PatternBaseCfg = MISSING