cyy/go2_rl_robotlab
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

v0.1; add commands_go2_rl_gym

Browse Source
This commit is contained in:
wty-yy
2026-04-01 21:23:00 +08:00
parent 257e69f793
commit 71b1f50370
7 changed files with 467 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
CHANGELOG.md Normal file
View File

@@ -0,0 +1,3 @@
# CHANGELOG
## v0.1 20260401
1. 新增[commands_go2_rl_gym.py](source/robot_lab/robot_lab/tasks/go2/mdp/commands_go2_rl_gym.py)，将go2_rl_gym全部命令配置对齐

54
source/robot_lab/robot_lab/tasks/go2/env_cfg.py
View File

@@ -138,47 +138,7 @@ class Go2SceneCfg(InteractiveSceneCfg):
@configclass
class CommandsCfg:
"""Command specifications for the MDP."""
base_velocity = mdp.UniformVelTerrainCmdCfg(
asset_name="robot",
resampling_time_range=(5.0, 5.0),
rel_standing_envs=0.1,
rel_heading_envs=1.0,
heading_command=False,
heading_control_stiffness=0.5,
debug_vis=True,
cycle_time=0.5,
use_phase_cmd=False,
ranges = mdp.UniformVelocityCommandCfg.Ranges(
lin_vel_x=(-0.5, 0.5), lin_vel_y=(-0.5, 0.5), ang_vel_z=(-1.0, 1.0), heading=(-1.57, 1.57)
),
terrain_max_ranges={
"pyramid_stairs": mdp.UniformVelocityCommandCfg.Ranges(
lin_vel_x=(-1.0, 1.0), lin_vel_y=(-1.0, 1.0), ang_vel_z=(-1.0, 1.0), heading=(-1.57, 1.57)
),
"pyramid_stairs_inv": mdp.UniformVelocityCommandCfg.Ranges(
lin_vel_x=(-1.0, 1.0), lin_vel_y=(-1.0, 1.0), ang_vel_z=(-1.0, 1.0), heading=(-1.57, 1.57)
),
"box": mdp.UniformVelocityCommandCfg.Ranges(
lin_vel_x=(-1.0, 1.0), lin_vel_y=(-1.0, 1.0), ang_vel_z=(-1.5, 1.5), heading=(-1.57, 1.57)
),
"random_rough": mdp.UniformVelocityCommandCfg.Ranges(
lin_vel_x=(-1.5, 1.5), lin_vel_y=(-1.0, 1.0), ang_vel_z=(-1.5, 1.5), heading=(-1.57, 1.57)
),
"flat": mdp.UniformVelocityCommandCfg.Ranges(
lin_vel_x=(-2.0, 2.0), lin_vel_y=(-1.0, 1.0), ang_vel_z=(-2.0, 2.0), heading=(-1.57, 1.57)
),
"hf_pyramid_slope": mdp.UniformVelocityCommandCfg.Ranges(
lin_vel_x=(-1.5, 1.5), lin_vel_y=(-1.0, 1.0), ang_vel_z=(-1.5, 1.5), heading=(-1.57, 1.57)
),
"hf_pyramid_slope_inv": mdp.UniformVelocityCommandCfg.Ranges(
lin_vel_x=(-1.5, 1.5), lin_vel_y=(-1.0, 1.0), ang_vel_z=(-1.5, 1.5), heading=(-1.57, 1.57)
),
},
curriculum_schedule=[
{'iter': 50000, 'lin_vel_x': [-2.0, 2.0], 'lin_vel_y': [-1.0, 1.0], 'ang_vel_yaw': [-2.0, 2.0], 'heading': [-1.57, 1.57]},
{'iter': 20000, 'lin_vel_x': [-1.0, 1.0], 'lin_vel_y': [-1.0, 1.0], 'ang_vel_yaw': [-1.5, 1.5], 'heading': [-1.57, 1.57]},
], # 根据iter倒序排布
)
base_velocity = mdp.Go2RLGymCommandCfg()
@configclass
class ActionsCfg:
@@ -528,12 +488,12 @@ class CurriculumCfg:
base_height_l2 = CurrTerm(mdp.gradual_reward_weight_modification, params={
"term_name": "base_height_l2", "initial_weight": -1.0, "final_weight": -10.0, "start_it": 0, "end_it": 5000
})
ref_stand_envs = CurrTerm(mdp.gradual_ref_stand_modification, params={
"term_name": "base_velocity", "initial": 0.0, "final": 0.1, "start_it": 0, "end_it": 1500
})
command_curr = CurrTerm(mdp.command_curriculum, params={
"command_term_name": "base_velocity", "num_steps_per_iter": 24,
})
# ref_stand_envs = CurrTerm(mdp.gradual_ref_stand_modification, params={
# "term_name": "base_velocity", "initial": 0.0, "final": 0.1, "start_it": 0, "end_it": 1500
# })
# command_curr = CurrTerm(mdp.command_curriculum, params={
# "command_term_name": "base_velocity", "num_steps_per_iter": 24,
# })
##

1
source/robot_lab/robot_lab/tasks/go2/mdp/__init__.py
View File

@@ -17,3 +17,4 @@ from .events import * # noqa: F401, F403
from .observations import * # noqa: F401, F403
from .rewards import * # noqa: F401, F403
from .utils import * # noqa: F401, F403
from .commands_go2_rl_gym import * # noqa: F401, F403

2
source/robot_lab/robot_lab/tasks/go2/mdp/commands.py
View File

@@ -24,7 +24,7 @@ from isaaclab.markers import VisualizationMarkers
from .utils import is_robot_on_terrain
if TYPE_CHECKING:
from isaaclab.envs import ManagerBasedEnv
from robot_lab.tasks.go2.env.go2_env import ManagerBasedRLEnv
class UniformVelTerrainCmd(CommandTerm):

422
source/robot_lab/robot_lab/tasks/go2/mdp/commands_go2_rl_gym.py Normal file
View File

@@ -0,0 +1,422 @@
# -*- coding: utf-8 -*-
'''
@File : commands_go2_rl_gym.py
@Time : 2026/04/01 17:16:44
@Author : wty-yy
@Version : 1.0
@Blog : https://wty-yy.github.io/
@Desc : CommandTerm for go2_rl_gym style command generation, reference to https://github.com/wty-yy/go2_rl_gym
IsaacLab CommandTerm working flow:
Env: after compute reward call command.compute(dt)
1. self._update_metrics(): update self.metrics dict for logging
2. self.time_left -= dt
3. self._resample(self.time_left <= 0)
4. self._update_command(): update command if needed
Get command from self.command property, return command, shape=(num_envs, command_dim)
Note:
1. We don't use original self._resample(env_ids) and self._resample_command(env_ids), because it will randomize time_left
2. Remove heading command
3. We don't use curriculum item to update curriculum, inplace update
'''
from __future__ import annotations # For forward reference of type hints
from typing import TYPE_CHECKING, Sequence
if TYPE_CHECKING: # Avoid circular import for type checking
from robot_lab.tasks.go2.env.go2_env import ActionDelayGo2Env
from itertools import product
import torch
from isaaclab.utils import configclass
from isaaclab.managers import CommandTerm, CommandTermCfg
from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg
from isaaclab.markers.config import BLUE_ARROW_X_MARKER_CFG, GREEN_ARROW_X_MARKER_CFG
from isaaclab.assets import Articulation
import isaaclab.utils.math as math_utils
from robot_lab.tasks.go2.mdp.utils import is_robot_on_terrain, sample_disjoint_intervals, sample_single_interval
class Go2RLGymCommand(CommandTerm):
cfg: Go2RLGymCommandCfg
_env: ActionDelayGo2Env
def __init__(self, cfg: Go2RLGymCommandCfg, env: ActionDelayGo2Env):
"""Reference: https://github.com/wty-yy/go2_rl_gym/blob/master/legged_gym/envs/base/legged_robot.py
LeggedRobot._resample_command() and LeggedRobot._post_physics_step_callback()
"""
super().__init__(cfg, env)
self.commands_xy_accumulation = torch.zeros(self.num_envs, 2, dtype=torch.float, device=self.device)
self.max_move_distance = torch.zeros(self.num_envs, dtype=torch.float, device=self.device)
self.last_is_limit_vel = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device)
self.commands = torch.zeros(self.num_envs, 3, dtype=torch.float, device=self.device) # [lin_vel_x, lin_vel_y, ang_vel_yaw]
self.command_ranges = self.cfg.ranges.to_dict()
self.env_command_ranges = {
'lin_vel_x': torch.tensor(self.command_ranges['lin_vel_x'], device=self.device).repeat(self.num_envs, 1),
'lin_vel_y': torch.tensor(self.command_ranges['lin_vel_y'], device=self.device).repeat(self.num_envs, 1),
'ang_vel_yaw': torch.tensor(self.command_ranges['ang_vel_yaw'], device=self.device).repeat(self.num_envs, 1),
}
self.max_lin_vel = max(abs(self.command_ranges["lin_vel_x"][0]), abs(self.command_ranges["lin_vel_x"][1]),
abs(self.command_ranges["lin_vel_y"][0]), abs(self.command_ranges["lin_vel_y"][1]))
self.limit_vel_comb = torch.tensor(list(product(
self.cfg.limit_vel["lin_vel_x"],
self.cfg.limit_vel["lin_vel_y"],
self.cfg.limit_vel["ang_vel_yaw"]
)), device=self.device)
self._init_terrain_infos()
self._update_env_command_ranges()
self.robot: Articulation = env.scene[cfg.asset_name]
self.zero_command_prob = 0
self.max_command_x = torch.zeros(self.num_envs, dtype=torch.float, device=self.device)
self.cfg.command_range_curriculum = sorted(self.cfg.command_range_curriculum, key=lambda x: x['iter'], reverse=True)
def __str__(self) -> str:
"""Return a string representation of the command term."""
msg = (f"""Go2RLGymCommand:\n"""
f"""Command shape: {self.commands.shape}""")
return msg
def _init_terrain_infos(self):
"""Initialize terrain types and indices for each environment."""
self.terrain_types = list(self._env.scene.terrain.cfg.terrain_generator.sub_terrains.keys())
for terrain_type in self.terrain_types:
if terrain_type not in self.cfg.terrain_max_command_ranges:
raise ValueError(f"Terrain type '{terrain_type}' is not defined in cfg.terrain_max_command_ranges.")
self.terrain_type2idx = {terrain_type: idx for idx, terrain_type in enumerate(self.terrain_types)}
self.terrain_idxs = torch.full((self.num_envs,), -1, dtype=torch.long, device=self.device)
for terrain_type in self.terrain_types:
idxs = is_robot_on_terrain(self._env, terrain_type).nonzero().flatten()
if len(idxs) > 0:
self.terrain_idxs[idxs] = self.terrain_type2idx[terrain_type]
self.terrain_length = self._env.scene.terrain.cfg.terrain_generator.size[0]
@property
def command(self) -> torch.Tensor:
return self.commands
def _update_metrics(self):
self.max_command_x[:] = self.command_ranges["lin_vel_x"][1]
self.metrics["max_command_x"] = self.max_command_x
def reset(self, env_ids: Sequence[int] | None = None):
self.time_left[env_ids] = self.cfg.resampling_time
self.commands_xy_accumulation[env_ids] = 0.0
self.max_move_distance[env_ids] = 0.0
return super().reset(env_ids)
def _resample(self, env_ids: Sequence[int]):
""" Randommly select commands of some environments
Args:
env_ids (List[int]): Environments ids for which new commands are needed
"""
env = self._env
if len(env_ids) == 0:
return
# update command curriculum with train steps
if len(self.cfg.command_range_curriculum):
current_iter = env.common_step_counter // self.cfg.num_steps_per_iter
for i in range(len(self.cfg.command_range_curriculum)-1, -1, -1): # iterate backwards to be able to pop entries
cfg = self.cfg.command_range_curriculum[i]
if current_iter >= cfg["iter"]:
self.command_ranges["lin_vel_x"] = cfg["lin_vel_x"]
self.command_ranges["lin_vel_y"] = cfg["lin_vel_y"]
self.command_ranges["ang_vel_yaw"] = cfg["ang_vel_yaw"]
self.max_lin_vel = max(abs(self.command_ranges["lin_vel_x"][0]), abs(self.command_ranges["lin_vel_x"][1]),
abs(self.command_ranges["lin_vel_y"][0]), abs(self.command_ranges["lin_vel_y"][1]))
self.cfg.command_range_curriculum.pop(i)
self._update_env_command_ranges()
print(f"Command range updated at iter {current_iter}: {self.command_ranges}")
remaining_dist = torch.clip(0.625 * self.terrain_length - torch.norm(self.commands_xy_accumulation[env_ids], dim=1) * self.cfg.resampling_time, 0.0)
self.time_left[env_ids] = self.cfg.resampling_time
if self.cfg.dynamic_resample_commands:
# arrive at boundary 0.625 times the width of the remaining distance
if ((env.max_episode_length - env.episode_length_buf[env_ids]) + 1 == 0).any():
raise ValueError("Some envs have zero remaining episode length during command resampling")
vel_low_bound = torch.clip(remaining_dist / ((env.max_episode_length - env.episode_length_buf[env_ids] + 1 + 1e-9) * env.step_dt), 0.0)
self.commands[env_ids, 0] = sample_disjoint_intervals(
env_ids,
vel_low_bound,
self.env_command_ranges["lin_vel_x"][env_ids, 0],
self.env_command_ranges["lin_vel_x"][env_ids, 1],
self.device
)
self.commands[env_ids, 1] = sample_disjoint_intervals(
env_ids,
vel_low_bound,
self.env_command_ranges["lin_vel_y"][env_ids, 0],
self.env_command_ranges["lin_vel_y"][env_ids, 1],
self.device
)
r = torch.rand(len(env_ids), device=self.device)
lower = self.env_command_ranges["ang_vel_yaw"][env_ids, 0]
upper = self.env_command_ranges["ang_vel_yaw"][env_ids, 1]
self.commands[env_ids, 2] = (upper - lower) * r + lower
else:
self.commands[env_ids, 0] = sample_single_interval(
env_ids,
self.env_command_ranges["lin_vel_x"][env_ids, 0],
self.env_command_ranges["lin_vel_x"][env_ids, 1],
self.device
)
self.commands[env_ids, 1] = sample_single_interval(
env_ids,
self.env_command_ranges["lin_vel_y"][env_ids, 0],
self.env_command_ranges["lin_vel_y"][env_ids, 1],
self.device
)
self.commands[env_ids, 2] = sample_single_interval(
env_ids,
self.env_command_ranges["ang_vel_yaw"][env_ids, 0],
self.env_command_ranges["ang_vel_yaw"][env_ids, 1],
self.device
)
# set small commands to zero
self.commands[env_ids, :2] *= (torch.norm(self.commands[env_ids, :2], dim=1) > 0.2).unsqueeze(1)
rand_prob = torch.rand(len(env_ids), device=self.device)
min_prob, max_prob = 0.0, 0.0
# set limitation lin vel
if self.cfg.limit_vel_prob > 0.0:
max_prob += self.cfg.limit_vel_prob
lim_mask = (rand_prob >= min_prob) * (rand_prob < max_prob)
lim_env_ids = env_ids[lim_mask]
if len(lim_env_ids) > 0:
change_lim_env_ids = lim_env_ids
if self.cfg.limit_vel_invert_when_continuous:
was_limited = self.last_is_limit_vel[lim_env_ids]
invert_env_ids = lim_env_ids[was_limited]
self.commands[invert_env_ids, 0] *= -1.0
self.commands[invert_env_ids, 1] *= -1.0
self.commands[invert_env_ids, 2] *= -1.0
change_lim_env_ids = lim_env_ids[~was_limited]
vel_idx = torch.randint(0, self.limit_vel_comb.shape[0], (len(change_lim_env_ids),), device=self.device)
lin_vel_x_lim = torch.where(
self.limit_vel_comb[vel_idx, 0] == -1,
self.env_command_ranges["lin_vel_x"][change_lim_env_ids, 0],
self.env_command_ranges["lin_vel_x"][change_lim_env_ids, 1],
)
lin_vel_x_lim[self.limit_vel_comb[vel_idx, 0] == 0] = 0.0
lin_vel_y_lim = torch.where(
self.limit_vel_comb[vel_idx, 1] == -1,
self.env_command_ranges["lin_vel_y"][change_lim_env_ids, 0],
self.env_command_ranges["lin_vel_y"][change_lim_env_ids, 1]
)
lin_vel_y_lim[self.limit_vel_comb[vel_idx, 1] == 0] = 0.0
ang_vel_z_lim = torch.where(
self.limit_vel_comb[vel_idx, 2] == -1,
self.env_command_ranges["ang_vel_yaw"][change_lim_env_ids, 0],
self.env_command_ranges["ang_vel_yaw"][change_lim_env_ids, 1]
)
ang_vel_z_lim[self.limit_vel_comb[vel_idx, 2] == 0] = 0.0
self.commands[change_lim_env_ids, 0] = lin_vel_x_lim
self.commands[change_lim_env_ids, 1] = lin_vel_y_lim
self.commands[change_lim_env_ids, 2] = ang_vel_z_lim
self.last_is_limit_vel[env_ids] = False
self.last_is_limit_vel[lim_env_ids] = True
else:
self.last_is_limit_vel[env_ids] = False
min_prob += self.cfg.limit_vel_prob
# set all commands to zero with some probability
if self.cfg.zero_command_curriculum is not None:
self.zero_command_prob = self.get_current_scale(self.cfg.zero_command_curriculum)
if self.zero_command_prob > 0.0:
max_prob += self.zero_command_prob
next_time_left = torch.clip(
env.max_episode_length_s - env.episode_length_buf[env_ids] * env.step_dt - (remaining_dist / (0.8 * self.max_lin_vel + 1e-9)),
min=0.0,
max=self.cfg.resampling_time,
)
zero_mask = (rand_prob >= min_prob) * (rand_prob < max_prob) * (next_time_left > 0.0)
zero_env_ids = env_ids[zero_mask]
if len(zero_env_ids) > 0:
self.commands[zero_env_ids, :2] = 0.0
self.time_left[zero_env_ids] = next_time_left[zero_mask]
if self.cfg.limit_ang_vel_at_zero_command_prob > 0.0:
ang_vel_rand = torch.rand(len(zero_env_ids), device=self.device) # independent distribution
add_ang_mask = ang_vel_rand < self.cfg.limit_ang_vel_at_zero_command_prob
add_ang_env_ids = zero_env_ids[add_ang_mask]
if len(add_ang_env_ids) > 0:
direction_rand = torch.rand(len(add_ang_env_ids), device=self.device)
self.commands[add_ang_env_ids, 2] = torch.where(
direction_rand < 0.5,
self.env_command_ranges["ang_vel_yaw"][add_ang_env_ids, 0],
self.env_command_ranges["ang_vel_yaw"][add_ang_env_ids, 1]
)
min_prob += self.zero_command_prob
self.commands_xy_accumulation[env_ids] += self.commands[env_ids, :2]
def _update_command(self):
current_dist = torch.norm(self.robot.data.root_pos_w[:, :2] - self._env.scene.env_origins[:, :2], dim=1)
self.max_move_distance = torch.max(self.max_move_distance, current_dist)
def _update_env_command_ranges(self):
""" Update environment-wise command ranges based on current command ranges and terrain type """
for terrain_type, terrain_command_ranges in self.cfg.terrain_max_command_ranges.items():
terrain_idx = self.terrain_type2idx[terrain_type]
env_ids = (self.terrain_idxs == terrain_idx).nonzero().flatten()
if len(env_ids) == 0:
continue
self.env_command_ranges['lin_vel_x'][env_ids, 0] = max(
terrain_command_ranges['lin_vel_x'][0],
self.command_ranges['lin_vel_x'][0],
)
self.env_command_ranges['lin_vel_x'][env_ids, 1] = min(
terrain_command_ranges['lin_vel_x'][1],
self.command_ranges['lin_vel_x'][1]
)
self.env_command_ranges['lin_vel_y'][env_ids, 0] = max(
terrain_command_ranges['lin_vel_y'][0],
self.command_ranges['lin_vel_y'][0]
)
self.env_command_ranges['lin_vel_y'][env_ids, 1] = min(
terrain_command_ranges['lin_vel_y'][1],
self.command_ranges['lin_vel_y'][1]
)
self.env_command_ranges['ang_vel_yaw'][env_ids, 0] = max(
terrain_command_ranges['ang_vel_yaw'][0],
self.command_ranges['ang_vel_yaw'][0]
)
self.env_command_ranges['ang_vel_yaw'][env_ids, 1] = min(
terrain_command_ranges['ang_vel_yaw'][1],
self.command_ranges['ang_vel_yaw'][1]
)
def get_current_scale(self, config: dict):
"""config: {'start_iter': 0, 'end_iter': 1500, 'start_value': 1.0, 'end_value': 0.0}"""
current_iter = self._env.common_step_counter // self.cfg.num_steps_per_iter
cfg_start_iter = config['start_iter']
cfg_end_iter = config['end_iter']
cfg_start_val = config['start_value']
cfg_end_val = config['end_value']
percentage = (current_iter - cfg_start_iter) / (cfg_end_iter - cfg_start_iter)
percentage = max(min(percentage, 1.0), 0.0)
current_scale = (1.0 - percentage) * cfg_start_val + percentage * cfg_end_val
return current_scale
"""Debug Visualization"""
def _set_debug_vis_impl(self, debug_vis: bool):
if debug_vis:
if not hasattr(self, "goal_vel_visualizer"):
self.goal_vel_visualizer = VisualizationMarkers(self.cfg.goal_vel_visualizer_cfg)
self.current_vel_visualizer = VisualizationMarkers(self.cfg.current_vel_visualizer_cfg)
self.goal_vel_visualizer.set_visibility(True)
self.current_vel_visualizer.set_visibility(True)
else:
if hasattr(self, "goal_vel_visualizer"):
self.goal_vel_visualizer.set_visibility(False)
self.current_vel_visualizer.set_visibility(False)
def _debug_vis_callback(self, event):
if not self.robot.is_initialized:
return
base_pos_w = self.robot.data.root_pos_w.clone()
base_pos_w[:, 2] += 0.5
vel_des_arrow_scale, vel_des_arrow_quat = self._resolve_xy_velocity_to_arrow(self.command[:, :2])
vel_arrow_scale, vel_arrow_quat = self._resolve_xy_velocity_to_arrow(self.robot.data.root_lin_vel_b[:, :2])
self.goal_vel_visualizer.visualize(base_pos_w, vel_des_arrow_quat, vel_des_arrow_scale)
self.current_vel_visualizer.visualize(base_pos_w, vel_arrow_quat, vel_arrow_scale)
def _resolve_xy_velocity_to_arrow(self, xy_velocity: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]:
"""Converts the XY base velocity command to arrow direction rotation."""
default_scale = self.goal_vel_visualizer.cfg.markers["arrow"].scale
arrow_scale = torch.tensor(default_scale, device=self.device).repeat(xy_velocity.shape[0], 1)
arrow_scale[:, 0] *= torch.linalg.norm(xy_velocity, dim=1) * 3.0
heading_angle = torch.atan2(xy_velocity[:, 1], xy_velocity[:, 0])
zeros = torch.zeros_like(heading_angle)
arrow_quat = math_utils.quat_from_euler_xyz(zeros, zeros, heading_angle)
base_quat_w = self.robot.data.root_quat_w
arrow_quat = math_utils.quat_mul(base_quat_w, arrow_quat)
return arrow_scale, arrow_quat
def _resample_command(self):
...
@configclass
class Go2RLGymCommandCfg(CommandTermCfg):
class_type: type = Go2RLGymCommand
asset_name: str = "robot"
"""Name of the asset in the environment for which the commands are generated."""
dynamic_resample_commands: bool = True
"""Sample commands with low bounds"""
limit_vel_invert_when_continuous: bool = True
"""Invert the limit logic when using continuous sample limit velocity commands"""
zero_command_curriculum: dict = {'start_iter': 0, 'end_iter': 1500, 'start_value': 0.0, 'end_value': 0.1}
"""Start training with zero commands and then gradually increase zero command probability"""
limit_vel: dict = {"lin_vel_x": [-1, 1], "lin_vel_y": [-1, 1], "ang_vel_yaw": [-1, 0, 1]}
"""Sample vel commands from min [-1] or zero [0] or max [1] range only"""
command_range_curriculum: list[dict] = [{
'iter': 20000, # training iteration at which the command ranges are updated
'lin_vel_x': [-1.0, 1.0], # min max [m/s]
'lin_vel_y': [-1.0, 1.0], # min max [m/s]
'ang_vel_yaw': [-1.5, 1.5], # min max [rad/s]
}, {
'iter': 50000, # training iteration at which the command ranges are updated
'lin_vel_x': [-2.0, 2.0], # min max [m/s]
'lin_vel_y': [-1.0, 1.0], # min max [m/s]
'ang_vel_yaw': [-2.0, 2.0], # min max [rad/s]
}]
"""List for command range curriculums at specific training iterations"""
terrain_max_command_ranges: dict[str, dict] = {
'random_rough':
{'lin_vel_x': [-1.5, 1.5], 'lin_vel_y': [-1.0, 1.0], 'ang_vel_yaw': [-1.5, 1.5]},
'hf_pyramid_slope':
{'lin_vel_x': [-1.5, 1.5], 'lin_vel_y': [-1.0, 1.0], 'ang_vel_yaw': [-1.5, 1.5]},
'hf_pyramid_slope_inv':
{'lin_vel_x': [-1.5, 1.5], 'lin_vel_y': [-1.0, 1.0], 'ang_vel_yaw': [-1.5, 1.5]},
'pyramid_stairs':
{'lin_vel_x': [-1.0, 1.0], 'lin_vel_y': [-1.0, 1.0], 'ang_vel_yaw': [-1.5, 1.5]},
'pyramid_stairs_inv':
{'lin_vel_x': [-1.0, 1.0], 'lin_vel_y': [-1.0, 1.0], 'ang_vel_yaw': [-1.5, 1.5]},
'boxes':
{'lin_vel_x': [-1.0, 1.0], 'lin_vel_y': [-1.0, 1.0], 'ang_vel_yaw': [-1.5, 1.5]},
'flat':
{'lin_vel_x': [-2.0, 2.0], 'lin_vel_y': [-1.0, 1.0], 'ang_vel_yaw': [-2.0, 2.0]},
}
resampling_time: float = 5.0
resampling_time_range: tuple[float, float] = (5.0, 5.0)
"""Time before command are changed [s]"""
limit_ang_vel_at_zero_command_prob: float = 0.2
"""Probability of add limiting angular velocity commands when zero command is sampled"""
limit_vel_prob: float = 0.2
"""Probability of limiting linear velocity command"""
num_steps_per_iter: int = 24
"""Number of envs steps for each training iteration"""
@configclass
class Ranges:
lin_vel_x: tuple[float, float] = [-0.5, 0.5]
"""Range for the linear-x velocity command [m/s]"""
lin_vel_y: tuple[float, float] = [-0.5, 0.5]
"""Range for the linear-y velocity command [m/s]"""
ang_vel_yaw: tuple[float, float] = [-1.0, 1.0]
"""Range for the angular-z velocity command [rad/s]"""
ranges: Ranges = Ranges()
goal_vel_visualizer_cfg: VisualizationMarkersCfg = GREEN_ARROW_X_MARKER_CFG.replace(
prim_path="/Visuals/Command/velocity_goal"
)
"""The configuration for the goal velocity visualization marker. Defaults to GREEN_ARROW_X_MARKER_CFG."""
current_vel_visualizer_cfg: VisualizationMarkersCfg = BLUE_ARROW_X_MARKER_CFG.replace(
prim_path="/Visuals/Command/velocity_current"
)
"""The configuration for the current velocity visualization marker. Defaults to BLUE_ARROW_X_MARKER_CFG."""

7
source/robot_lab/robot_lab/tasks/go2/mdp/curriculums.py
View File

@@ -15,6 +15,7 @@ from typing import TYPE_CHECKING
if TYPE_CHECKING:
from isaaclab.envs import ManagerBasedRLEnv
from robot_lab.tasks.go2.mdp.commands_go2_rl_gym import Go2RLGymCommand
def command_levels_lin_vel(
@@ -200,13 +201,13 @@ def terrain_levels_vel_gym(env: ManagerBasedRLEnv, env_ids: Sequence[int]) -> fl
使用 max_move_distance 而非 reset 时的瞬间位移, 比较标准基于 commands_xy_accumulation
"""
terrain = env.scene.terrain
command = env.command_manager.get_term("base_velocity")
command: Go2RLGymCommand = env.command_manager.get_term("base_velocity")
max_move_dist = command.max_move_distance[env_ids]
cmd_accum = command.commands_xy_accumulation[env_ids]
resampling_time = command.cfg.resampling_time_range[0]
zero_prob = command.cfg.rel_standing_envs
resampling_time = command.cfg.resampling_time
zero_prob = command.zero_command_prob
move_up = max_move_dist > terrain.cfg.terrain_generator.size[0] / 2
target_dist = torch.norm(cmd_accum, dim=1) * (resampling_time * (1 - zero_prob))

51
source/robot_lab/robot_lab/tasks/go2/mdp/utils.py
View File

@@ -70,10 +70,11 @@ def is_env_assigned_to_terrain(env: ManagerBasedEnv, terrain_name: str) -> torch
def is_robot_on_terrain(env: ManagerBasedEnv, terrain_name: str, asset_name: str = "robot") -> torch.Tensor:
"""Check which robots are currently standing on the specified terrain type.
"""Check which environments are currently assigned to the specified terrain type.
This function calculates which terrain grid cell each robot is on based on its world position,
then checks if that cell's terrain type matches the specified terrain.
The terrain importer tracks the active terrain column for every environment.
This helper uses that assignment directly instead of inferring terrain membership
from robot world positions.
Args:
env: The environment instance.
@@ -97,30 +98,32 @@ def is_robot_on_terrain(env: ManagerBasedEnv, terrain_name: str, asset_name: str
col_start, col_end = col_range
# Get robot positions in world frame
asset = env.scene[asset_name]
robot_pos_w = asset.data.root_pos_w[:, :2] # [num_envs, 2] (x, y)
# The terrain importer already tracks the active terrain column for each environment.
# Using that source of truth keeps this aligned with curriculum updates and avoids
# misclassifying robots by searching the nearest tile in world coordinates.
del asset_name
return (terrain.terrain_types >= col_start) & (terrain.terrain_types < col_end)
# Get terrain grid information
terrain_origins = terrain.terrain_origins # [num_rows, num_cols, 3]
num_rows, num_cols, _ = terrain_origins.shape
# Use terrain_origins to directly compute which cell each robot is in
# terrain_origins[r, c, :2] is the center of cell (r, c)
# We need to find the closest terrain origin for each robot
"""Commands Utilities"""
# Reshape terrain_origins for distance calculation
terrain_origins_2d = terrain_origins[:, :, :2].reshape(num_rows * num_cols, 2) # [num_rows*num_cols, 2]
def sample_disjoint_intervals(env_ids, limit_bound, cfg_min, cfg_max, device):
"""Sample uniform distribution from [cfg_min, -limit_bound] U [limit_bound, cfg_max]"""
width_neg = torch.nn.functional.relu(-limit_bound - cfg_min)
width_pos = torch.nn.functional.relu(cfg_max - limit_bound)
# Calculate distances from each robot to all terrain origins
distances = torch.cdist(robot_pos_w, terrain_origins_2d) # [num_envs, num_rows*num_cols]
total_width = width_neg + width_pos + 1e-6 # 加极小值防除零
u = torch.rand(len(env_ids), device=device) * total_width
# Find the closest terrain origin for each robot
closest_flat_idx = torch.argmin(distances, dim=1) # [num_envs]
samples = torch.where(
u < width_neg,
cfg_min + u,
cfg_max - width_pos + (u - width_neg)
)
return samples
# Convert flat index to column index
# flat_idx = row * num_cols + col
col_idx = closest_flat_idx % num_cols # [num_envs]
# Check if the robot's current terrain column is in the specified terrain's range
return (col_idx >= col_start) & (col_idx < col_end)
def sample_single_interval(env_ids, cfg_min, cfg_max, device):
"""Sample uniform distribution from [cfg_min, cfg_max]"""
r = torch.rand(len(env_ids), device=device)
samples = cfg_min + r * (cfg_max - cfg_min)
return samples