diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index 9476e530aba..8ecc13b56b7 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -54,6 +54,7 @@ Guidelines for modifications: * David Yang * Dhananjay Shendre * Dorsa Rohani +* Fabian Jenelten * Felipe Mohr * Felix Yu * Gary Lvov @@ -66,6 +67,7 @@ Guidelines for modifications: * Jack Zeng * Jan Kerner * Jean Tampon +* Jeonghwan Kim * Jia Lin Yuan * Jiakai Zhang * Jinghuan Shang 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..ebb6aeb26c0 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,43 @@ 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" 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 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 +286,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..8b4f8e1fa4c 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 @@ -47,6 +48,18 @@ class OffsetCfg: """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. + * `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 +69,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"],