From d8c5d340912b5820e14ede967856df7d2b5a644d Mon Sep 17 00:00:00 2001 From: eastzio <3156045992@qq.com> Date: Mon, 27 Jul 2026 12:48:12 +0800 Subject: [PATCH] =?UTF-8?q?[train]=20=E6=95=B4=E7=90=86=E6=AF=94=E8=B5=9B?= =?UTF-8?q?=E6=9C=80=E7=BB=88=E8=AE=AD=E7=BB=83=E6=9E=B6=E6=9E=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- 01_doc/training_evolution.md | 37 ++ 01_doc/version_history.md | 14 + 05_software/README.md | 2 +- 05_software/train/README.md | 4 +- 05_software/train/rc_mjlab/README.md | 2 +- .../rc_mjlab/src/robot/config/env_cfgs.py | 295 +++++++++++--- .../train/rc_mjlab/src/robot/config/rl_cfg.py | 6 +- .../train/rc_mjlab/src/robot/mdp/commands.py | 85 +++- .../rc_mjlab/src/robot/mdp/curriculums.py | 377 ++++++++++++++++++ .../train/rc_mjlab/src/robot/mdp/rewards.py | 334 ++++++++++++++++ .../train/rc_mjlab/src/robot/robot_cfg.py | 6 +- .../robot/terrains/competition_terrains.py | 15 +- README.md | 3 +- 13 files changed, 1106 insertions(+), 74 deletions(-) create mode 100644 01_doc/training_evolution.md diff --git a/01_doc/training_evolution.md b/01_doc/training_evolution.md new file mode 100644 index 0000000..d03baad --- /dev/null +++ b/01_doc/training_evolution.md @@ -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` 是比赛最终部署工件，不用模型编号替代训练代码版本号。它将在最终比赛部署版本中与运行配置一起归档。 diff --git a/01_doc/version_history.md b/01_doc/version_history.md index c11560a..a45b9ac 100644 --- a/01_doc/version_history.md +++ b/01_doc/version_history.md @@ -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)。 diff --git a/05_software/README.md b/05_software/README.md index 1367fbd..9c0299a 100644 --- a/05_software/README.md +++ b/05_software/README.md @@ -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`。 详细说明见： diff --git a/05_software/train/README.md b/05_software/train/README.md index 9343672..f703ece 100644 --- a/05_software/train/README.md +++ b/05_software/train/README.md @@ -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)。 diff --git a/05_software/train/rc_mjlab/README.md b/05_software/train/rc_mjlab/README.md index 2fae555..55bcc4c 100644 --- a/05_software/train/rc_mjlab/README.md +++ b/05_software/train/rc_mjlab/README.md @@ -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` 将随最终部署版本归档。 --- diff --git a/05_software/train/rc_mjlab/src/robot/config/env_cfgs.py b/05_software/train/rc_mjlab/src/robot/config/env_cfgs.py index 6bacc38..2399e91 100644 --- a/05_software/train/rc_mjlab/src/robot/config/env_cfgs.py +++ b/05_software/train/rc_mjlab/src/robot/config/env_cfgs.py @@ -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 diff --git a/05_software/train/rc_mjlab/src/robot/config/rl_cfg.py b/05_software/train/rc_mjlab/src/robot/config/rl_cfg.py index 700ab99..af79b75 100644 --- a/05_software/train/rc_mjlab/src/robot/config/rl_cfg.py +++ b/05_software/train/rc_mjlab/src/robot/config/rl_cfg.py @@ -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, diff --git a/05_software/train/rc_mjlab/src/robot/mdp/commands.py b/05_software/train/rc_mjlab/src/robot/mdp/commands.py index 4d81703..e09cb7a 100644 --- a/05_software/train/rc_mjlab/src/robot/mdp/commands.py +++ b/05_software/train/rc_mjlab/src/robot/mdp/commands.py @@ -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 diff --git a/05_software/train/rc_mjlab/src/robot/mdp/curriculums.py b/05_software/train/rc_mjlab/src/robot/mdp/curriculums.py index 4f65df5..c7f2658 100644 --- a/05_software/train/rc_mjlab/src/robot/mdp/curriculums.py +++ b/05_software/train/rc_mjlab/src/robot/mdp/curriculums.py @@ -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 diff --git a/05_software/train/rc_mjlab/src/robot/mdp/rewards.py b/05_software/train/rc_mjlab/src/robot/mdp/rewards.py index e8dfde6..9c7fb01 100644 --- a/05_software/train/rc_mjlab/src/robot/mdp/rewards.py +++ b/05_software/train/rc_mjlab/src/robot/mdp/rewards.py @@ -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() diff --git a/05_software/train/rc_mjlab/src/robot/robot_cfg.py b/05_software/train/rc_mjlab/src/robot/robot_cfg.py index 5d7403e..70ecde1 100644 --- a/05_software/train/rc_mjlab/src/robot/robot_cfg.py +++ b/05_software/train/rc_mjlab/src/robot/robot_cfg.py @@ -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}, diff --git a/05_software/train/rc_mjlab/src/robot/terrains/competition_terrains.py b/05_software/train/rc_mjlab/src/robot/terrains/competition_terrains.py index ac65bf5..420b969 100644 --- a/05_software/train/rc_mjlab/src/robot/terrains/competition_terrains.py +++ b/05_software/train/rc_mjlab/src/robot/terrains/competition_terrains.py @@ -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) diff --git a/README.md b/README.md index 9a50dfb..4e6e208 100644 --- a/README.md +++ b/README.md @@ -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 和比赛版本