Robocon/RC_WheelLeg
4
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

[train] 整理比赛最终训练架构

Browse Source
This commit is contained in:
eastzio
2026-07-27 12:48:12 +08:00
parent 60f7a08e91
commit d8c5d34091
13 changed files with 1106 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
01_doc/training_evolution.md
+37
View File
@@ -0,0 +1,37 @@
# 训练代码演进
本项目将“训练代码架构”和“训练产生的模型 checkpoint”分别管理。代码、奖励、课程或观测契约发生变化时形成新的 Git 版本；同一架构下继续训练产生的模型编号作为实验和部署工件记录。
| 版本 | 来源快照 | 主要目的 |
| --- | --- | --- |
| `v0.4.0` | `uni_mjlab(1)` | 第一份完整的新 MJCF 与新 mjlab 训练工程 |
| `v0.5.0` | `uni_mjlab_new` | 扩大观测、延迟和动力学随机化，探索更强 Sim2Real 鲁棒性 |
| `v0.6.0` | `best` | 面向比赛越障重新设计奖励、课程、站姿和诊断体系 |
## 从随机化增强到比赛训练
`v0.6.0` 不是简单地继续增大 `v0.5.0` 的随机范围。实践中重新降低了部分噪声、延迟和动力学随机化强度，并把训练重点转向可控的比赛任务课程：
- 投影重力噪声由 `±0.08` 调回 `±0.05`。
- 腿和轮动作最大随机延迟由 4 步调回 2 步。
- 摩擦、刚度和阻尼随机化调回较窄范围。
- 移除该阶段的连续机身外力扰动与腿部质量随机化。
- 将 x、y 和 yaw 跟踪拆成独立奖励和独立指令课程。
- 障碍由课程逐步释放，高墙训练地形改为五道重复横墙。
- 增加楼梯侧向漂移和偏航漂移惩罚。
- 增加大量只用于训练评估的误差、轮速、姿态和接触指标。
这种调整反映的是从“广泛鲁棒性探索”转向“比赛场景定向优化”，不表示 `v0.5.0` 被删除；它仍由对应 Tag 完整保留。
## 模型与比赛部署
比赛训练可能先获得基模，再修改参数继续训练和筛选 checkpoint。最终关系为：
```text
训练代码架构：v0.6.0 / best
比赛 Rough 策略：model_6800.onnx
比赛真机工程：last_not_slalom_1050
比赛得分：1050
```
`model_6800.onnx` 是比赛最终部署工件，不用模型编号替代训练代码版本号。它将在最终比赛部署版本中与运行配置一起归档。
01_doc/version_history.md
+14
View File
@@ -10,6 +10,7 @@
| `v0.3.1` | 实机记录 | 补充第一代 Sim2Real 实机视频 |
| `v0.4.0` | 新训练基线 | 第一份完整的新 MJCF、新 mjlab 框架和 Rough 策略工程 |
| `v0.5.0` | 随机化增强 | 扩大观测、延迟和动力学随机化，加入持续外力扰动 |
| `v0.6.0` | 比赛训练架构 | 分轴奖励、自适应指令课程、障碍释放课程和比赛站姿 |
## `v0.4.0` 的模型变化
@@ -28,3 +29,16 @@
- 执行器刚度和阻尼缩放由 `0.9–1.1` 扩大到 `0.5–1.5`。
- 增加膝部和轮部质量的 `0.7–1.3` 随机缩放。
- 增加作用于机身的连续随机外力和力矩扰动。
## `v0.6.0` 的比赛训练架构
- 保持 `v0.4.0` 引入的新 MJCF 和 mjlab 框架不变。
- 将线速度奖励拆分为 x/y 两轴，并独立配置偏航角速度奖励。
- 增加自适应 x/y/yaw 指令范围课程。
- 增加障碍地形逐步释放与更严格的地形晋级逻辑。
- 增加楼梯横向速度和偏航漂移约束。
- 默认站姿调整为髋俯仰 `0.550`、膝关节 `-1.125`，初始机身高度为 `0.42 m`。
- 增加速度误差、轮速跟踪、动作和姿态等训练诊断指标。
- 该 Tag 保存比赛训练代码架构，不把每次继续训练产生的 checkpoint 误记为新的软件版本。
训练阶段的详细关系见 [`training_evolution.md`](training_evolution.md)。
05_software/README.md
+1 -1
View File
@@ -26,7 +26,7 @@ MJCF + mjlab task
IK real --------------------------------> 电机
```
`rc_mjlab` 是自包含工程。训练、MJCF、Sim2Sim 和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，增强 Sim2Real 随机化的第二版训练配置见 `v0.5.0`。
`rc_mjlab` 是自包含工程。训练、MJCF、Sim2Sim 和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`。
详细说明见：
05_software/train/README.md
+3 -1
View File
@@ -2,7 +2,7 @@
`rc_mjlab/` 保存 16DOF 轮足机器人的当前训练与 Sim2Sim 工程。历史快照由 Git Tag 保留，不在目录中复制 `old`、`new` 或 `final` 版本。
当前内容对应 `v0.5.0`，在 `v0.4.0` 的完整新版训练工程上增强了面向 Sim2Real 的随机化配置。
当前内容对应 `v0.6.0`，是比赛使用的最终训练代码架构。训练过程可能先获得基模，再调整奖励、课程和环境参数继续训练；模型 checkpoint 的变化不等同于软件架构变化。
## 内容
@@ -17,4 +17,6 @@
与 `v0.4.0` 相比，本版本没有再次修改 MJCF 和训练框架，只调整训练环境配置：投影重力噪声从 `±0.05` 扩大到 `±0.08`，最大动作延迟从 2 步增加到 4 步，扩大摩擦、刚度和阻尼随机化，增加腿部质量随机化与连续外力/力矩扰动。
`v0.6.0` 在 `v0.5.0` 之后转向比赛任务优化：降低部分过强随机化，加入分轴速度跟踪奖励、自适应指令课程、障碍地形释放课程、楼梯横向/偏航约束以及更完整的训练诊断。详细对比见 [`../../01_doc/training_evolution.md`](../../01_doc/training_evolution.md)。
工程命令和任务说明见 [`rc_mjlab/README.md`](rc_mjlab/README.md)，本地依赖来源见 [`rc_mjlab/DEPENDENCIES.md`](rc_mjlab/DEPENDENCIES.md)。
05_software/train/rc_mjlab/README.md
+1 -1
View File
@@ -2,7 +2,7 @@
基于 [mjlab](https://github.com/google-deepmind/mjlab) 框架的四轮腿混合机器人强化学习训练与部署部署项目，面向机器人竞赛场景（如越障、匍匐、斜坡、台阶等复合任务）。
> 本目录对应 `v0.5.0`：在第一份完整的新 MJCF 与新框架训练工程上，扩大观测噪声、动作延迟和动力学随机化范围，并加入持续外力扰动。该快照包含 `model_rough.pt`；未包含独立 `mujoco_sim` 工具和单独的 Crawl 策略权重，相关早期内容仍可通过 `v0.3.0` 查看。
> 本目录对应 `v0.6.0`：比赛使用的最终训练架构。它在前两版训练代码上重新平衡随机化强度，引入分轴速度奖励、自适应命令课程、障碍逐步释放、楼梯稳定约束和训练诊断指标。当前目录中的 `model_rough.pt` 是早期参考权重；比赛最终使用的 `model_6800.onnx` 将随最终部署版本归档。
---
05_software/train/rc_mjlab/src/robot/config/env_cfgs.py
+240 -55
View File
@@ -52,7 +52,11 @@ from ..mdp.only_positive_rewards import enable_only_positive_rewards
from ..mdp.rewards import (
track_linear_velocity,
track_linear_velocity_l1,
track_linear_velocity_x,
track_linear_velocity_y,
track_angular_velocity,
track_angular_velocity_z,
stair_lateral_yaw_drift_l2,
base_height_l2,
safe_base_lin_vel,
safe_foot_contact,
@@ -79,8 +83,34 @@ from ..mdp.rewards import (
ang_vel_xy_l2,
undesired_contacts,
contact_forces,
tracking_lin_vel_error,
tracking_yaw_vel_error,
tracking_lin_vel_x_error,
tracking_lin_vel_y_error,
tracking_lin_vel_along_command_error,
actual_lin_vel_orthogonal_command_mean,
command_lin_vel_mean,
command_yaw_vel_abs_mean,
actual_lin_vel_mean,
tracking_lin_vel_error_band_mean,
tracking_lin_vel_axis_error_band_mean,
command_band_active,
wheel_raw_action_abs_mean,
wheel_target_vel_abs_mean,
wheel_actual_vel_abs_mean,
wheel_target_actual_vel_error_mean,
wheel_actual_to_target_vel_ratio_mean,
wheel_target_actual_sign_agreement,
upright_metric,
base_ground_contact_metric,
)
from ..mdp.curriculums import (
command_axis_levels_vel,
command_levels_adaptive,
terrain_levels_obstacle_release,
terrain_levels_ramp_strict,
terrain_levels_vel_strict,
)
from ..mdp.curriculums import terrain_levels_vel_strict
from ..mdp.commands import UniformThresholdVelocityCommandCfg
# Constant Definitions
@@ -153,7 +183,7 @@ def _make_base_env_cfg() -> ManagerBasedRlEnvCfg:
),
"projected_gravity": ObservationTermCfg(
func=velocity_mdp.projected_gravity,
noise=Unoise(n_min=-0.08, n_max=0.08),
noise=Unoise(n_min=-0.05, n_max=0.05),
),
"command": ObservationTermCfg(
func=velocity_mdp.generated_commands,
@@ -212,13 +242,13 @@ def _make_base_env_cfg() -> ManagerBasedRlEnvCfg:
actuator_names=(".*_hip_abduction_joint", ".*_hip_pitch_joint", ".*_knee_joint"),
scale={".*_hip_abduction_joint": 0.125, "^(?!.*_hip_abduction_joint).*": 0.25}, use_default_offset=True,
control_frequency=50.0, cut_off_frequency=5.0,
min_delay=0, max_delay=4,
min_delay=0, max_delay=2,
),
"wheel_joint_vel": JointVelocityDelayedLowPassActionCfg(
entity_name="robot", actuator_names=(".*_wheel_joint",),
scale=5.0, offset=0.0, use_default_offset=False,
control_frequency=50.0, cut_off_frequency=15.0,
min_delay=0, max_delay=4,
min_delay=0, max_delay=2,
),
}
@@ -262,20 +292,20 @@ def _make_base_env_cfg() -> ManagerBasedRlEnvCfg:
),
"base_com": EventTermCfg(
func=envs_dr.body_com_offset, mode="startup",
params={"asset_cfg": SceneEntityCfg("robot", body_names=(".*",)),
params={"asset_cfg": SceneEntityCfg("robot", body_names=("base_link",)),
"operation": "add", "ranges": {0: (-0.05, 0.05), 1: (-0.05, 0.05), 2: (-0.05, 0.05)}},
),
"body_friction": EventTermCfg(
func=envs_dr.geom_friction, mode="startup",
params={"asset_cfg": SceneEntityCfg("robot", geom_names=(".*",)), "operation": "abs", "ranges": (0.15, 1.25)},
params={"asset_cfg": SceneEntityCfg("robot", geom_names=(".*",)), "operation": "abs", "ranges": (0.3, 1.0)},
),
"actuator_stiffness": EventTermCfg(
func=envs_dr.joint_stiffness, mode="startup",
params={"asset_cfg": SceneEntityCfg("robot"), "ranges": (0.5, 1.5), "operation": "scale", "distribution": "log_uniform"},
params={"asset_cfg": SceneEntityCfg("robot"), "ranges": (0.9, 1.1), "operation": "scale", "distribution": "log_uniform"},
),
"actuator_damping": EventTermCfg(
func=envs_dr.joint_damping, mode="startup",
params={"asset_cfg": SceneEntityCfg("robot"), "ranges": (0.5, 1.5), "operation": "scale", "distribution": "log_uniform"},
params={"asset_cfg": SceneEntityCfg("robot"), "ranges": (0.9, 1.1), "operation": "scale", "distribution": "log_uniform"},
),
"body_mass_base": EventTermCfg(
func=envs_dr.body_mass, mode="startup",
@@ -285,24 +315,6 @@ def _make_base_env_cfg() -> ManagerBasedRlEnvCfg:
"ranges": (-1.0, 3.0),
},
),
"body_mass_limbs": EventTermCfg(
func=envs_dr.body_mass, mode="startup",
params={
"asset_cfg": SceneEntityCfg("robot", body_names=(".*_knee_.*", ".*_wheel_.*")),
"operation": "scale",
"ranges": (0.7, 1.3),
},
),
"apply_continuous_disturbance": EventTermCfg(
func=apply_continuous_disturbance, mode="step",
params={
"asset_cfg": SceneEntityCfg("robot", body_names=("base_link",)),
"force_range": (-10.0, 10.0),
"torque_range": (-10.0, 10.0),
"resample_time_range": (5.0, 10.0),
"time_constant": 1.0,
},
),
}
# ------------------
@@ -385,13 +397,18 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
terrain_generator=TerrainGeneratorCfg(
size=(8.0, 8.0), border_width=20.0, num_rows=10, num_cols=20, curriculum=True,
sub_terrains={
"flat": BoxFlatTerrainCfg(proportion=0.05, size=(8.0, 8.0)),
"pyramid_stairs": BoxPyramidStairsTerrainCfg(proportion=0.05, step_height_range=(0.0, 0.3), step_width=0.30, size=(8.0, 8.0)),
"pyramid_stairs_inv": BoxInvertedPyramidStairsTerrainCfg(proportion=0.45, step_height_range=(0.0, 0.3), step_width=0.30, size=(8.0, 8.0)),
"random_grid": BoxRandomGridTerrainCfg(proportion=0.27, grid_width=0.45, grid_height_range=(0.0, 0.3), size=(8.0, 8.0)),
"flat": BoxFlatTerrainCfg(proportion=0.15, size=(8.0, 8.0)),
"pyramid_stairs": BoxPyramidStairsTerrainCfg(proportion=0.05, step_height_range=(0.0, 0.20), step_width=0.30, size=(8.0, 8.0)),
"pyramid_stairs_inv": BoxInvertedPyramidStairsTerrainCfg(proportion=0.35, step_height_range=(0.0, 0.20), step_width=0.30, size=(8.0, 8.0)),
"random_grid": BoxRandomGridTerrainCfg(proportion=0.27, grid_width=0.45, grid_height_range=(0.0, 0.20), size=(8.0, 8.0)),
"random_rough": HfRandomUniformTerrainCfg(proportion=0.01, noise_range=(0.0, 0.06), noise_step=0.01, horizontal_scale=0.20, downsampled_scale=0.20, border_width=0.25, base_thickness_ratio=100.0, size=(8.0, 8.0)),
"perlin_noise": HfPerlinNoiseTerrainCfg(proportion=0.01, height_range=(0.0, 0.06), octaves=2, persistence=0.4, lacunarity=2.0, horizontal_scale=0.20, resolution=0.20, border_width=0.50, base_thickness_ratio=100.0, size=(8.0, 8.0)),
"rc_wall": RCWallTerrainCfg(proportion=0.15, wall_height_range=(0.0, 0.45), size=(8.0, 8.0)),
"rc_wall": RCWallTerrainCfg(
proportion=0.15,
wall_height_range=(0.10, 0.35),
wall_centers_x=(2.1, 3.2, 4.3, 5.4, 6.5),
size=(8.0, 8.0),
),
"sloped_terrain": HfPyramidSlopedTerrainCfg(proportion=0.01, slope_range=(0.052, 0.325), platform_width=2.0, border_width=0.25, base_thickness_ratio=100.0, horizontal_scale=0.20, size=(8.0, 8.0)),
},
),
@@ -400,15 +417,65 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
# Keep the custom terrain set, but align command/curriculum behavior with go2w rough.
cfg.curriculum.pop("command_vel", None)
cfg.curriculum["terrain_levels"] = CurriculumTermCfg(func=velocity_mdp.terrain_levels_vel, params={"command_name": "twist"})
cfg.curriculum["terrain_levels"] = CurriculumTermCfg(
func=terrain_levels_obstacle_release,
params={
"command_name": "twist",
"initial_terrain_names": ("flat", "random_rough", "perlin_noise", "sloped_terrain", "pyramid_stairs"),
"release_schedule": (
(200 * 24, ("random_grid",)),
(500 * 24, ("pyramid_stairs_inv",)),
(700 * 24, ("rc_wall",)),
),
},
)
cfg.curriculum["command_x_levels"] = CurriculumTermCfg(
func=command_levels_adaptive,
params={
"command_name": "twist",
"reward_term_name": "track_lin_vel_x_exp",
"axis": "x",
"initial_range": (-0.5, 0.5),
"delta_command": 0.05,
"target_ratio": 0.8,
"ema_alpha": 0.5,
},
)
cfg.curriculum["command_y_levels"] = CurriculumTermCfg(
func=command_levels_adaptive,
params={
"command_name": "twist",
"reward_term_name": "track_lin_vel_y_exp",
"axis": "y",
"initial_range": (-0.5, 0.5),
"delta_command": 0.05,
"target_ratio": 0.8,
"ema_alpha": 0.5,
},
)
cfg.curriculum["command_yaw_levels"] = CurriculumTermCfg(
func=command_levels_adaptive,
params={
"command_name": "twist",
"reward_term_name": "track_ang_vel_z_exp",
"axis": "yaw",
"initial_range": (-0.5, 0.5),
"delta_command": 0.05,
"target_ratio": 0.8,
"ema_alpha": 0.5,
},
)
cfg.commands["twist"].heading_command = True
cfg.commands["twist"].rel_heading_envs = 1.0
cfg.commands["twist"].heading_control_stiffness = 0.5
cfg.commands["twist"].ranges.heading = (-math.pi, math.pi)
cfg.commands["twist"].rel_standing_envs = 0.02
cfg.commands["twist"].rel_forward_envs = 0.30
cfg.commands["twist"].rel_lateral_envs = 0.20
cfg.commands["twist"].rel_yaw_envs = 0.20
cfg.commands["twist"].ranges.lin_vel_x = (-1.0, 1.0)
cfg.commands["twist"].ranges.lin_vel_y = (-0.6, 0.6)
cfg.commands["twist"].ranges.lin_vel_y = (-1.0, 1.0)
cfg.commands["twist"].ranges.ang_vel_z = (-1.0, 1.0)
# ------------------
@@ -420,7 +487,7 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
cfg.events["reset_base"] = EventTermCfg(
func=envs_mdp.reset_root_state_uniform, mode="reset",
params={
"pose_range": {"z": (0.40, 0.45), "yaw": (-math.pi, math.pi)},
"pose_range": {"z": (0.42, 0.42), "yaw": (-math.pi, math.pi)},
"velocity_range": {"x": (-0.2, 0.2), "y": (-0.1, 0.1), "yaw": (-0.2, 0.2)},
"asset_cfg": SceneEntityCfg("robot"),
},
@@ -434,15 +501,32 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
# ------------------
# Rewards Integration
# ------------------
cfg.rewards["track_lin_vel"] = RewardTermCfg(
func=track_linear_velocity,
weight=3.0,
params={"std": 0.5, "command_name": "twist"}
cfg.rewards.pop("track_lin_vel", None)
cfg.rewards.pop("track_ang_vel", None)
cfg.rewards["track_lin_vel_x_exp"] = RewardTermCfg(
func=track_linear_velocity_x,
weight=1.0,
params={"std": 0.25, "command_name": "twist"},
)
cfg.rewards["track_ang_vel"] = RewardTermCfg(
func=track_angular_velocity,
weight=1.5,
params={"std": 0.5, "command_name": "twist"}
cfg.rewards["track_lin_vel_y_exp"] = RewardTermCfg(
func=track_linear_velocity_y,
weight=1.0,
params={"std": 0.25, "command_name": "twist"},
)
cfg.rewards["track_ang_vel_z_exp"] = RewardTermCfg(
func=track_angular_velocity_z,
weight=1.0,
params={"std": 0.25, "command_name": "twist"},
)
cfg.rewards["stair_lateral_yaw_drift"] = RewardTermCfg(
func=stair_lateral_yaw_drift_l2,
weight=-1.0,
params={
"terrain_names": ("pyramid_stairs", "pyramid_stairs_inv", "random_grid"),
"y_scale": 1.0,
"yaw_scale": 1.0,
"asset_cfg": SceneEntityCfg("robot"),
},
)
cfg.rewards["lin_vel_z"] = RewardTermCfg(func=lin_vel_z_l2, weight=-2.0)
@@ -473,8 +557,8 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
weight=-0.05,
params={
"mirror_joints": [
["fl_(hip_abduction|hip_pitch|knee)_joint", "rr_(hip_abduction|hip_pitch|knee)_joint"],
["fr_(hip_abduction|hip_pitch|knee)_joint", "rl_(hip_abduction|hip_pitch|knee)_joint"]
["fl_(hip_pitch|knee)_joint", "rr_(hip_pitch|knee)_joint"],
["fr_(hip_pitch|knee)_joint", "rl_(hip_pitch|knee)_joint"]
],
"asset_cfg": SceneEntityCfg("robot")
}
@@ -483,33 +567,61 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
# 移除 variable_posture 及其产生的静止奖励陷阱，换用极轻微的偏离惩罚
cfg.rewards.pop("stand_still", None)
cfg.rewards["stand_still"] = RewardTermCfg(func=stand_still, weight=-2.0, params={"command_name": "twist", "command_threshold": 0.1})
cfg.rewards.pop("hip_deviation", None)
cfg.rewards.pop("variable_posture", None)
cfg.rewards.pop("joint_deviation_l2", None)
cfg.rewards["joint_pos_penalty"] = RewardTermCfg(
func=joint_pos_penalty,
# 针对 ab 关节施加较严厉的惩罚，防止在 yaw 时乱撇腿
cfg.rewards["joint_pos_penalty_ab"] = RewardTermCfg(
func=joint_pos_penalty,
weight=-1.0,
params={
"stand_still_scale": 5.0,
"velocity_threshold": 0.5,
"command_threshold": 0.1,
"asset_cfg": SceneEntityCfg("robot", joint_names=(".*_hip_abduction_joint", ".*_hip_pitch_joint", ".*_knee_joint")),
"asset_cfg": SceneEntityCfg("robot", joint_names=(".*_hip_abduction_joint",)),
"command_name": "twist"
}
)
# 针对 pitch 和 knee 关节施加较宽松的惩罚，保留跨越障碍的抬腿自由度
cfg.rewards["joint_pos_penalty_sagittal"] = RewardTermCfg(
func=joint_pos_penalty,
weight=-0.3,
params={
"stand_still_scale": 5.0,
"velocity_threshold": 0.5,
"command_threshold": 0.1,
"asset_cfg": SceneEntityCfg("robot", joint_names=(".*_hip_pitch_joint", ".*_knee_joint")),
"command_name": "twist"
}
)
# 🌟 强力约束同侧外展关节平行对称，消除转向时的前后剪刀式摆动
cfg.rewards["abduction_mirror"] = RewardTermCfg(
func=joint_mirror,
weight=-0.5, # 施加合理惩罚，限制前后腿同侧外展关节反向运动
params={
"mirror_joints": [
["fl_hip_abduction_joint", "rl_hip_abduction_joint"],
["fr_hip_abduction_joint", "rr_hip_abduction_joint"]
],
"asset_cfg": SceneEntityCfg("robot")
}
)
cfg.rewards["feet_contact_without_cmd"] = RewardTermCfg(
func=feet_contact_without_cmd,
weight=0.1,
func=feet_contact_without_cmd,
weight=0.1,
params={"command_name": "twist", "sensor_name": "feet_ground_contact"}
)
cfg.rewards["feet_air_time"].weight = 0.0
cfg.rewards["upward"] = RewardTermCfg(func=upward, weight=1.0)
cfg.rewards["feet_air_time"].weight = 0.15
cfg.rewards["upward"] = RewardTermCfg(func=upward, weight=0.5)
cfg.rewards["base_height_l2"].weight = 0.0
cfg.rewards["base_height_l2"].params["target_height"] = 0.40
cfg.rewards["base_height_l2"].params["target_height"] = 0.42
cfg.rewards["base_height_l2"].params["sensor_cfg"] = SceneEntityCfg("height_scanner")
# 恢复机身碰撞惩罚为-1.0，逼迫机器人高抬腿跨越障碍，防止拖地
@@ -533,6 +645,79 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
cfg.scene.terrain.num_envs = 2048
cfg.scene.terrain.env_spacing = 2.5
cfg.metrics.update(
{
"tracking_lin_vel_error": MetricsTermCfg(func=tracking_lin_vel_error, params={"command_name": "twist"}),
"tracking_lin_vel_x_error": MetricsTermCfg(func=tracking_lin_vel_x_error, params={"command_name": "twist"}),
"tracking_lin_vel_y_error": MetricsTermCfg(func=tracking_lin_vel_y_error, params={"command_name": "twist"}),
"tracking_lin_vel_along_cmd_error": MetricsTermCfg(
func=tracking_lin_vel_along_command_error, params={"command_name": "twist"}
),
"actual_lin_vel_orthogonal_cmd": MetricsTermCfg(
func=actual_lin_vel_orthogonal_command_mean, params={"command_name": "twist"}
),
"tracking_yaw_vel_error": MetricsTermCfg(func=tracking_yaw_vel_error, params={"command_name": "twist"}),
"cmd_lin_vel": MetricsTermCfg(func=command_lin_vel_mean, params={"command_name": "twist"}),
"cmd_yaw_vel": MetricsTermCfg(func=command_yaw_vel_abs_mean, params={"command_name": "twist"}),
"actual_lin_vel": MetricsTermCfg(func=actual_lin_vel_mean),
"tracking_lin_vel_error_cmd_0_03": MetricsTermCfg(
func=tracking_lin_vel_error_band_mean,
params={"command_name": "twist", "min_speed": 0.0, "max_speed": 0.3},
),
"tracking_lin_vel_error_cmd_03_07": MetricsTermCfg(
func=tracking_lin_vel_error_band_mean,
params={"command_name": "twist", "min_speed": 0.3, "max_speed": 0.7},
),
"tracking_lin_vel_error_cmd_07_up": MetricsTermCfg(
func=tracking_lin_vel_error_band_mean,
params={"command_name": "twist", "min_speed": 0.7, "max_speed": 10.0},
),
"tracking_lin_vel_x_error_cmd_0_03": MetricsTermCfg(
func=tracking_lin_vel_axis_error_band_mean,
params={"axis": 0, "command_name": "twist", "min_speed": 0.0, "max_speed": 0.3},
),
"tracking_lin_vel_x_error_cmd_03_07": MetricsTermCfg(
func=tracking_lin_vel_axis_error_band_mean,
params={"axis": 0, "command_name": "twist", "min_speed": 0.3, "max_speed": 0.7},
),
"tracking_lin_vel_x_error_cmd_07_up": MetricsTermCfg(
func=tracking_lin_vel_axis_error_band_mean,
params={"axis": 0, "command_name": "twist", "min_speed": 0.7, "max_speed": 10.0},
),
"tracking_lin_vel_y_error_cmd_0_03": MetricsTermCfg(
func=tracking_lin_vel_axis_error_band_mean,
params={"axis": 1, "command_name": "twist", "min_speed": 0.0, "max_speed": 0.3},
),
"tracking_lin_vel_y_error_cmd_03_07": MetricsTermCfg(
func=tracking_lin_vel_axis_error_band_mean,
params={"axis": 1, "command_name": "twist", "min_speed": 0.3, "max_speed": 0.7},
),
"tracking_lin_vel_y_error_cmd_07_up": MetricsTermCfg(
func=tracking_lin_vel_axis_error_band_mean,
params={"axis": 1, "command_name": "twist", "min_speed": 0.7, "max_speed": 10.0},
),
"cmd_band_0_03": MetricsTermCfg(
func=command_band_active, params={"command_name": "twist", "min_speed": 0.0, "max_speed": 0.3}
),
"cmd_band_03_07": MetricsTermCfg(
func=command_band_active, params={"command_name": "twist", "min_speed": 0.3, "max_speed": 0.7}
),
"cmd_band_07_up": MetricsTermCfg(
func=command_band_active, params={"command_name": "twist", "min_speed": 0.7, "max_speed": 10.0}
),
"wheel_raw_action_abs": MetricsTermCfg(func=wheel_raw_action_abs_mean),
"wheel_target_vel_abs": MetricsTermCfg(func=wheel_target_vel_abs_mean),
"wheel_actual_vel_abs": MetricsTermCfg(func=wheel_actual_vel_abs_mean),
"wheel_target_actual_vel_error": MetricsTermCfg(func=wheel_target_actual_vel_error_mean),
"wheel_actual_to_target_vel_ratio": MetricsTermCfg(func=wheel_actual_to_target_vel_ratio_mean),
"wheel_target_actual_sign_agreement": MetricsTermCfg(func=wheel_target_actual_sign_agreement),
"upright": MetricsTermCfg(func=upright_metric),
"base_ground_contact_rate": MetricsTermCfg(
func=base_ground_contact_metric, params={"sensor_name": "base_ground_contact"}
),
}
)
if play:
cfg.episode_length_s = int(1e9)
cfg.observations["actor"].enable_corruption = False
05_software/train/rc_mjlab/src/robot/config/rl_cfg.py
+3 -3
View File
@@ -33,9 +33,9 @@ def rough_ppo_runner_cfg() -> RslRlOnPolicyRunnerCfg:
value_loss_coef=1.0,
use_clipped_value_loss=True,
clip_param=0.2,
entropy_coef=0.001,#第一轮为0.003
num_learning_epochs=5,
num_mini_batches=4,
entropy_coef=0.003,#第一轮为0.003，第二轮0.001，第三轮0.0008
num_learning_epochs=5,#2048为5 4096为3
num_mini_batches=4,#2048为4 4096为8
learning_rate=8.0e-4,
schedule="adaptive",
gamma=0.99,
05_software/train/rc_mjlab/src/robot/mdp/commands.py
+84 -1
View File
@@ -21,14 +21,20 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
def __init__(self, cfg: UniformThresholdVelocityCommandCfg, env: ManagerBasedRlEnv):
super().__init__(cfg, env)
self.is_lateral_env = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device)
self.is_yaw_env = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device)
self.was_climbing = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device)
# 缓存地形类型的索引（台阶、反向台阶、垂直短墙），实现高容错动态查找
self._climbing_indices = []
self._flat_index = -1
terrain = getattr(self._env.scene, "terrain", None)
if terrain is not None and getattr(terrain.cfg, "terrain_generator", None) is not None:
sub_terrain_names = list(terrain.cfg.terrain_generator.sub_terrains.keys())
for name in ["pyramid_stairs", "pyramid_stairs_inv", "rc_wall"]:
if name in sub_terrain_names:
self._climbing_indices.append(sub_terrain_names.index(name))
if "flat" in sub_terrain_names:
self._flat_index = sub_terrain_names.index("flat")
def _resample_command(self, env_ids: torch.Tensor) -> None:
# 1. 调用基类的标准采样
@@ -42,6 +48,41 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
self.vel_command_b[small_cmd_ids, :] = 0.0
self.vel_command_w[small_cmd_ids, :] = 0.0
# 重置并采样单 Y（只横移）与单 Z（只原地转向）指令分布
self.is_lateral_env[env_ids] = False
self.is_yaw_env[env_ids] = False
# 对没有设为前进且没有静止的激活环境进行单轴采样
active_non_fwd_mask = (~self.is_forward_env[env_ids]) & (~self.is_standing_env[env_ids])
active_non_fwd_ids = env_ids[active_non_fwd_mask]
if len(active_non_fwd_ids) > 0:
r = torch.empty(len(active_non_fwd_ids), device=self.device)
# 采样单 Y 占比
self.is_lateral_env[active_non_fwd_ids] = r.uniform_(0.0, 1.0) <= self.cfg.rel_lateral_envs
lat_ids = active_non_fwd_ids[self.is_lateral_env[active_non_fwd_ids]]
if len(lat_ids) > 0:
self.vel_command_b[lat_ids, 0] = 0.0
y_signs = self.vel_command_b[lat_ids, 1].sign()
y_signs[y_signs == 0] = 1.0
self.vel_command_b[lat_ids, 1] = y_signs * self.vel_command_b[lat_ids, 1].abs().clamp(min=0.3)
self.vel_command_b[lat_ids, 2] = 0.0
# 对不是 forward 也不是 lateral 的环境采样单 Z 占比
non_lat_mask = ~self.is_lateral_env[active_non_fwd_ids]
non_lat_ids = active_non_fwd_ids[non_lat_mask]
if len(non_lat_ids) > 0:
r_yaw = torch.empty(len(non_lat_ids), device=self.device)
self.is_yaw_env[non_lat_ids] = r_yaw.uniform_(0.0, 1.0) <= self.cfg.rel_yaw_envs
yaw_ids = non_lat_ids[self.is_yaw_env[non_lat_ids]]
if len(yaw_ids) > 0:
self.vel_command_b[yaw_ids, 0] = 0.0
self.vel_command_b[yaw_ids, 1] = 0.0
z_signs = self.vel_command_b[yaw_ids, 2].sign()
z_signs[z_signs == 0] = 1.0
self.vel_command_b[yaw_ids, 2] = z_signs * self.vel_command_b[yaw_ids, 2].abs().clamp(min=0.3)
# 3. 地形自适应重采样限制：若在爬行地形，强制纯前进方向且速度 >= 0.3 m/s
terrain = getattr(self._env.scene, "terrain", None)
terrain_types = getattr(terrain, "terrain_types", None)
@@ -58,6 +99,37 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
self.vel_command_b[climbing_env_ids, 2] = 0.0
self.is_heading_env[climbing_env_ids] = True
# 4. 在平地（flat）地形上生成 35% 纯 Y 和 35% 纯 Z 指令，排除静止环境
if terrain_types is not None and self._flat_index != -1:
is_flat = terrain_types == self._flat_index
flat_env_ids = env_ids[is_flat[env_ids]]
if len(flat_env_ids) > 0:
active_flat_mask = ~self.is_standing_env[flat_env_ids]
active_flat_ids = flat_env_ids[active_flat_mask]
if len(active_flat_ids) > 0:
r = torch.empty(len(active_flat_ids), device=self.device).uniform_(0.0, 1.0)
y_only_mask = r < 0.35
z_only_mask = (r >= 0.35) & (r < 0.70)
y_only_ids = active_flat_ids[y_only_mask]
if len(y_only_ids) > 0:
self.vel_command_b[y_only_ids, 0] = 0.0 # x = 0
self.vel_command_b[y_only_ids, 2] = 0.0 # yaw = 0
if self.cfg.heading_command:
self.heading_target[y_only_ids] = self.robot.data.heading_w[y_only_ids]
self.is_heading_env[y_only_ids] = True
z_only_ids = active_flat_ids[z_only_mask]
if len(z_only_ids) > 0:
self.vel_command_b[z_only_ids, 0] = 0.0 # x = 0
self.vel_command_b[z_only_ids, 1] = 0.0 # y = 0
if self.cfg.heading_command:
yaw_delta = torch.empty(len(z_only_ids), device=self.device).uniform_(
-self.cfg.yaw_only_heading_range, self.cfg.yaw_only_heading_range
)
self.heading_target[z_only_ids] = self.robot.data.heading_w[z_only_ids] + yaw_delta
self.is_heading_env[z_only_ids] = True
def _update_command(self) -> None:
# 调用基类的每步更新
super()._update_command()
@@ -69,7 +141,14 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
is_climbing = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device)
for idx in self._climbing_indices:
is_climbing |= (terrain_types == idx)
# 【出台阶实时重采样】：检查从爬坡状态刚刚离开环境的机器人，进行即时指令重采样
left_climbing_mask = self.was_climbing & ~is_climbing
if left_climbing_mask.any():
left_climbing_ids = torch.where(left_climbing_mask)[0]
self._resample_command(left_climbing_ids)
# 【入台阶实时截断】：对于正在爬楼梯/翻越短墙的环境，强行截断其横向横移指令，并进行温和偏航对齐
climbing_env_ids = is_climbing.nonzero(as_tuple=False).flatten()
if len(climbing_env_ids) > 0:
self.vel_command_b[climbing_env_ids, 1] = 0.0
@@ -79,6 +158,7 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
min=-0.3,
max=0.3
)
self.was_climbing = is_climbing
# 5. 高速侧向解耦（适用于平地/斜坡等混合路面）：当前进速度 >= 0.8 m/s 时，清空侧向指令，防止高速甩尾甩飞
high_speed_mask = self.vel_command_b[:, 0].abs() >= 0.8
@@ -90,3 +170,6 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
@dataclass(kw_only=True)
class UniformThresholdVelocityCommandCfg(UniformVelocityCommandCfg):
class_type: type = UniformThresholdVelocityCommand
yaw_only_heading_range: float = 1.0
rel_lateral_envs: float = 0.0
rel_yaw_envs: float = 0.0
05_software/train/rc_mjlab/src/robot/mdp/curriculums.py
+377
View File
@@ -99,6 +99,165 @@ class adaptive_command_vel:
}
class command_axis_levels_vel:
"""Linearly expand one command axis over training steps."""
def __init__(self, cfg: CurriculumTermCfg, env: ManagerBasedRlEnv):
from mjlab.tasks.velocity.mdp import UniformVelocityCommandCfg
p = cfg.params
self._command_name: str = p.get("command_name", "twist")
self._reward_name: str = p["reward_term_name"]
self._axis: str = p["axis"]
self._range_multiplier: tuple[float, float] = tuple(p.get("range_multiplier", (0.4, 1.0)))
self._initial_range_param: tuple[float, float] | None = (
tuple(p["initial_range"]) if "initial_range" in p else None
)
self._ema_alpha: float = p.get("ema_alpha", 0.5)
self._warmup_steps: int = p.get("warmup_steps", 0)
self._ramp_steps: int = p.get("ramp_steps", 800 * 24)
command_term = env.command_manager.get_term(self._command_name)
self._cfg = cast(UniformVelocityCommandCfg, command_term.cfg)
self._reward_idx = list(env.reward_manager._term_names).index(self._reward_name)
self._reward_weight = env.reward_manager.get_term_cfg(self._reward_name).weight
self._running_mean = 0.0
self._full_range = self._get_range()
self._min_range = self._initial_range_param or self._scaled_range(self._range_multiplier[0])
self._set_range(self._min_range)
def __call__(self, env: ManagerBasedRlEnv, env_ids: torch.Tensor, **kwargs) -> dict[str, torch.Tensor]:
if len(env_ids) > 0:
episode_sums = env.reward_manager._episode_sums[self._reward_name][env_ids]
mean_raw = torch.mean(episode_sums / env.cfg.episode_length_s / self._reward_weight).item()
self._running_mean = self._ema_alpha * mean_raw + (1.0 - self._ema_alpha) * self._running_mean
self._set_range(self._current_max_range(env.common_step_counter))
lo, hi = self._get_range()
return {
f"{self._axis}_range_min": torch.tensor(lo),
f"{self._axis}_range_max": torch.tensor(hi),
f"{self._axis}_tracking_ema": torch.tensor(self._running_mean),
f"{self._axis}_warmup_active": torch.tensor(float(env.common_step_counter < self._warmup_steps)),
f"{self._axis}_range_cap": torch.tensor(self._current_multiplier(env.common_step_counter)),
}
def _get_range(self) -> tuple[float, float]:
if self._axis == "x":
return tuple(self._cfg.ranges.lin_vel_x)
if self._axis == "y":
return tuple(self._cfg.ranges.lin_vel_y)
if self._axis == "yaw":
return tuple(self._cfg.ranges.ang_vel_z)
raise ValueError(f"Unknown command curriculum axis: {self._axis}")
def _set_range(self, value: tuple[float, float]) -> None:
if self._axis == "x":
self._cfg.ranges.lin_vel_x = value
elif self._axis == "y":
self._cfg.ranges.lin_vel_y = value
elif self._axis == "yaw":
self._cfg.ranges.ang_vel_z = value
else:
raise ValueError(f"Unknown command curriculum axis: {self._axis}")
def _scaled_range(self, multiplier: float) -> tuple[float, float]:
lo, hi = self._full_range
return (lo * multiplier, hi * multiplier)
def _current_multiplier(self, step: int) -> float:
start, end = self._range_multiplier
if self._ramp_steps <= 0:
return end
progress = max(0.0, min(1.0, (step - self._warmup_steps) / self._ramp_steps))
return start + (end - start) * progress
def _current_max_range(self, step: int) -> tuple[float, float]:
if self._initial_range_param is None:
return self._scaled_range(self._current_multiplier(step))
progress = self._current_progress(step)
lo = self._min_range[0] + (self._full_range[0] - self._min_range[0]) * progress
hi = self._min_range[1] + (self._full_range[1] - self._min_range[1]) * progress
return (lo, hi)
def _current_progress(self, step: int) -> float:
if self._ramp_steps <= 0:
return 1.0
return max(0.0, min(1.0, (step - self._warmup_steps) / self._ramp_steps))
class command_levels_adaptive:
"""Adaptive command range curriculum based on average tracking performance."""
def __init__(self, cfg: CurriculumTermCfg, env: ManagerBasedRlEnv):
p = cfg.params
self._command_name: str = p.get("command_name", "twist")
self._reward_name: str = p["reward_term_name"]
self._axis: str = p["axis"]
self._delta_command: float = p.get("delta_command", 0.05)
self._target_ratio: float = p.get("target_ratio", 0.8)
self._ema_alpha: float = p.get("ema_alpha", 0.5)
command_term = env.command_manager.get_term(self._command_name)
self._cfg = command_term.cfg
self._reward_weight = env.reward_manager.get_term_cfg(self._reward_name).weight
self._running_mean = 0.0
# Read the full range configured in the environment configuration
self._full_range = self._get_range()
# Set the command range to initial range at the start
self._initial_range = list(p["initial_range"]) # e.g. [-0.5, 0.5]
self._current_range = list(self._initial_range)
self._set_range(self._current_range)
def __call__(self, env: ManagerBasedRlEnv, env_ids: torch.Tensor, **kwargs) -> dict[str, torch.Tensor]:
if len(env_ids) > 0:
episode_sums = env.reward_manager._episode_sums[self._reward_name][env_ids]
mean_raw = torch.mean(episode_sums / env.cfg.episode_length_s / self._reward_weight).item()
self._running_mean = self._ema_alpha * mean_raw + (1.0 - self._ema_alpha) * self._running_mean
# Check performance at the end of every episode (or every max_episode_length_s)
episode_length_steps = int(env.cfg.episode_length_s / env.step_dt)
if env.common_step_counter > 0 and env.common_step_counter % episode_length_steps == 0:
# If performance exceeds target ratio (e.g., 0.8), widen the range
if self._running_mean > self._target_ratio:
# Widen the range
lo, hi = self._current_range
new_lo = max(self._full_range[0], lo - self._delta_command)
new_hi = min(self._full_range[1], hi + self._delta_command)
self._current_range = [new_lo, new_hi]
self._set_range(self._current_range)
lo, hi = self._current_range
return {
f"{self._axis}_range_min": torch.tensor(lo),
f"{self._axis}_range_max": torch.tensor(hi),
f"{self._axis}_tracking_ema": torch.tensor(self._running_mean),
f"{self._axis}_target_ratio": torch.tensor(self._target_ratio),
}
def _get_range(self) -> tuple[float, float]:
if self._axis == "x":
return tuple(self._cfg.ranges.lin_vel_x)
if self._axis == "y":
return tuple(self._cfg.ranges.lin_vel_y)
if self._axis == "yaw":
return tuple(self._cfg.ranges.ang_vel_z)
raise ValueError(f"Unknown command curriculum axis: {self._axis}")
def _set_range(self, value: list[float]) -> None:
if self._axis == "x":
self._cfg.ranges.lin_vel_x = tuple(value)
elif self._axis == "y":
self._cfg.ranges.lin_vel_y = tuple(value)
elif self._axis == "yaw":
self._cfg.ranges.ang_vel_z = tuple(value)
else:
raise ValueError(f"Unknown command curriculum axis: {self._axis}")
def terrain_levels_vel_strict(
env: ManagerBasedRlEnv,
env_ids: torch.Tensor,
@@ -163,3 +322,221 @@ def terrain_levels_vel_strict(
return result
def terrain_levels_ramp_strict(
env: ManagerBasedRlEnv,
env_ids: torch.Tensor,
command_name: str,
ramp_steps: int = 50 * 24,
move_up_expected_distance_ratio: float = 0.60,
move_down_distance_ratio: float = 0.50,
min_command_speed: float = 0.15,
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
) -> dict[str, torch.Tensor]:
"""Terrain curriculum active from start, with strict promotion and time-ramped max level."""
asset: Entity = env.scene[asset_cfg.name]
terrain = env.scene.terrain
assert terrain is not None
terrain_generator = terrain.cfg.terrain_generator
assert terrain_generator is not None
assert terrain.terrain_origins is not None
assert terrain.env_origins is not None
command = env.command_manager.get_command(command_name)
assert command is not None
distance = torch.norm(
asset.data.root_link_pos_w[env_ids, :2] - env.scene.env_origins[env_ids, :2],
dim=1,
)
cmd_speed = torch.norm(command[env_ids, :2], dim=1)
active_command = cmd_speed >= min_command_speed
expected_distance = cmd_speed * env.max_episode_length_s
move_up = (distance > expected_distance * move_up_expected_distance_ratio) & active_command
move_down = (
(distance < expected_distance * move_down_distance_ratio)
& active_command
& ~move_up
)
terrain.terrain_levels[env_ids] += 1 * move_up - 1 * move_down
max_level = max(int(terrain.max_terrain_level) - 1, 0)
if ramp_steps <= 0:
level_cap = max_level
else:
progress = max(0.0, min(1.0, env.common_step_counter / ramp_steps))
level_cap = int(round(progress * max_level))
terrain.terrain_levels[env_ids] = torch.clamp(
terrain.terrain_levels[env_ids],
min=0,
max=min(level_cap, max_level),
)
terrain.env_origins[env_ids] = terrain.terrain_origins[
terrain.terrain_levels[env_ids], terrain.terrain_types[env_ids]
]
levels = terrain.terrain_levels.float()
result: dict[str, torch.Tensor] = {
"mean": torch.mean(levels),
"max": torch.max(levels),
"level_cap": torch.tensor(float(level_cap), device=env.device),
}
sub_terrain_names = list(terrain_generator.sub_terrains.keys())
num_cols = terrain.terrain_origins.shape[1]
if num_cols == len(sub_terrain_names):
types = terrain.terrain_types
for i, name in enumerate(sub_terrain_names):
mask = types == i
if mask.any():
result[name] = torch.mean(levels[mask])
return result
def terrain_levels_flat_warmup(
env: ManagerBasedRlEnv,
env_ids: torch.Tensor,
command_name: str,
warmup_steps: int = 4800,
flat_terrain_name: str = "flat",
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
) -> dict[str, torch.Tensor]:
"""Keep all reset envs on flat level 0 before enabling terrain curriculum."""
terrain = env.scene.terrain
assert terrain is not None
terrain_generator = terrain.cfg.terrain_generator
assert terrain_generator is not None
assert terrain.terrain_origins is not None
assert terrain.env_origins is not None
sub_terrain_names = list(terrain_generator.sub_terrains.keys())
flat_type = sub_terrain_names.index(flat_terrain_name) if flat_terrain_name in sub_terrain_names else 0
if env.common_step_counter < warmup_steps:
terrain.terrain_levels[env_ids] = 0
terrain.terrain_types[env_ids] = flat_type
terrain.env_origins[env_ids] = terrain.terrain_origins[0, flat_type]
if not hasattr(terrain, "_flat_warmup_env_released"):
terrain._flat_warmup_env_released = torch.zeros(
env.num_envs, dtype=torch.bool, device=env.device
)
terrain._flat_warmup_env_released[env_ids] = False
levels = terrain.terrain_levels.float()
result: dict[str, torch.Tensor] = {
"mean": torch.mean(levels),
"max": torch.max(levels),
"warmup_active": torch.ones((), device=env.device),
}
for i, name in enumerate(sub_terrain_names):
mask = terrain.terrain_types == i
if mask.any():
result[name] = torch.mean(levels[mask])
return result
if not hasattr(terrain, "_flat_warmup_released"):
terrain._flat_warmup_released = True
terrain._flat_warmup_release_counts = 0
if not hasattr(terrain, "_flat_warmup_env_released"):
terrain._flat_warmup_env_released = torch.zeros(
env.num_envs, dtype=torch.bool, device=env.device
)
newly_released = env_ids[~terrain._flat_warmup_env_released[env_ids]]
if len(newly_released) > 0:
proportions = torch.tensor(
[sub.proportion for sub in terrain_generator.sub_terrains.values()],
device=env.device,
dtype=torch.float,
)
proportions = proportions / torch.clamp(proportions.sum(), min=1.0e-6)
terrain.terrain_types[newly_released] = torch.multinomial(
proportions, len(newly_released), replacement=True
)
terrain.terrain_levels[newly_released] = 0
terrain.env_origins[newly_released] = terrain.terrain_origins[
terrain.terrain_levels[newly_released], terrain.terrain_types[newly_released]
]
terrain._flat_warmup_env_released[newly_released] = True
terrain._flat_warmup_release_counts += len(newly_released)
result = terrain_levels_vel_strict(env, env_ids, command_name, asset_cfg=asset_cfg)
result["warmup_active"] = torch.zeros((), device=env.device)
result["released_envs"] = torch.tensor(float(getattr(terrain, "_flat_warmup_release_counts", 0)), device=env.device)
return result
def terrain_levels_obstacle_release(
env: ManagerBasedRlEnv,
env_ids: torch.Tensor,
command_name: str,
release_schedule: tuple[tuple[int, tuple[str, ...]], ...],
initial_terrain_names: tuple[str, ...] = ("flat", "random_rough", "perlin_noise", "sloped_terrain"),
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
) -> dict[str, torch.Tensor]:
"""Release obstacle terrain types gradually while keeping standard level progression."""
terrain = env.scene.terrain
assert terrain is not None
terrain_generator = terrain.cfg.terrain_generator
assert terrain_generator is not None
assert terrain.terrain_origins is not None
assert terrain.env_origins is not None
sub_terrain_names = list(terrain_generator.sub_terrains.keys())
allowed_names = list(initial_terrain_names)
for step, names in release_schedule:
if env.common_step_counter >= step:
allowed_names.extend(names)
allowed_type_ids = [
sub_terrain_names.index(name) for name in allowed_names if name in sub_terrain_names
]
if not allowed_type_ids:
allowed_type_ids = [0]
if not hasattr(terrain, "_obstacle_release_env_allowed"):
terrain._obstacle_release_env_allowed = torch.zeros(
env.num_envs, dtype=torch.bool, device=env.device
)
terrain._last_allowed_count = len(allowed_type_ids)
# If new terrains were released, force all envs to eventually resample upon their next reset
if len(allowed_type_ids) > terrain._last_allowed_count:
terrain._obstacle_release_env_allowed.fill_(False)
terrain._last_allowed_count = len(allowed_type_ids)
allowed_tensor = torch.tensor(allowed_type_ids, dtype=torch.long, device=env.device)
current_allowed = torch.isin(terrain.terrain_types[env_ids], allowed_tensor)
need_resample = env_ids[~terrain._obstacle_release_env_allowed[env_ids] | ~current_allowed]
if len(need_resample) > 0:
proportions = torch.tensor(
[terrain_generator.sub_terrains[sub_terrain_names[i]].proportion for i in allowed_type_ids],
device=env.device,
dtype=torch.float,
)
proportions = proportions / torch.clamp(proportions.sum(), min=1.0e-6)
sampled = allowed_tensor[torch.multinomial(proportions, len(need_resample), replacement=True)]
# Check which envs actually changed terrain type
changed_mask = terrain.terrain_types[need_resample] != sampled
changed_envs = need_resample[changed_mask]
terrain.terrain_types[need_resample] = sampled
# Only reset the level to 0 if the terrain type was actually changed
if len(changed_envs) > 0:
terrain.terrain_levels[changed_envs] = 0
terrain.env_origins[need_resample] = terrain.terrain_origins[
terrain.terrain_levels[need_resample], terrain.terrain_types[need_resample]
]
terrain._obstacle_release_env_allowed[need_resample] = True
result = terrain_levels_vel_strict(env, env_ids, command_name, asset_cfg=asset_cfg)
result["allowed_types"] = torch.tensor(float(len(allowed_type_ids)), device=env.device)
for name in ("pyramid_stairs", "pyramid_stairs_inv", "random_grid", "rc_wall"):
result[f"{name}_released"] = torch.tensor(float(name in allowed_names), device=env.device)
return result
05_software/train/rc_mjlab/src/robot/mdp/rewards.py
+334
View File
@@ -14,6 +14,11 @@ if TYPE_CHECKING:
from mjlab.envs import ManagerBasedRlEnv
def _finite(value: torch.Tensor, nan: float = 0.0, posinf: float = 0.0, neginf: float = 0.0) -> torch.Tensor:
"""Keep diagnostic metrics finite when a terminating env has invalid physics state."""
return torch.nan_to_num(value, nan=nan, posinf=posinf, neginf=neginf)
def track_linear_velocity(
env: ManagerBasedRlEnv,
std: float,
@@ -32,6 +37,69 @@ def track_linear_velocity(
return reward
def track_linear_velocity_x(
env: ManagerBasedRlEnv,
std: float,
command_name: str,
gravity_z_power: float | None = None,
asset_cfg: SceneEntityCfg | None = None,
) -> torch.Tensor:
"""LocoLeggedWheel-style independent x velocity tracking reward."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot")
asset: Entity = env.scene[asset_cfg.name]
command = env.command_manager.get_command(command_name)
x_error = torch.square(command[:, 0] - asset.data.root_link_lin_vel_b[:, 0])
reward = torch.exp(-x_error / std**2)
if gravity_z_power is not None:
reward *= torch.clamp(-asset.data.projected_gravity_b[:, 2], min=0.0) ** gravity_z_power
else:
reward *= -asset.data.projected_gravity_b[:, 2]
return reward
def track_linear_velocity_y(
env: ManagerBasedRlEnv,
std: float,
command_name: str,
gravity_z_power: float | None = None,
asset_cfg: SceneEntityCfg | None = None,
) -> torch.Tensor:
"""LocoLeggedWheel-style independent y velocity tracking reward."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot")
asset: Entity = env.scene[asset_cfg.name]
command = env.command_manager.get_command(command_name)
y_error = torch.square(command[:, 1] - asset.data.root_link_lin_vel_b[:, 1])
reward = torch.exp(-y_error / std**2)
if gravity_z_power is not None:
reward *= torch.clamp(-asset.data.projected_gravity_b[:, 2], min=0.0) ** gravity_z_power
else:
reward *= -asset.data.projected_gravity_b[:, 2]
return reward
def track_angular_velocity_z(
env: ManagerBasedRlEnv,
std: float,
command_name: str,
gravity_z_power: float | None = None,
asset_cfg: SceneEntityCfg | None = None,
) -> torch.Tensor:
"""LocoLeggedWheel-style independent yaw velocity tracking reward."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot")
asset: Entity = env.scene[asset_cfg.name]
command = env.command_manager.get_command(command_name)
z_error = torch.square(command[:, 2] - asset.data.root_link_ang_vel_b[:, 2])
reward = torch.exp(-z_error / std**2)
if gravity_z_power is not None:
reward *= torch.clamp(-asset.data.projected_gravity_b[:, 2], min=0.0) ** gravity_z_power
else:
reward *= -asset.data.projected_gravity_b[:, 2]
return reward
def track_angular_velocity(
env: ManagerBasedRlEnv,
std: float,
@@ -50,6 +118,37 @@ def track_angular_velocity(
return reward
def stair_lateral_yaw_drift_l2(
env: ManagerBasedRlEnv,
terrain_names: tuple[str, ...] = ("pyramid_stairs", "pyramid_stairs_inv"),
y_scale: float = 1.0,
yaw_scale: float = 1.0,
asset_cfg: SceneEntityCfg | None = None,
) -> torch.Tensor:
"""Penalize sideways velocity and yaw-rate drift only on stair terrains."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot")
asset: Entity = env.scene[asset_cfg.name]
terrain = getattr(env.scene, "terrain", None)
terrain_types = getattr(terrain, "terrain_types", None)
terrain_cfg = getattr(terrain, "cfg", None)
terrain_generator = getattr(terrain_cfg, "terrain_generator", None)
if terrain_types is None or terrain_generator is None:
return torch.zeros(env.num_envs, device=env.device)
sub_terrain_names = list(terrain_generator.sub_terrains.keys())
mask = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device)
for name in terrain_names:
if name in sub_terrain_names:
mask |= terrain_types == sub_terrain_names.index(name)
y_vel = asset.data.root_link_lin_vel_b[:, 1]
yaw_vel = asset.data.root_link_ang_vel_b[:, 2]
penalty = y_scale * torch.square(y_vel) + yaw_scale * torch.square(yaw_vel)
return _finite(torch.where(mask, penalty, torch.zeros_like(penalty)))
def base_height_l2(
env: ManagerBasedRlEnv,
target_height: float = 0.36,
@@ -809,3 +908,238 @@ def pitch_control_penalty(env, max_pitch_rad: float = 0.50, asset_cfg=None) -> t
excessive_pitch = torch.clamp(torch.abs(g_x) - g_x_threshold, min=0.0)
reward = torch.square(excessive_pitch)
return reward
def tracking_lin_vel_error(env, command_name: str = "twist", asset_cfg=None) -> torch.Tensor:
"""Current-step xy velocity tracking error."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot")
asset = env.scene[asset_cfg.name]
cmd = env.command_manager.get_command(command_name)
return _finite(torch.linalg.norm(asset.data.root_link_lin_vel_b[:, :2] - cmd[:, :2], dim=1))
def tracking_yaw_vel_error(env, command_name: str = "twist", asset_cfg=None) -> torch.Tensor:
"""Current-step yaw velocity tracking error."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot")
asset = env.scene[asset_cfg.name]
cmd = env.command_manager.get_command(command_name)
return _finite(torch.abs(asset.data.root_link_ang_vel_b[:, 2] - cmd[:, 2]))
def tracking_lin_vel_x_error(env, command_name: str = "twist", asset_cfg=None) -> torch.Tensor:
"""Current-step x velocity tracking error."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot")
asset = env.scene[asset_cfg.name]
cmd = env.command_manager.get_command(command_name)
return _finite(torch.abs(asset.data.root_link_lin_vel_b[:, 0] - cmd[:, 0]))
def tracking_lin_vel_y_error(env, command_name: str = "twist", asset_cfg=None) -> torch.Tensor:
"""Current-step y velocity tracking error."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot")
asset = env.scene[asset_cfg.name]
cmd = env.command_manager.get_command(command_name)
return _finite(torch.abs(asset.data.root_link_lin_vel_b[:, 1] - cmd[:, 1]))
def tracking_lin_vel_along_command_error(
env,
command_name: str = "twist",
asset_cfg: SceneEntityCfg | None = None,
) -> torch.Tensor:
"""Error of velocity projected onto the commanded xy direction."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot")
asset = env.scene[asset_cfg.name]
cmd = env.command_manager.get_command(command_name)
cmd_xy = cmd[:, :2]
cmd_speed = torch.linalg.norm(cmd_xy, dim=1)
direction = cmd_xy / torch.clamp(cmd_speed.unsqueeze(1), min=1.0e-6)
actual_along = torch.sum(asset.data.root_link_lin_vel_b[:, :2] * direction, dim=1)
return _finite(torch.abs(actual_along - cmd_speed))
def actual_lin_vel_orthogonal_command_mean(
env,
command_name: str = "twist",
asset_cfg: SceneEntityCfg | None = None,
) -> torch.Tensor:
"""Absolute velocity component perpendicular to the commanded xy direction."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot")
asset = env.scene[asset_cfg.name]
cmd = env.command_manager.get_command(command_name)
cmd_xy = cmd[:, :2]
cmd_speed = torch.linalg.norm(cmd_xy, dim=1)
direction = cmd_xy / torch.clamp(cmd_speed.unsqueeze(1), min=1.0e-6)
actual = asset.data.root_link_lin_vel_b[:, :2]
actual_along = torch.sum(actual * direction, dim=1, keepdim=True) * direction
orthogonal = actual - actual_along
return _finite(torch.linalg.norm(orthogonal, dim=1))
def command_lin_vel_mean(env, command_name: str = "twist") -> torch.Tensor:
"""Current commanded xy speed magnitude."""
cmd = env.command_manager.get_command(command_name)
return torch.linalg.norm(cmd[:, :2], dim=1)
def command_yaw_vel_abs_mean(env, command_name: str = "twist") -> torch.Tensor:
"""Current commanded yaw speed magnitude."""
cmd = env.command_manager.get_command(command_name)
return torch.abs(cmd[:, 2])
def actual_lin_vel_mean(env, asset_cfg=None) -> torch.Tensor:
"""Current actual xy speed magnitude."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot")
asset = env.scene[asset_cfg.name]
return _finite(torch.linalg.norm(asset.data.root_link_lin_vel_b[:, :2], dim=1))
def tracking_lin_vel_error_band_mean(
env,
command_name: str = "twist",
min_speed: float = 0.0,
max_speed: float = 10.0,
asset_cfg=None,
) -> torch.Tensor:
"""Broadcast the masked mean xy error for a command speed band."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot")
asset = env.scene[asset_cfg.name]
cmd = env.command_manager.get_command(command_name)
speed = torch.linalg.norm(cmd[:, :2], dim=1)
err = _finite(torch.linalg.norm(asset.data.root_link_lin_vel_b[:, :2] - cmd[:, :2], dim=1))
active = torch.logical_and(speed >= min_speed, speed < max_speed)
denom = active.float().sum().clamp_min(1.0)
mean_err = torch.sum(torch.where(active, err, torch.zeros_like(err))) / denom
return torch.full_like(err, mean_err)
def tracking_lin_vel_axis_error_band_mean(
env,
axis: int,
command_name: str = "twist",
min_speed: float = 0.0,
max_speed: float = 10.0,
asset_cfg=None,
) -> torch.Tensor:
"""Broadcast the masked mean x/y error for a command speed band."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot")
asset = env.scene[asset_cfg.name]
cmd = env.command_manager.get_command(command_name)
speed = torch.linalg.norm(cmd[:, :2], dim=1)
err = _finite(torch.abs(asset.data.root_link_lin_vel_b[:, axis] - cmd[:, axis]))
active = torch.logical_and(speed >= min_speed, speed < max_speed)
denom = active.float().sum().clamp_min(1.0)
mean_err = torch.sum(torch.where(active, err, torch.zeros_like(err))) / denom
return torch.full_like(err, mean_err)
def command_band_active(
env,
command_name: str = "twist",
min_speed: float = 0.0,
max_speed: float = 10.0,
) -> torch.Tensor:
"""Fraction helper for command speed bands."""
cmd = env.command_manager.get_command(command_name)
speed = torch.linalg.norm(cmd[:, :2], dim=1)
return torch.logical_and(speed >= min_speed, speed < max_speed).float()
def wheel_raw_action_abs_mean(env, action_name: str = "wheel_joint_vel") -> torch.Tensor:
"""Mean absolute raw wheel action."""
action = env.action_manager.get_term(action_name).raw_action
return torch.mean(torch.abs(action), dim=1)
def wheel_target_vel_abs_mean(env, action_name: str = "wheel_joint_vel") -> torch.Tensor:
"""Mean absolute processed wheel velocity target."""
term = env.action_manager.get_term(action_name)
target = getattr(term, "_processed_actions")
return torch.mean(torch.abs(target), dim=1)
def wheel_actual_vel_abs_mean(env, asset_cfg: SceneEntityCfg | None = None) -> torch.Tensor:
"""Mean absolute actual wheel joint velocity."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot", joint_names=(".*_wheel_joint",))
asset = env.scene[asset_cfg.name]
joint_ids = asset.find_joints(asset_cfg.joint_names)[0]
return _finite(torch.mean(torch.abs(asset.data.joint_vel[:, joint_ids]), dim=1))
def wheel_target_actual_vel_error_mean(
env,
action_name: str = "wheel_joint_vel",
asset_cfg: SceneEntityCfg | None = None,
) -> torch.Tensor:
"""Mean absolute error between processed wheel target and actual wheel velocity."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot", joint_names=(".*_wheel_joint",))
term = env.action_manager.get_term(action_name)
target = getattr(term, "_processed_actions")
asset = env.scene[asset_cfg.name]
joint_ids = asset.find_joints(asset_cfg.joint_names)[0]
actual = asset.data.joint_vel[:, joint_ids]
return _finite(torch.mean(torch.abs(target - actual), dim=1))
def wheel_actual_to_target_vel_ratio_mean(
env,
action_name: str = "wheel_joint_vel",
asset_cfg: SceneEntityCfg | None = None,
) -> torch.Tensor:
"""Mean |actual wheel velocity| / |target wheel velocity|, clipped for readable logs."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot", joint_names=(".*_wheel_joint",))
term = env.action_manager.get_term(action_name)
target = getattr(term, "_processed_actions")
asset = env.scene[asset_cfg.name]
joint_ids = asset.find_joints(asset_cfg.joint_names)[0]
actual = asset.data.joint_vel[:, joint_ids]
ratio = torch.abs(actual) / torch.clamp(torch.abs(target), min=0.1)
return _finite(torch.mean(torch.clamp(ratio, max=3.0), dim=1))
def wheel_target_actual_sign_agreement(
env,
action_name: str = "wheel_joint_vel",
asset_cfg: SceneEntityCfg | None = None,
) -> torch.Tensor:
"""Fraction of wheel targets and actual velocities with matching sign."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot", joint_names=(".*_wheel_joint",))
term = env.action_manager.get_term(action_name)
target = getattr(term, "_processed_actions")
asset = env.scene[asset_cfg.name]
joint_ids = asset.find_joints(asset_cfg.joint_names)[0]
actual = asset.data.joint_vel[:, joint_ids]
active = torch.abs(target) > 0.1
same_sign = torch.sign(target) == torch.sign(actual)
return torch.sum((active & same_sign).float(), dim=1) / torch.clamp(torch.sum(active.float(), dim=1), min=1.0)
def upright_metric(env, asset_cfg=None) -> torch.Tensor:
"""1 means upright, 0 means fully inverted according to projected gravity."""
if asset_cfg is None:
asset_cfg = SceneEntityCfg("robot")
asset = env.scene[asset_cfg.name]
return torch.clamp(-asset.data.projected_gravity_b[:, 2], 0.0, 1.0)
def base_ground_contact_metric(env, sensor_name: str) -> torch.Tensor:
"""Per-step base contact flag for diagnosing reset-biased metrics."""
sensor = env.scene[sensor_name]
contact = sensor.data.found > 0
while contact.ndim > 1:
contact = torch.any(contact, dim=-1)
return contact.float()
05_software/train/rc_mjlab/src/robot/robot_cfg.py
+3 -3
View File
@@ -53,11 +53,11 @@ WHEEL_ACTUATOR_CFG = BuiltinVelocityActuatorCfg(
)
INIT_STATE = EntityCfg.InitialStateCfg(
pos=(0.0, 0.0, 0.40),
pos=(0.0, 0.0, 0.42),
joint_pos={
".*_hip_abduction_joint": 0.0,
".*_hip_pitch_joint": 0.9,
".*_knee_joint": -1.8,
".*_hip_pitch_joint": 0.550,
".*_knee_joint": -1.125,
".*_wheel_joint": 0.0,
},
joint_vel={".*": 0.0},
05_software/train/rc_mjlab/src/robot/terrains/competition_terrains.py
+7 -8
View File
@@ -24,9 +24,9 @@ _COLOR_PURPLE = (0.60, 0.20, 0.80)
@dataclass(kw_only=True)
class RCWallTerrainCfg(SubTerrainCfg):
"""Triple transverse wall obstacle terrain representing repeated race high walls.
"""Repeated transverse wall obstacle terrain for high-wall gait training.
The robot must sprint from the flat platform, vault over three walls, and proceed.
The robot must repeatedly step/vault over transverse walls and proceed.
As difficulty scales from 0 to 1, the wall height increases linearly from
wall_height_range[0] to wall_height_range[1].
@@ -41,9 +41,9 @@ class RCWallTerrainCfg(SubTerrainCfg):
wall_length_frac: float = 0.8
"""Wall length fraction of the terrain width (leaving gaps for visualization/debugging)."""
platform_width: float = 1.5
"""Sprint platform width (m)."""
wall_centers_x: tuple[float, float, float] = (2.9, 4.45, 6.0)
"""Wall center positions along x, spaced to keep a short sprint, two recovery gaps, and exit room."""
"""Nominal start platform width (m)."""
wall_centers_x: tuple[float, ...] = (2.1, 3.2, 4.3, 5.4, 6.5)
"""Wall center positions along x."""
def function(
self,
@@ -69,7 +69,7 @@ class RCWallTerrainCfg(SubTerrainCfg):
origin = np.array([self.size[0] / 2, self.size[1] / 2, 0.0])
return TerrainOutput(origin=origin, geometries=geometries)
# -- Wall geometry: three transverse walls oriented along y-axis --
# -- Wall geometry: transverse walls oriented along y-axis --
wall_length = self.wall_length_frac * self.size[1]
cy = self.size[1] / 2
@@ -87,8 +87,7 @@ class RCWallTerrainCfg(SubTerrainCfg):
)
geometries.append(TerrainGeometry(geom=wall_geom, color=wall_color))
# Spawn origin is set to the left platform area to allow a short sprint
# before the first wall and limited recovery space between subsequent walls.
# Spawn origin is set to the left platform area.
origin = np.array([1.5, cy, 0.0])
return TerrainOutput(origin=origin, geometries=geometries)
README.md
+2 -1
View File
@@ -2,7 +2,7 @@
RC_WheelLeg 是山东华宇工学院 16DOF 串联轮足机器人项目。
当前 `16dof` 分支用于整理 16DOF 机械、强化学习训练、Sim2Sim、Sim2Real、ROS 2 部署和比赛版本。机械资料、第一代软件闭环和前两版新版训练配置已经完成整理。
当前 `16dof` 分支用于整理 16DOF 机械、强化学习训练、Sim2Sim、Sim2Real、ROS 2 部署和比赛版本。机械资料、第一代软件闭环和比赛最终训练架构已经完成整理。
## 平台概览
@@ -35,6 +35,7 @@ RC_WheelLeg/
- [x] 整理 IK 真机控制与第一代 Python Sim2Real
- [x] 整理第一份新版 MJCF 与 mjlab 训练框架
- [x] 整理第二版 Sim2Real 随机化训练配置
- [x] 整理比赛最终训练代码架构
- [ ] 核对比赛机械与仿真模型参数
- [ ] 整理 URDF/MJCF 机器人描述
- [ ] 整理后续统一训练、ROS 2 和比赛版本