Unverified Commit 21173c3e authored by robotsfan's avatar robotsfan Committed by GitHub

Corrects calculation of target height adjustment based on sensor data (#1710)

# Description

Corrected calculation of target height adjustment based on sensor data.

Fixes #1546 and #1698

Thanks to @ClemensSchwarke and @shendredm for the modification
suggestions. To use this `base_height_l2` reward function, users need to
make the following changes:

1. Customize a new sensor

```python
class MySceneCfg(InteractiveSceneCfg):
    height_scanner_base = RayCasterCfg(
        prim_path="{ENV_REGEX_NS}/Robot/base",
        offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)),
        attach_yaw_only=True,
        pattern_cfg=patterns.GridPatternCfg(resolution=0.05, size=(0.1, 0.1)),
        debug_vis=False,
        mesh_prim_paths=["/World/ground"],
    )
```

2. Pass this new sensor to the reward function

```python
class RewardsCfg:
    base_height_l2 = RewTerm(
        func=mdp.base_height_l2,
        weight=0.0,
        params={
            "asset_cfg": SceneEntityCfg("robot", body_names=""),
            "sensor_cfg": SceneEntityCfg("height_scanner_base"),
            "target_height": 0.0,
        },
    )
```

This will create a 10 by 10 cm patch consisting of 9 rays and should
provide a good estimate of the ground height at the robot's position.

## Type of change

- Bug fix (non-breaking change which fixes an issue)

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.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 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
parent 999c1e9a
......@@ -114,7 +114,7 @@ def base_height_l2(
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]
adjusted_target_height = target_height + torch.mean(sensor.data.ray_hits_w[..., 2], dim=1)
else:
# Use the provided target height directly for flat terrain
adjusted_target_height = target_height
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment