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

fan-ziqi · 2025-01-18T08:53:55Z

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

The text was updated successfully, but these errors were encountered:

zoctipus · 2025-01-18T12:02:51Z

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 · 2025-01-18T12:34:04Z

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 · 2025-01-18T13:34:44Z

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 · 2025-01-18T13:46:26Z

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 · 2025-01-18T13:48:18Z

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 · 2025-01-18T13:50:38Z

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 · 2025-01-21T15:03:28Z

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 · 2025-01-21T15:09:37Z

Is there anything I can help improve?

shendredm · 2025-01-21T15:44:56Z

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 · 2025-01-22T12:17:57Z

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:

fix the reward bug like @shendredm proposed:

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

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"],
    )

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 · 2025-01-22T12:54:04Z

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:

fix the reward bug like @shendredm proposed:
adjusted_target_height = target_height + torch.mean(sensor.data.ray_hits_w[..., 2], dim=1) 
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"],
    )
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

@ClemensSchwarke

…#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

fan-ziqi · 2025-01-24T12:11:04Z

The PR #1710 cause a new bug, so reopen this issue

I used the code that @ClemensSchwarke suggested above, and after a period of training, the following problems will occur:

################################################################################
 Learning iteration 2586/5000 

Computation: 85132 steps/s (collection: 1.089s, learning 0.066s)
Value function loss: 0.1565
Surrogate loss: -0.0064
Mean action noise std: 0.76
Mean reward: 44.35
Mean episode length: 804.99
Episode_Reward/lin_vel_z_l2: -0.0784
Episode_Reward/ang_vel_xy_l2: -0.4739
Episode_Reward/base_height_l2: -0.0072
Episode_Reward/joint_torques_l2: -0.0109
Episode_Reward/joint_acc_l2: -0.2853
Episode_Reward/joint_pos_limits: -0.0104
Episode_Reward/action_rate_l2: -0.2222
Episode_Reward/undesired_contacts: -0.0567
Episode_Reward/track_lin_vel_xy_exp: 0.4555
Episode_Reward/track_ang_vel_z_exp: 0.2709
Episode_Reward/feet_air_time: 0.1819
Episode_Reward/feet_gait: 2.1170
Episode_Reward/feet_height_exp: 0.9502
Episode_Reward/joint_power: -0.0029
Episode_Reward/stand_still_when_zero_command: -0.0909
Episode_Reward/joint_position_penalty: -0.5345
Curriculum/terrain_levels: 4.4967
Metrics/base_velocity/error_vel_xy: 0.9245
Metrics/base_velocity/error_vel_yaw: 1.0580
Episode_Termination/time_out: 3.0000
Episode_Termination/terrain_out_of_bounds: 0.0000
Episode_Termination/illegal_contact: 1.3333
--------------------------------------------------------------------------------
Total timesteps: 254312448
Iteration time: 1.15s
Total time: 2878.07s
ETA: 2685.6s

Error executing job with overrides: []
Traceback (most recent call last):
File "/home/ubuntu/workspaces/IsaacLab/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/utils/hydra.py", line 99, in hydra_main
func(env_cfg, agent_cfg, *args, **kwargs)
File "/home/ubuntu/workspaces/robot_lab/scripts/rsl_rl/base/train.py", line 146, in main
runner.learn(num_learning_iterations=agent_cfg.max_iterations, init_at_random_ep_len=True)
File "/home/ubuntu/workspaces/robot_lab/exts/robot_lab/robot_lab/third_party/rsl_rl/runners/on_policy_runner.py", line 149, in learn
mean_value_loss, mean_surrogate_loss = self.alg.update()
File "/home/ubuntu/workspaces/robot_lab/exts/robot_lab/robot_lab/third_party/rsl_rl/algorithms/ppo.py", line 121, in update
self.actor_critic.act(obs_batch, masks=masks_batch, hidden_states=hid_states_batch[0])
File "/home/ubuntu/workspaces/robot_lab/exts/robot_lab/robot_lab/third_party/rsl_rl/modules/actor_critic.py", line 104, in act
self.update_distribution(observations)
File "/home/ubuntu/workspaces/robot_lab/exts/robot_lab/robot_lab/third_party/rsl_rl/modules/actor_critic.py", line 101, in update_distribution
self.distribution = Normal(mean, mean * 0.0 + self.std)
File "/home/ubuntu/anaconda3/envs/lab/lib/python3.10/site-packages/torch/distributions/normal.py", line 57, in __init__
super().__init__(batch_shape, validate_args=validate_args)
File "/home/ubuntu/anaconda3/envs/lab/lib/python3.10/site-packages/torch/distributions/distribution.py", line 70, in __init__
raise ValueError(
ValueError: Expected parameter loc (Tensor of shape (24576, 12)) of distribution Normal(loc: torch.Size([24576, 12]), scale: torch.Size([24576, 12])) to satisfy the constraint Real(), but found invalid values:
tensor([[nan, nan, nan,  ..., nan, nan, nan],
[nan, nan, nan,  ..., nan, nan, nan],
[nan, nan, nan,  ..., nan, nan, nan],
...,
[nan, nan, nan,  ..., nan, nan, nan],
[nan, nan, nan,  ..., nan, nan, nan],
[nan, nan, nan,  ..., nan, nan, nan]], device='cuda:0',
grad_fn=<AddmmBackward0>)

Set the environment variable HYDRA_FULL_ERROR=1 for a complete stack trace.

jaykorea · 2025-01-24T12:26:39Z

That's because of robot fall out of the terrain.

you should handle termination condition when robot's out of border

shendredm · 2025-01-24T13:51:24Z

Either we need to design some termination based on height sensor data or we can simply use this termination function.
reference - source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/con
fig/spot/flat_env_cfg.py

terrain_out_of_bounds = DoneTerm( func=mdp.terrain_out_of_bounds, params={"asset_cfg": SceneEntityCfg("robot"), "distance_buffer": 3.0}, time_out=True, )

fan-ziqi · 2025-01-24T13:54:26Z

Either we need to design some termination based on height sensor data or we can simply use this termination function. reference - source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/con fig/spot/flat_env_cfg.py

terrain_out_of_bounds = DoneTerm( func=mdp.terrain_out_of_bounds, params={"asset_cfg": SceneEntityCfg("robot"), "distance_buffer": 3.0}, time_out=True, )

I have already used this terrain_out_of_bounds in the above training @jaykorea @shendredm

jaykorea · 2025-01-24T15:42:50Z

Do you use height scan in observations? Sometimes height scan outputs none, so you should properly clip it

fan-ziqi · 2025-01-24T15:52:16Z

I used this in the critic obs, but when I deleted this reward func, the error disappeared, so it shouldn't be caused by this.

By the way, why does it output nan sometimes?

jaykorea · 2025-01-24T16:14:12Z

Mine works really fine,

I think you should reinspect the termination out of bound condition or clip the height scan to -1.0 to 1.0

fan-ziqi · 2025-01-26T02:19:55Z

Why you clip the height scan to -1.0 to 1.0?

RandomOakForest added the bug Something isn't working label Jan 21, 2025

fan-ziqi mentioned this issue Jan 22, 2025

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

Merged

6 tasks

fan-ziqi closed this as completed Jan 23, 2025

fan-ziqi reopened this Jan 24, 2025

fan-ziqi linked a pull request Jan 26, 2025 that will close this issue

Fixes handling of invalid values for sensor data #1727

Open

6 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

fan-ziqi commented Jan 18, 2025 •

edited

Loading

zoctipus commented Jan 18, 2025 •

edited

Loading

fan-ziqi commented Jan 18, 2025

zoctipus commented Jan 18, 2025 •

edited

Loading

shendredm commented Jan 18, 2025 •

edited

Loading

fan-ziqi commented Jan 18, 2025

fan-ziqi commented Jan 18, 2025

RandomOakForest commented Jan 21, 2025

fan-ziqi commented Jan 21, 2025

shendredm commented Jan 21, 2025 •

edited

Loading

ClemensSchwarke commented Jan 22, 2025

fan-ziqi commented Jan 22, 2025

fan-ziqi commented Jan 24, 2025 •

edited

Loading

jaykorea commented Jan 24, 2025 •

edited

Loading

shendredm commented Jan 24, 2025

fan-ziqi commented Jan 24, 2025

jaykorea commented Jan 24, 2025

fan-ziqi commented Jan 24, 2025

jaykorea commented Jan 24, 2025

fan-ziqi commented Jan 26, 2025

[Bug Report] base_height_l2 for rough terrain is wrong! #1698

[Bug Report] base_height_l2 for rough terrain is wrong! #1698

Comments

fan-ziqi commented Jan 18, 2025 • edited Loading

Describe the bug

Checklist

zoctipus commented Jan 18, 2025 • edited Loading

fan-ziqi commented Jan 18, 2025

zoctipus commented Jan 18, 2025 • edited Loading

shendredm commented Jan 18, 2025 • edited Loading

fan-ziqi commented Jan 18, 2025

fan-ziqi commented Jan 18, 2025

RandomOakForest commented Jan 21, 2025

fan-ziqi commented Jan 21, 2025

shendredm commented Jan 21, 2025 • edited Loading

ClemensSchwarke commented Jan 22, 2025

fan-ziqi commented Jan 22, 2025

fan-ziqi commented Jan 24, 2025 • edited Loading

jaykorea commented Jan 24, 2025 • edited Loading

shendredm commented Jan 24, 2025

fan-ziqi commented Jan 24, 2025

jaykorea commented Jan 24, 2025

fan-ziqi commented Jan 24, 2025

jaykorea commented Jan 24, 2025

fan-ziqi commented Jan 26, 2025

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

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

fan-ziqi commented Jan 18, 2025 •

edited

Loading

zoctipus commented Jan 18, 2025 •

edited

Loading

zoctipus commented Jan 18, 2025 •

edited

Loading

shendredm commented Jan 18, 2025 •

edited

Loading

shendredm commented Jan 21, 2025 •

edited

Loading

fan-ziqi commented Jan 24, 2025 •

edited

Loading

jaykorea commented Jan 24, 2025 •

edited

Loading