Updates to ray caster

* Adds a new ray_alignment parameter - either world, yaw, or base
* Fixes drift height sampling

Author: jsmith-bdai

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"],

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"],

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)

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"],

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

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"

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.
     """

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"],

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"],

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"],