From d8bc725692200bc99e04b58979068a7557273a2c Mon Sep 17 00:00:00 2001 From: Hongwei Xiong Date: Fri, 13 Dec 2024 21:57:12 +0800 Subject: [PATCH] Adds RayCaster rough terrain base height to reward (#1525) # Description Enhance `base_height_l2` function to support rough terrain adjustments: - Added an optional `sensor_cfg` parameter to allow dynamic target height adjustments based on sensor readings (e.g., RayCaster). - Updated the function to calculate the L2 squared penalty using adjusted height values for rough terrain scenarios. - Preserved existing behavior for flat terrain by using the fixed `target_height`. - Improved compatibility for applications involving uneven terrain. Fixes #1524 ## Type of change - New feature (non-breaking change which adds functionality) ## Checklist - [x ] I have run the [`pre-commit` checks](https://pre-commit.com/) with `./isaaclab.sh --format` - [x ] I have made corresponding changes to the documentation - [x ] My changes generate no new warnings - [x ] I have added tests that prove my fix is effective or that my feature works - [x ] I have updated the changelog and the corresponding version in the extension's `config/extension.toml` file - [x ] I have added my name to the `CONTRIBUTORS.md` or my name already exists there --------- Signed-off-by: Kelly Guo Signed-off-by: Kelly Guo Co-authored-by: Kelly Guo Co-authored-by: Kelly Guo --- CONTRIBUTORS.md | 1 + .../omni.isaac.lab/config/extension.toml | 2 +- .../omni.isaac.lab/docs/CHANGELOG.rst | 11 ++++++++++ .../omni/isaac/lab/envs/mdp/rewards.py | 21 ++++++++++++++----- 4 files changed, 29 insertions(+), 6 deletions(-) diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index 9b3ea57d63..bd48566556 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -49,6 +49,7 @@ Guidelines for modifications: * Giulio Romualdi * Haoran Zhou * HoJin Jeon +* Hongwei Xiong * Jan Kerner * Jean Tampon * Jia Lin Yuan diff --git a/source/extensions/omni.isaac.lab/config/extension.toml b/source/extensions/omni.isaac.lab/config/extension.toml index 082ec35c24..9b53e611d5 100644 --- a/source/extensions/omni.isaac.lab/config/extension.toml +++ b/source/extensions/omni.isaac.lab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.27.25" +version = "0.27.26" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst index 7ff5ef82d4..e250c40900 100644 --- a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst @@ -1,6 +1,17 @@ Changelog --------- +0.27.26 (2024-12-11) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Introduced an optional ``sensor_cfg`` parameter to the :meth:`~omni.isaac.lab.envs.mdp.rewards.base_height_l2` function, enabling the use of + :class:`~omni.isaac.lab.sensors.RayCaster` for height adjustments. For flat terrains, the function retains its previous behavior. +* Improved documentation to clarify the usage of the :meth:`~omni.isaac.lab.envs.mdp.rewards.base_height_l2` function in both flat and rough terrain settings. + + 0.27.25 (2024-12-11) ~~~~~~~~~~~~~~~~~~~~ diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/rewards.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/rewards.py index 1a68d321ab..9b3fe7440c 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/rewards.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/rewards.py @@ -18,7 +18,7 @@ from omni.isaac.lab.managers import SceneEntityCfg from omni.isaac.lab.managers.manager_base import ManagerTermBase from omni.isaac.lab.managers.manager_term_cfg import RewardTermCfg -from omni.isaac.lab.sensors import ContactSensor +from omni.isaac.lab.sensors import ContactSensor, RayCaster if TYPE_CHECKING: from omni.isaac.lab.envs import ManagerBasedRLEnv @@ -98,17 +98,28 @@ def flat_orientation_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = Scen def base_height_l2( - env: ManagerBasedRLEnv, target_height: float, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") + env: ManagerBasedRLEnv, + target_height: float, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + sensor_cfg: SceneEntityCfg | None = None, ) -> torch.Tensor: """Penalize asset height from its target using L2 squared kernel. Note: - Currently, it assumes a flat terrain, i.e. the target height is in the world frame. + For flat terrain, target height is in the world frame. For rough terrain, + sensor readings can adjust the target height to account for the terrain. """ # extract the used quantities (to enable type-hinting) asset: RigidObject = env.scene[asset_cfg.name] - # TODO: Fix this for rough-terrain. - return torch.square(asset.data.root_pos_w[:, 2] - target_height) + if sensor_cfg is not None: + sensor: RayCaster = env.scene[sensor_cfg.name] + # Adjust the target height using the sensor data + adjusted_target_height = target_height + sensor.data.pos_w[:, 2] + else: + # Use the provided target height directly for flat terrain + adjusted_target_height = target_height + # Compute the L2 squared penalty + return torch.square(asset.data.root_pos_w[:, 2] - adjusted_target_height) def body_lin_acc_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: