diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..ceb17d7 --- /dev/null +++ b/CHANGELOG.md @@ -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全部命令配置对齐 diff --git a/source/robot_lab/robot_lab/tasks/go2/env_cfg.py b/source/robot_lab/robot_lab/tasks/go2/env_cfg.py index 403c4b2..a430638 100644 --- a/source/robot_lab/robot_lab/tasks/go2/env_cfg.py +++ b/source/robot_lab/robot_lab/tasks/go2/env_cfg.py @@ -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, + # }) ## diff --git a/source/robot_lab/robot_lab/tasks/go2/mdp/__init__.py b/source/robot_lab/robot_lab/tasks/go2/mdp/__init__.py index bda76aa..7f57ddb 100644 --- a/source/robot_lab/robot_lab/tasks/go2/mdp/__init__.py +++ b/source/robot_lab/robot_lab/tasks/go2/mdp/__init__.py @@ -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 diff --git a/source/robot_lab/robot_lab/tasks/go2/mdp/commands.py b/source/robot_lab/robot_lab/tasks/go2/mdp/commands.py index b512bfc..b0b99fd 100644 --- a/source/robot_lab/robot_lab/tasks/go2/mdp/commands.py +++ b/source/robot_lab/robot_lab/tasks/go2/mdp/commands.py @@ -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): diff --git a/source/robot_lab/robot_lab/tasks/go2/mdp/commands_go2_rl_gym.py b/source/robot_lab/robot_lab/tasks/go2/mdp/commands_go2_rl_gym.py new file mode 100644 index 0000000..480e380 --- /dev/null +++ b/source/robot_lab/robot_lab/tasks/go2/mdp/commands_go2_rl_gym.py @@ -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.""" diff --git a/source/robot_lab/robot_lab/tasks/go2/mdp/curriculums.py b/source/robot_lab/robot_lab/tasks/go2/mdp/curriculums.py index 074ebe9..17bc5da 100644 --- a/source/robot_lab/robot_lab/tasks/go2/mdp/curriculums.py +++ b/source/robot_lab/robot_lab/tasks/go2/mdp/curriculums.py @@ -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)) diff --git a/source/robot_lab/robot_lab/tasks/go2/mdp/utils.py b/source/robot_lab/robot_lab/tasks/go2/mdp/utils.py index 9c0229d..61636a9 100644 --- a/source/robot_lab/robot_lab/tasks/go2/mdp/utils.py +++ b/source/robot_lab/robot_lab/tasks/go2/mdp/utils.py @@ -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) + + total_width = width_neg + width_pos + 1e-6 # 加极小值防除零 + u = torch.rand(len(env_ids), device=device) * total_width + + samples = torch.where( + u < width_neg, + cfg_min + u, + cfg_max - width_pos + (u - width_neg) + ) + return samples - # Calculate distances from each robot to all terrain origins - distances = torch.cdist(robot_pos_w, terrain_origins_2d) # [num_envs, num_rows*num_cols] - - # Find the closest terrain origin for each robot - closest_flat_idx = torch.argmin(distances, dim=1) # [num_envs] - - # 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