[Bug Report] `base_height_l2` for rough terrain is wrong!

[fan-ziqi]

Describe the bug

The code added in #1525 is wrong!

Also marked in #1546

IsaacLab/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/rewards.py

Lines 100 to 122 in b5078de

	def base_height_l2(
	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:
	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]
	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_link_pos_w[:, 2] - adjusted_target_height)

sensor.data.pos_w[:, 2] is the position of sensor origin in world frame, and it is equal to asset.data.root_link_pos_w[:, 2].

In rough terrain, the func return

torch.square(asset.data.root_link_pos_w[:, 2] - adjusted_target_height)
= torch.square(asset.data.root_link_pos_w[:, 2] - target_height - sensor.data.pos_w[:, 2])
= torch.square(- target_height)

This is meaningless.

I think we should take the height of the center point as the reference height, but this cannot be achieved with the current API.

Checklist

• I have checked that there is no similar issue in the repo (required)
• I have checked that the issue is not in running Isaac Sim itself and is related to the repo

[zoctipus]

I am also not super sure what sensor cfg should be there, i do a agree a more clear example or comment is needed to understand the useage of it.

how I currently get around with this issue is exactly how you described, querying the center point of the height scanner(assume your height scanner is center around robot base), then by subtracting hit z from base z

[fan-ziqi]

I am also not super sure what sensor cfg should be there, i do a agree a more clear example or comment is needed to understand the useage of it.

how I currently get around with this issue is exactly how you described, querying the center point of the height scanner(assume your height scanner is center around robot base), then by subtracting hit z from base z

May I ask how you get the center index?

[zoctipus]

I am also not super sure what sensor cfg should be there, i do a agree a more clear example or comment is needed to understand the useage of it.
how I currently get around with this issue is exactly how you described, querying the center point of the height scanner(assume your height scanner is center around robot base), then by subtracting hit z from base z

May I ask how you get the center index?

Yes! This is the code

sensor: RayCaster = env.scene[sensor_cfg.name]
ray_hits_w = sensor.data.ray_hits_w
hit_center_z = ray_hits_w[:, ray_hits_w.shape[1] // 2, 2]

and hit center z can be treated as the z of the floor

[shendredm]

This issue is already marked as bug in #1546, in this code is as -

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 + torch.mean(sensor.data.ray_hits_w[..., 2], dim=1) 

Here using the mean of ray hits

[fan-ziqi]

I am also not super sure what sensor cfg should be there, i do a agree a more clear example or comment is needed to understand the useage of it.
how I currently get around with this issue is exactly how you described, querying the center point of the height scanner(assume your height scanner is center around robot base), then by subtracting hit z from base z

May I ask how you get the center index?

Yes! This is the code

sensor: RayCaster = env.scene[sensor_cfg.name]
ray_hits_w = sensor.data.ray_hits_w
hit_center_z = ray_hits_w[:, ray_hits_w.shape[1] // 2, 2]

and hit center z can be treated as the z of the floor

But I think this only satisfies an odd number of points. If it is an even number, it will definitely be indexed to the edge instead of the average of the center points

[fan-ziqi]

This issue is already marked as bug in #1546, in this code is as -

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 + torch.mean(sensor.data.ray_hits_w[..., 2], dim=1) 

Here using the mean of ray hits

I don't know if the average of all height points can represent the height of the base

[RandomOakForest]

Thank you for posting this. We appreciate the detail. I'll tag this for the team to follow up, aggregating to #1546, as mentioned by @shendredm. Thank you.

[fan-ziqi]

Is there anything I can help improve?

[shendredm]

I was going through some IsaacGym envs found the base height calculation in the repo: legged_robot. I'm not sure how to implement this in isaaclab. attaching code for reference.

legged_robot.txt

[ClemensSchwarke]

A proper sensor configuration would require a circular ray casting pattern that is unfortunately not yet implemented. To get a working solution before the official fix and with minimal effort I would recommend doing the following:

1. fix the reward bug like @shendredm proposed:

adjusted_target_height = target_height + torch.mean(sensor.data.ray_hits_w[..., 2], dim=1) 

2. add a new sensor to your SceneCfg, e.g.:

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=True,
        mesh_prim_paths=["/World/ground"],
    )

3. pass the new sensor, in this case height_scanner_base, to the reward function's parameters.

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.

Hope this helps until the issue is fixed :)

[fan-ziqi]

A proper sensor configuration would require a circular ray casting pattern that is unfortunately not yet implemented. To get a working solution before the official fix and with minimal effort I would recommend doing the following:

1. fix the reward bug like @shendredm proposed:

adjusted_target_height = target_height + torch.mean(sensor.data.ray_hits_w[..., 2], dim=1) 

2. add a new sensor to your SceneCfg, e.g.:

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=True,
        mesh_prim_paths=["/World/ground"],
    )

3. pass the new sensor, in this case height_scanner_base, to the reward function's parameters.

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.

Hope this helps until the issue is fixed :)

Thank you for your suggestion, I will open a PR to hotfix this bug