From 670a8c97db645d50a3ea89b06f7366fc2212fc06 Mon Sep 17 00:00:00 2001
From: Nikita Rudin <48368649+nikitardn@users.noreply.github.com>
Date: Tue, 30 Jan 2024 10:10:24 +0100
Subject: [PATCH] Moves height-scan observation offset to its function's
 argument (#366)

# Description

Adds an optional argument to the height-scan obs term defining the
offset. Previously, it was hardcoded to 0.5.

## 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
`./orbit.sh --format`
- [ ] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [ ] I have added tests that prove my fix is effective or that my
feature works
- [ ] I have run all the tests with `./orbit.sh --test` and they pass
- [ 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
---
 .../omni.isaac.orbit/config/extension.toml           |  2 +-
 .../extensions/omni.isaac.orbit/docs/CHANGELOG.rst   | 12 +++++++++++-
 .../omni/isaac/orbit/envs/mdp/observations.py        | 10 ++++++----
 3 files changed, 18 insertions(+), 6 deletions(-)

diff --git a/source/extensions/omni.isaac.orbit/config/extension.toml b/source/extensions/omni.isaac.orbit/config/extension.toml
index 0fd4ad290b..e2732161d4 100644
--- a/source/extensions/omni.isaac.orbit/config/extension.toml
+++ b/source/extensions/omni.isaac.orbit/config/extension.toml
@@ -1,7 +1,7 @@
 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.10.15"
+version = "0.10.16"
 
 # Description
 title = "ORBIT framework for Robot Learning"
diff --git a/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst b/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst
index eea342995b..77cd44ef4b 100644
--- a/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst
+++ b/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst
@@ -1,6 +1,16 @@
 Changelog
 ---------
 
+0.10.16 (2024-01-29)
+~~~~~~~~~~~~~~~~~~~~
+
+Added
+^^^^^^
+
+* Added an offset parameter to the height scan observation term. This allows the user to specify the
+  height offset of the scan from the tracked body. Previously it was hard-coded to be 0.5.
+
+
 0.10.15 (2024-01-29)
 ~~~~~~~~~~~~~~~~~~~~
 
@@ -12,7 +22,7 @@ Fixed
 
 
 0.10.14 (2024-01-22)
-~~~~~~~~~~~~~~~~~
+~~~~~~~~~~~~~~~~~~~~
 
 Fixed
 ^^^^^
diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/envs/mdp/observations.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/envs/mdp/observations.py
index d69c9758d5..4279aedd8f 100644
--- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/envs/mdp/observations.py
+++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/envs/mdp/observations.py
@@ -88,12 +88,14 @@ def joint_vel_rel(env: BaseEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robo
 """
 
 
-def height_scan(env: BaseEnv, sensor_cfg: SceneEntityCfg) -> torch.Tensor:
-    """Height scan from the given sensor w.r.t. the sensor's frame."""
+def height_scan(env: BaseEnv, sensor_cfg: SceneEntityCfg, offset: float = 0.5) -> torch.Tensor:
+    """Height scan from the given sensor w.r.t. the sensor's frame.
+    The provided offset (Defaults to 0.5) is subtracted from the returned values.
+    """
     # extract the used quantities (to enable type-hinting)
     sensor: RayCaster = env.scene.sensors[sensor_cfg.name]
-    # height scan: height = sensor_height - hit_point_z - 0.5
-    return sensor.data.pos_w[:, 2].unsqueeze(1) - sensor.data.ray_hits_w[..., 2] - 0.5
+    # height scan: height = sensor_height - hit_point_z - offset
+    return sensor.data.pos_w[:, 2].unsqueeze(1) - sensor.data.ray_hits_w[..., 2] - offset
 
 
 def body_incoming_wrench(env: BaseEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor: