cmvr_ai_lab/docs/humanoid_locomotion_onboarding_guide.md

# EngineAI Lab 新人形机器人行走训练攻略

本文面向 `engineai_lab` 原生 Isaac Lab + RSL-RL 训练链路，目标是把一个新的人形机器人从 URDF/MJCF 资产逐步推进到：

1. 模型能够稳定加载、复位和站立。
2. 低速行走时左右脚交替，不靠滑动前进。
3. 速度、稳定性和响应逐步提升。
4. 手臂与腿形成自然的对侧摆动，手腕保持稳定。
5. 通过课程学习达到更高速度，同时保留已有能力。

文中的数值以 Gen2 的实践为例。新机器人只能复用方法、结构和量级判断，不能直接照搬 Gen2 或 PM01 的高度、关节符号、摆幅和碰撞参数。

## 1. 先记住十条原则

1. **先验证模型，再设计奖励。** 错误的碰撞体、初始高度、关节方向和质量参数无法靠 PPO 修复。
2. **URDF root link 不等于运动控制 base。** root 可以是胸部，行走参考可以明确选髋部或 pelvis link。
3. **所有高度和姿态奖励都必须绑定明确的连杆。** 不要默认 `root_pos_w` 就是髋部状态。
4. **先固定低速平地，再加入复杂地形、推力和大范围指令。** 一次只增加一种难度。
5. **总 reward 不能跨奖励版本直接比较。** 应比较归一化跟踪能力、摔倒率、脚滑和固定指令回放。
6. **训练日志必须配合 replay。** 机器人可能用腰部扭转、脚底滑动或高频抖动获得看似不错的奖励。
7. **速度上限必须用课程逐步扩展。** 从 `0.6 m/s` 直接跳到 `1.6 m/s` 通常会破坏已有步态。
8. **关节坐标符号不能凭名字猜。** 左右镜像关节经常需要相同数值符号才能产生物理上的反向运动。
9. **训练任务和 Play 任务的 action scale 必须一致。** 否则回放时手腕、手臂或脚踝动作会被错误放大。
10. **每一阶段都保留可回退 checkpoint。** 新奖励没有提升时，从阶段入口重新训练，不要在已经退化的最终模型上继续堆补丁。

## 2. EngineAI Lab 中各模块的职责

以当前 Gen2 实现为参考：

| 模块 | 典型路径 | 职责 |
|---|---|---|
| 机器人资产 | `source/engineai_lab/assets/gen2/` | URDF、mesh 和简化碰撞体；审计结果写入 `outputs/` |
| 机器人配置 | `source/engineai_lab/robots/gen2.py` | q0、初始高度、关节顺序、执行器参数和资产加载 |
| 环境共享配置 | `source/engineai_lab/tasks/velocity/config/gen2/common_env_cfg.py` | scene、action、observation、reset、termination 和 Play 公共设置 |
| 分阶段环境 | `source/engineai_lab/tasks/velocity/config/gen2/stages/` | 每阶段独立的 reward、command、curriculum 和覆盖参数 |
| 分阶段 PPO | `source/engineai_lab/tasks/velocity/config/gen2/agents/` | 每阶段独立的迭代数、熵系数和学习率 |
| MDP 函数 | `source/engineai_lab/tasks/velocity/mdp/` | 自定义命令、观测、奖励和终止逻辑 |
| 任务注册 | `source/engineai_lab/tasks/velocity/config/gen2/__init__.py` | Gym task ID 到环境/PPO 配置的映射 |
| 训练入口 | `scripts/train.py` | 创建环境、加载 checkpoint、运行 RSL-RL |
| 回放入口 | `scripts/play.py` | 固定/随机/键盘指令、箭头、checkpoint 回放和 ONNX 导出 |
| 日志目录 | `logs/rsl_rl/<experiment_name>/` | checkpoint、TensorBoard events、env.yaml 和 agent.yaml |

推荐为新机器人复制结构，不要把所有机器人逻辑继续堆到 PM01 配置里：

```text
source/engineai_lab/robots/<robot>.py
source/engineai_lab/tasks/velocity/config/<robot>/
├── __init__.py
├── registry.py
├── common_env_cfg.py
├── stages/
│   ├── walk.py
│   ├── speed.py
│   └── natural.py
└── agents/
    ├── __init__.py
    ├── walk_ppo_cfg.py
    ├── speed_ppo_cfg.py
    └── natural_ppo_cfg.py
```

## 3. 总体推进路线和阶段门

```mermaid
flowchart LR
    A["资产静态检查"] --> B["Isaac 中加载"]
    B --> C["q0 与 zero-action"]
    C --> D["站立"]
    D --> E["低速交替踏步"]
    E --> F["平地速度与稳定性"]
    F --> G["自然全身协调"]
    G --> H["高速课程"]
    H --> I["地形与扰动鲁棒性"]
```

| 阶段 | 必须达到的结果 | 未达到时禁止做什么 |
|---|---|---|
| 资产检查 | 质量、惯量、关节限位和碰撞体可信 | 禁止开始 PPO |
| 加载与 reset | 无 NaN、无爆炸、脚底不深陷地面 | 禁止调行走奖励 |
| zero-action | q0 附近短时稳定，力矩和速度有限 | 禁止扩大 command |
| 站立 | timeout 高、非法接触低、姿态正确 | 禁止加入地形课程 |
| 低速行走 | 左右交替，有真实位移，不靠脚滑 | 禁止直接追求高速 |
| 速度巩固 | 固定速度误差下降，滑动和晃动可控 | 禁止同时增加地形和推力 |
| 自然协调 | 对侧摆臂，腕部稳定，步态未退化 | 禁止直接改全身 action scale |
| 高速课程 | 各级速度逐步通过，最终才接触目标上限 | 禁止一次性采样全范围高速 |

## 4. 第一步：建立机器人尺寸和语义表

在写配置之前，先从 URDF 和 CAD 资料整理一张表。后续所有奖励参数都从这里推导。

| 项目 | 要记录的内容 | 用途 |
|---|---|---|
| URDF root | root link 名称和物理位置 | 判断 `root_*` 状态代表胸还是髋 |
| pelvis | 最适合作为运动参考的髋部连杆 | 速度、朝向、高度和 command marker |
| torso | 胸部/上身连杆 | roll/pitch、上身稳定和腰髋一致性 |
| feet | 左右脚底对应的 link | 接触、滑动、腾空时间和落脚速度 |
| arm phase bodies | 左右肘或前臂 link | 判断对侧摆臂，避免手腕取巧 |
| 腿长 | pelvis 到脚底的默认垂直距离 | 高度、步幅、相位距离和 clearance |
| 臂长 | 肩到手或肘的近似链长 | 肩摆角、肘屈曲和空间相位幅度 |
| 髋宽 | 左右腿根或默认脚横向距离 | 站立脚位和交叉腿约束 |
| 默认站立高度 | pelvis、torso 和 root 的世界高度 | 高度奖励与 reset z |
| 关节语义 | pitch/roll/yaw、左右符号和 limit | action scale、默认位和对称奖励 |

### 4.1 不要用“机器人总身高”代替控制尺寸

行走奖励最常用的是：

- pelvis 到脚底的有效腿长。
- pelvis 和 torso 在 q0 下的实际世界高度。
- 肩到肘、肩到腕的链长。
- 默认两脚相对 pelvis 的三维位置。

头部高度通常不参与基础 locomotion。两台机器人总身高不同，不代表 pelvis 高度按同一比例变化。

### 4.2 从参考机器人缩放参数

如果参考机器人有可用参数，只按对应物理量缩放。

步态相位距离：

```text
d_phase,new = d_phase,ref * L_leg,new / L_leg,ref
```

若希望两台机器人手端的线位移接近，肩摆角近似按臂长反比缩放：

```text
theta_shoulder,new = theta_shoulder,ref * L_arm,ref / L_arm,new
```

高度目标必须直接测量新机器人 q0，不建议用总身高比例推算：

```text
h_pelvis,target = z(pelvis at q0 with soles on ground)
h_torso,target  = z(torso at q0 with soles on ground)
```

Gen2 实例中，PM01 参考腿长约 `0.82 m`，Gen2 有效腿长约 `0.786 m`；PM01 肩臂链约 `0.575 m`，Gen2 约 `0.70 m`。因此 Gen2 的相位距离略小，而肩、肘角幅也应减小。

## 5. 第二步：导入和清理模型资产

### 5.1 URDF 基础检查

导入前逐项确认：

| 检查项 | 合格标准 |
|---|---|
| link 名唯一 | 无重复、空名字或非法层级 |
| joint 名唯一 | 与控制器和硬件接口命名一致 |
| joint 类型 | 固定、旋转和连续关节语义正确 |
| axis | 与实际 pitch/roll/yaw 方向一致 |
| limit | `lower < upper`，effort/velocity 为正 |
| inertia | 对称、正定、单位为 SI |
| mass | 无 0 或负质量，整机总质量合理 |
| mesh 单位 | 统一为米，视觉模型没有 1000 倍缩放 |
| mesh 路径 | 绝对可解析或相对资产目录稳定 |
| root | `fix_base=False` 时形成合法浮动基座 |

可以把 Gen2 的静态检查脚本复制并参数化：

```bash
cd /home/xtkuang/Projects/cmvr/RL/cmvr_ai_lab

/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python \
  scripts/gen2_check_rl_readiness.py
```

该脚本检查质量、惯量、关节限位、碰撞重叠、脚底高度、reset z，以及 pelvis/torso 高度目标。用于新机器人时，必须修改其中的 URDF、foot link、robot cfg 和高度常量路径；任何 `unknown` 都会使检查失败，不能当作通过。

### 5.2 视觉 mesh 不适合直接作为训练碰撞体

高精度 mesh collision 常见问题：

- 碰撞三角面过多，仿真吞吐下降。
- 相邻连杆在 q0 下互相穿透。
- 足底不平，产生虚假侧向力和脚滑。
- 手臂、手腕碰撞体过大，站立时与躯干接触。
- 薄片 mesh 或非封闭 mesh 导致接触不稳定。

推荐顺序：

1. 保留原始 URDF，不直接覆盖。
2. 为训练生成 box、capsule 或简化 convex collision。
3. 单独输出碰撞审计 CSV。
4. 渲染正视、侧视、俯视和 3D 图。
5. 在 Isaac viewer 中打开 collision display 再确认。

Gen2 的辅助命令：

```bash
/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python \
  scripts/gen2_generate_simplified_collisions.py

/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python \
  scripts/gen2_visualize_collisions.py
```

### 5.3 足底碰撞体是第一优先级

足底应满足：

- 底面平整并接近视觉鞋底。
- q0 下左右脚底高度一致。
- collision bottom 与地面留有约 `5~10 mm` 的初始化余量。
- 足底前后长度和左右宽度合理，不能缩成点接触。
- foot frame 不必与世界轴对齐，但必须记录其固定 RPY offset。

如果脚底碰撞体不可信，`feet_slide`、`feet_contact`、`feet_orientation` 和 `landing_velocity` 都会给出误导信号。

### 5.4 自碰撞的启用顺序

初期建议：

```python
enabled_self_collisions = False
```

先让模型完成站立和基础行走。之后若真实部署需要处理手臂撞躯干、双腿相撞，再简化碰撞体并逐步启用 self collision。不要用严重重叠的碰撞体直接开启自碰撞训练。

## 6. 第三步：编写机器人 ArticulationCfg

机器人配置至少包含：

```python
ROBOT_CFG = ArticulationCfg(
    spawn=sim_utils.UrdfFileCfg(
        asset_path=str(ROBOT_URDF_PATH),
        activate_contact_sensors=True,
        fix_base=False,
        replace_cylinders_with_capsules=True,
        rigid_props=...,
        articulation_props=...,
        joint_drive=...,
    ),
    init_state=ArticulationCfg.InitialStateCfg(
        pos=(0.0, 0.0, INITIAL_ROOT_Z),
        joint_pos={...},
        joint_vel={".*": 0.0},
    ),
    actuators={...},
)
```

### 6.1 建立唯一、固定的关节顺序

必须明确三份顺序：

- 模型 articulation 内部顺序。
- 策略 observation/action 顺序。
- 硬件 DFS 或部署顺序。

推荐在机器人文件中显式定义：

```python
ROBOT_LEG_JOINT_NAMES = [...]
ROBOT_WAIST_JOINT_NAMES = [...]
ROBOT_ARM_JOINT_NAMES = [...]
ROBOT_DFS_JOINT_NAMES = ROBOT_LEG_JOINT_NAMES + ROBOT_WAIST_JOINT_NAMES + ROBOT_ARM_JOINT_NAMES

ROBOT_DFS_JOINT_ORDER_ASSET_CFG = SceneEntityCfg(
    "robot",
    joint_names=ROBOT_DFS_JOINT_NAMES,
    preserve_order=True,
)
```

任何一次新增、删除或重排 observation/action 都会改变 policy 输入输出契约，旧 checkpoint 通常不能继续加载。

### 6.2 q0 的要求

q0 应满足：

- 所有关节在 limit 内，并留出控制余量。
- 膝盖不是完全奇异的锁直状态。
- 左右脚底近似平行地面。
- 手臂与躯干无碰撞。
- 非零默认关节的位置真实可达。
- 左右镜像姿态符合 URDF 的符号定义。

当前 action 使用 `use_default_offset=True`，因此策略输出零通常表示 q0，而不是所有关节绝对角度为零。

### 6.3 为什么配置了 q0，reset 后仍不完全相同

有效初始姿态还会受到以下配置影响：

- `randomize_joint_default_pos` 修改 `default_joint_pos`。
- `reset_joints_by_scale` 对非零 q0 做乘法扰动。
- reset velocity 给关节初速度。
- delayed actuator 的历史缓冲在最初几步产生瞬态。
- 接触和重力在第一帧后改变姿态。

尤其注意：q0 为 `1.57 rad` 的腕关节使用 `(0.8, 1.2)` scale reset 时，会被随机到约 `1.26~1.88 rad`，幅度远大于零位关节。自然摆臂微调阶段应收窄 reset 范围或对腕部单独复位。

### 6.4 执行器参数的初始设置

先按硬件或厂商控制器给定的 stiffness、damping、effort 和 velocity limit 建模。没有可靠数据时：

- 髋、膝通常比踝和腕更强。
- 大腿主关节 stiffness/damping 较高。
- 腰部需要足够阻尼，避免用腰摆满足 yaw 奖励。
- 腕部执行器较弱，action scale 和速度约束应更小。
- 训练初期不要为了“站住”无限提高 stiffness。

## 7. 第四步：明确 locomotion reference body

### 7.1 root 在胸部时，不必强改 URDF 树

Gen2 的 URDF root 是胸部 `body_link`，但行走参考使用 pelvis `waist_link_2`。正确做法是让奖励、command metric 和 marker 显式选择 pelvis body，而不是假定 root 就是髋。

建议至少定义：

```python
ROBOT_TORSO_BODY_NAME = "..."
ROBOT_PELVIS_BODY_NAME = "..."
ROBOT_FEET_BODY_NAMES = ["left_foot", "right_foot"]
```

然后使用 body-specific 状态：

```python
asset.data.body_pos_w[:, pelvis_id]
asset.data.body_quat_w[:, pelvis_id]
asset.data.body_lin_vel_w[:, pelvis_id]
asset.data.body_ang_vel_w[:, pelvis_id]
```

### 7.2 root/base 语义错误的典型表现

| 表现 | 原因 |
|---|---|
| yaw reward 很好但腿没有转向 | 胸部或腰部旋转满足了 root yaw |
| orientation 很好但髋部明显歪 | 奖励只看胸部 roll/pitch |
| height 一直异常 | 使用胸部高度套用了 pelvis target |
| command 箭头位置不对 | marker 仍画在 root body |
| 实际速度箭头与运动方向不一致 | 使用了错误 body frame 或 heading offset |

### 7.3 heading offset 的求法

如果 pelvis 局部前向轴不是 URDF frame 的 `+X`，需要固定 yaw offset。

```text
heading_yaw = yaw(body_quat_w) - heading_yaw_offset
```

判断方法：

1. q0 下令机器人面向世界 `+X`。
2. 读取 pelvis body 的世界 yaw。
3. 令 `heading_yaw` 等于 0。
4. 得到固定 `heading_yaw_offset`。
5. 用固定前进指令检查目标箭头和实际箭头是否都指向世界 `+X`。

### 7.4 同时约束 pelvis 和 torso

只看 pelvis 会允许胸部摆动，只看 torso 会允许腰部取巧。推荐组合：

- pelvis 负责平面速度、高度和主要 heading。
- torso 负责 roll/pitch 姿态。
- pelvis 和 torso 共同跟踪 yaw rate。
- 增加 torso-pelvis yaw alignment。
- 增加 torso-pelvis yaw-rate difference penalty。

## 8. 第五步：测量 q0 几何常量

不要凭 URDF 文本手算所有目标。加载仿真并在 q0/reset 后直接记录：

```text
pelvis height
torso height
left/right foot position in pelvis heading frame
left/right foot orientation offset
left/right shoulder/elbow/hand position
```

推荐把结果固化为机器人专属常量：

```python
ROBOT_PELVIS_HEIGHT_TARGET = ...
ROBOT_TORSO_HEIGHT_TARGET = ...
ROBOT_PELVIS_HEADING_YAW_OFFSET = ...
ROBOT_DEFAULT_FOOT_POS_PELVIS_FRAME = (..., ...)
ROBOT_FOOT_SOLE_FRAME_OFFSETS_RPY = (..., ...)
```

高度 target 应以“脚底正确接触地面时的 body 高度”为准，而不是 `init_state.pos.z`。如果 root 是胸部，这两个数更不可能相同。

## 9. 第六步：接入 ManagerBasedRLEnv

### 9.1 Scene

Scene 至少包含：

- terrain importer。
- robot articulation。
- contact sensor，建议 `history_length >= 3` 且 `track_air_time=True`。
- 可选 height scanner。
- light，仅影响显示。

基础站立和平地速度巩固阶段可以直接使用 plane 并移除 height scanner。复杂地形阶段再恢复 terrain generator。

### 9.2 Action

位置控制的常见形式：

```python
joint_pos = mdp.JointPositionActionCfg(
    asset_name="robot",
    use_default_offset=True,
    preserve_order=True,
    joint_names=ROBOT_DFS_JOINT_NAMES,
    scale={...},
)
```

action scale 应按关节功能分组：

| 关节组 | 初始策略 |
|---|---|
| 髋/膝 sagittal | 较大，允许形成步幅 |
| 髋 roll/yaw | 较小，防止横摆和扭腿 |
| 踝 pitch | 中等，允许推蹬和落脚 |
| 踝 roll | 较小，避免脚底翻滚 |
| 肩 pitch | 中等，用于摆臂 |
| 肩 roll/yaw | 较小并保持默认位 |
| 肘 | 中小 |
| 腕 | 最小，通常单独限制 |

### 9.3 Observation

基础 humanoid velocity policy 推荐包含：

- 按固定顺序排列的 joint position relative to q0。
- joint velocity。
- previous action。
- pelvis angular velocity in heading frame。
- torso projected gravity。
- velocity command。
- 可选 terrain height scan。

历史长度可以改善相位感知，但输入维度会成倍增加。Gen2 使用 15 帧 history 后，policy observation 为 1353 维。确定 history 后不要在续训中随意改变。

### 9.4 Command

如果 root 不是 pelvis，使用类似 `BodyVelocityCommand` 的 body-referenced command term：

- 在指定 body heading frame 下计算实际速度。
- marker 画在指定 body 上方。
- yaw metric 使用指定 body 或 pelvis+torso 组合。
- 支持 standing、straight、heading 等采样比例。

低速初始范围建议从保守范围开始，例如：

```text
vx: 0.0 ~ 0.2/0.4 m/s
vy: 0 或非常小
wz: 0 或非常小
```

先学会向前，再加入横移和转向。

### 9.5 Event 和 reset

随机化应分阶段添加：

| 阶段 | 建议随机化 |
|---|---|
| 模型验证 | 全部关闭 |
| 站立 | 很小的 joint/reset 变化 |
| 基础行走 | friction、轻微 q0 和 COM 变化 |
| 速度巩固 | 保留物理随机化，关闭 push |
| 高速建立 | plane、无 push、窄转向 |
| 鲁棒性 | 最后恢复 push、terrain、较宽 friction/COM |

不要在“速度、地形、推力、横移、转向”五项中同时增加两三项以上。

### 9.6 Termination

基础终止通常包含：

- time out。
- 非法 body contact。
- 可选姿态、高度或越界终止。

非法接触 body 列表应包含胸、腰、手臂、大腿/小腿等不应触地的部位，但不要误把足部或合法膝部接触加入。

### 9.7 注册 Train 和 Play task

每个机器人至少注册：

```text
Flat-<Robot>-v0
Flat-<Robot>-Play-v0
```

不同阶段最好独立注册：

```text
Flat-<Robot>-Speed-v0
Flat-<Robot>-Natural-v0
Flat-<Robot>-Fast-v0
```

在 `config/<robot>/__init__.py` 中使用明确的环境和 PPO 配置入口：

```python
import gymnasium as gym

from . import agents


gym.register(
    id="Flat-<Robot>-v0",
    entry_point="isaaclab.envs:ManagerBasedRLEnv",
    disable_env_checker=True,
    kwargs={
        "env_cfg_entry_point": f"{__name__}.flat_env_cfg:RobotFlatEnvCfg",
        "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:RobotFlatPPORunnerCfg",
    },
)

gym.register(
    id="Flat-<Robot>-Play-v0",
    entry_point="isaaclab.envs:ManagerBasedRLEnv",
    disable_env_checker=True,
    kwargs={
        "env_cfg_entry_point": f"{__name__}.flat_env_cfg:RobotFlatEnvCfg_PLAY",
        "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:RobotFlatPPORunnerCfg",
    },
)
```

同时确认 `source/engineai_lab/tasks/velocity/config/__init__.py` 或其递归导入链会导入 `<robot>` 包。仅创建文件而不触发包导入，`gym.make()` 仍会报告 task ID 不存在。

只要 action mapping 不同，就必须有对应 Play task。例如训练中腕部 scale 为 `0.06`，普通 Play task 为 `0.15`，回放会把腕部动作放大 2.5 倍。

## 10. 第七步：训练前的静态与动态 sanity check

### 10.1 静态检查

```bash
/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python -m compileall \
  source/engineai_lab/robots/<robot>.py \
  source/engineai_lab/tasks/velocity/config/<robot>/
```

### 10.2 环境加载检查

用 1~4 个环境确认：

- Gym task 能从 registry 加载。
- scene、command、reward、termination manager 全部创建。
- joint/body regex 恰好匹配预期实体。
- observation 和 reward 全为 finite。
- command body id 是 pelvis。
- action dimension 与关节数一致。

### 10.3 reset/zero-action 检查

至少检查 20~100 个 policy steps：

- q0 误差。
- 最大关节速度。
- 最大力矩。
- pelvis/torso 高度。
- 足底是否穿地。
- 非足部是否接触地面。
- 延迟执行器启动时是否出现大动作。

如果 zero-action 会爆炸，不要开始 PPO。优先检查碰撞、初始高度、q0、执行器目标、关节索引和 reset 逻辑。

## 11. RewardManager 的数值语义

Isaac Lab RewardManager 每步执行：

```text
step_reward_i = raw_term_i * weight_i * step_dt
```

Gen2 当前：

```text
sim.dt = 0.002 s
decimation = 5
step_dt = 0.01 s
episode_length = 20 s
```

因此一个取值始终为 1、weight 为 1 的正奖励，完整 20 秒 episode 的累计值约为 20。TensorBoard 中 `Episode_Reward/*` 还会按 episode 最大时长归一化，所以对于原始范围为 `[0,1]` 的正奖励：

```text
Episode_Reward / weight
```

可以近似理解为平均完成度。这个方法不适用于无界 penalty，也不适用于改过公式或采样分布后的直接横向比较。

## 12. 奖励函数的推荐分层

不要从一开始加入几十个强奖励。按以下优先级逐层添加。

### 12.1 第一层：任务必要条件

| 奖励 | 目的 |
|---|---|
| pelvis planar velocity tracking | 真正跟随平面速度命令 |
| whole-body yaw-rate tracking | pelvis 和 torso 一起转向，腰不能取巧 |
| torso orientation | 控制胸部 roll/pitch |
| pelvis/torso height | 防止蹲塌和跳跃 |
| termination penalty | 明确惩罚摔倒和非法接触 |

### 12.2 第二层：可行步态

| 奖励 | 目的 |
|---|---|
| biped contact mode | 移动时鼓励单脚支撑，站立时双脚接触 |
| feet air time | 形成完整摆动期 |
| dense air time | 避免稀疏奖励难以探索 |
| swing clearance | 抬脚越过地面而不是拖脚 |
| default foot placement | 站立时保持合理脚距 |
| foot orientation | 避免脚底翻转 |

### 12.3 第三层：质量和效率

| 惩罚 | 目的 |
|---|---|
| feet slide | 减少接触脚滑动 |
| action rate | 限制一阶动作变化 |
| action smoothness | 限制二阶动作变化 |
| dof acceleration | 降低高频关节振动 |
| torque/energy | 降低不必要输出 |
| joint limits | 避免撞限位 |
| stumble | 避免脚侧面撞击障碍/地面 |

### 12.4 第四层：全身自然性

| 奖励 | 目的 |
|---|---|
| torso-pelvis alignment | 防止腰胸相对扭转 |
| contralateral arm phase | 左脚前时右臂前，反之亦然 |
| wrist default deviation | 手腕只允许小范围补偿 |
| wrist velocity | 防止腕部高频乱动 |
| speed-scaled arm target | 速度越快摆臂越明显，但幅度封顶 |

## 13. 分阶段训练建议

下面是一套可复用的阶段划分。每一阶段从上一阶段最好的 checkpoint 续训。

### 13.1 阶段 0：站立与模型验证

目标：证明模型、q0、碰撞和执行器可用。

配置：

- command 为零或绝大部分 standing。
- plane。
- 关闭 push 和 terrain curriculum。
- 强化姿态、高度、双脚接触和 termination。
- 暂不强调 air time 和高速 tracking。

通过标准：

- timeout 接近 100%。
- base contact 接近 0%。
- pelvis/torso 高度稳定。
- zero-action 和 policy replay 都不爆炸。

### 13.2 阶段 1：低速交替踏步

目标：打破“原地站立最优解”，形成左右交替。

配置：

- `vx` 上限约 `0.2~0.4 m/s`。
- `vy=0`、`wz=0` 或很小。
- 增加 contact mode、dense air time 和 clearance。
- 跟踪奖励必须足够强，不能被站立姿态奖励淹没。

通过标准：

- replay 中左右脚轮流抬起。
- pelvis 实际速度方向与 command 一致。
- 不是双脚小碎步或脚底滑行。
- 固定低速命令下能完成大部分 episode。

### 13.3 阶段 2A：步态清晰度

目标：解决“会挪动，但抬脚不清晰、步态像拖地”。

调整顺序：

1. 检查足底 contact 和 clearance 几何是否正确。
2. 增加 dense air time，不先盲目提高稀疏 touchdown 奖励。
3. 适度提高 clearance target 和 weight。
4. 保留 feet slide penalty。
5. 观察是否开始高抬腿；若是，降低 clearance 或增大落脚约束。

### 13.4 阶段 2B：速度与稳定性

目标：从慢速挪动提升到稳定跟踪中速命令。

配置：

- 扩大 `vx`，但保持 plane。
- 大部分环境采样直线。
- 增加 pelvis vertical velocity、pelvis/torso roll-pitch angular velocity 和 landing velocity。
- 降低 PPO entropy，减少后期探索抖动。
- 暂停 terrain curriculum 和 push。

这一步最容易犯的错误是同时扩大速度、地形和推力。Gen2 曾出现 terrain level 从约 `0.9` 快速升到 `5.5`，速度跟踪只小幅提高，但 base contact、脚滑和加速度明显恶化。

### 13.5 阶段 2C：平地巩固

如果速度增加但 replay 看起来没有变好，做一轮专门的平地巩固：

- plane。
- 无 terrain curriculum。
- 无 push。
- 约 80% 直线。
- 加强 feet slide、落脚、pelvis vertical 和 torso angular velocity。
- entropy 从约 `0.008` 降到 `0.004` 量级。

Gen2 在这一步才把 `0.4 m/s` 固定命令下的实际速度从约 `0.23` 提升到 `0.31 m/s`，同时显著降低胸部倾斜；代价是脚滑仍需继续约束。

### 13.6 阶段 3：自然摆臂

目标：保持已有腿部步态，只让上肢学会自然协调。

推荐双重约束：

1. 用肩 pitch 和肘 flexion joint target 跟踪腿部 phase。
2. 用左右肘/前臂在 heading frame 下的空间位置验证真实对侧摆动。

不要用手端位置直接作为唯一相位奖励，否则策略可能通过大幅转动手腕来“移动手端”。

腕部处理：

- J5/J6/J7 action scale 单独减小。
- 位置偏差使用带小 deadband 的 L1 penalty，避免指数奖励在大误差时完全饱和。
- 增加 wrist velocity penalty。
- Natural Play task 使用相同 action scale。
- reset 对非零腕关节 q0 的随机范围要收窄。

### 13.7 阶段 4：高速课程

目标速度跨度较大时使用分段 curriculum。例如从 `0.6` 提升到 `1.6 m/s`：

```text
0.3~0.8 -> 0.3~1.0 -> 0.3~1.2 -> 0.3~1.4 -> 0.3~1.6 m/s
```

每个阶段至少保留约 100~200 PPO iterations。最后一个目标速度还要单独训练一段时间，不能课程一到 `1.6` 就结束。

高速阶段建议：

- 90% 直线。
- 5% standing。
- 横向和 yaw 范围缩小。
- plane、无 push。
- 增加 dense forward undertracking L1 penalty。
- 扩大 tracking kernel，避免大误差下指数奖励接近零。
- clearance 略提高，但放宽合理的 pelvis vertical motion 和 landing threshold。

达到目标速度后，再开一个独立鲁棒性阶段恢复 push、friction 和 terrain，不要污染高速能力建立阶段。

## 14. 训练命令模板

### 14.1 从头训练

下列 `<...>` 是需要替换的占位符，不能原样执行。

```bash
cd /home/xtkuang/Projects/cmvr/RL/cmvr_ai_lab

/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python scripts/train.py \
  --task Flat-<Robot>-v0 \
  --num_envs 4096 \
  --seed 42 \
  --max_iterations <iterations> \
  --run_name <run_name> \
  --device cuda:0 \
  --rl_device cuda:0 \
  --headless
```

### 14.2 同一 Task 从 checkpoint 完整续训

同一任务中断后继续时使用 `resume`，它会恢复 optimizer、iteration 和 curriculum：

```bash
/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python scripts/train.py \
  --task Flat-<Robot>-<Stage>-v0 \
  --num_envs 4096 \
  --seed 42 \
  --max_iterations <additional_iterations> \
  --resume True \
  --load_mode resume \
  --load_run <previous_run_directory> \
  --checkpoint model_<iteration>.pt \
  --run_name <new_run_name> \
  --device cuda:0 \
  --rl_device cuda:0 \
  --headless
```

当前 CLI 的 `--resume` 使用布尔值参数，应写成 `--resume True`，不能只写一个裸 `--resume`。

### 14.3 跨阶段迁移 checkpoint

从一个 Task 进入下一阶段时必须使用 `finetune`。它只加载 actor/critic，使用新阶段的 optimizer 和 curriculum：

```bash
/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python scripts/train.py \
  --task Flat-<Robot>-<NextStage>-v0 \
  --num_envs 4096 \
  --seed 42 \
  --max_iterations <iterations> \
  --resume True \
  --load_mode finetune \
  --load_run <previous_stage_run_directory> \
  --checkpoint model_<iteration>.pt \
  --run_name <next_stage_run_name> \
  --device cuda:0 \
  --rl_device cuda:0 \
  --headless
```

checkpoint 会记录原 Task ID；跨 Task 使用完整 `resume` 会被训练入口拒绝，这是为了防止错误恢复旧 curriculum。

### 14.4 TensorBoard

```bash
/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/tensorboard \
  --logdir logs/rsl_rl/<experiment_name> \
  --port 6006
```

## 15. 回放与固定基准

### 15.1 不要用随机 replay 比较 checkpoint

随机命令、随机摩擦、随机 q0 和不同地形会掩盖真实差异。比较策略时统一使用：

- 同一个 deterministic Play task。
- 同一个 command source。
- 同一个 `vx/vy/wz`。
- 同一个 command mode 和 acceleration limit。
- 同一个仿真时长。

### 15.2 固定速度回放

```bash
/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python scripts/play.py \
  --task Flat-<Robot>-<Stage>-Play-v0 \
  --load_run '<run_regex>' \
  --checkpoint model_<iteration>.pt \
  --num_envs 1 \
  --command_source fixed \
  --command_mode ramp \
  --linear_accel 0.6 \
  --vx 0.4 \
  --vy 0.0 \
  --wz 0.0 \
  --device cuda:0 \
  --rl_device cuda:0
```

高速测试优先使用 ramp，避免从 0 瞬间跳到目标速度。响应测试再使用 step。

### 15.3 建议记录的固定基准

| 指标 | 含义 |
|---|---|
| commanded vx | 目标速度 |
| pelvis actual vx mean | 实际平均前进速度 |
| vx MAE | 跟踪误差 |
| vx std | 前后速度抖动 |
| lateral velocity RMS | 左右晃动 |
| torso tilt RMS | 上身倾斜 |
| pelvis vertical velocity RMS | 上下颠簸 |
| contact-foot slide | 接触脚滑动 |
| action delta RMS | 控制抖动 |
| base-contact resets | 摔倒/非法接触 |

至少测试：

```text
stand, 0.2, 0.4, 0.6 m/s
```

高速策略再测试：

```text
0.8, 1.0, 1.2, 1.4, 1.6 m/s
```

## 16. 如何读训练日志

### 16.1 第一优先级：是否活着

| 指标 | 健康趋势 |
|---|---|
| `Episode_Termination/time_out` | 越接近 1 越好 |
| `Episode_Termination/base_contact` | 越接近 0 越好 |
| `Train/mean_episode_length` | 接近最大 episode steps |

如果 base contact 持续升高，即使 mean reward 上升，也不能认为策略更好。

### 16.2 第二优先级：是否完成任务

对 `[0,1]` positive reward，可以查看：

```text
normalized_completion ~= Episode_Reward / weight
```

例如 tracking 日志为 `1.93`、weight 为 `2.5`，归一化完成度约 `0.77`。

调整 weight 后，必须用归一化值比较，不能比较原始日志。

### 16.3 第三优先级：动作质量

重点观察：

- `feet_slide`。
- `dof_acc`。
- `action_rate`。
- `action_smoothness`。
- `pelvis_vertical_velocity`。
- `pelvis_roll_pitch_ang_vel`。
- `feet_landing_velocity`。
- `Policy/mean_std`。

高速度奖励上升但 slide、acc、action rate 同时恶化，通常表示策略通过更激烈的动作换速度，视觉效果不一定提升。

### 16.4 不能直接比较的指标

- 修改 reward 数量或 weight 后的 mean reward。
- 修改 command 范围后的 tracking reward。
- 修改 command resampling time 后的 command metric。
- 修改 terrain curriculum 后的 episode reward。
- 修改 action scale 后的 action magnitude。

当前 `BodyVelocityCommand._update_metrics` 使用最大 command 时间归一化。若 resampling 从 `7.5 s` 改成 `5 s`，同样误差的日志尺度也会变化。

## 17. 症状反查表

| 症状 | 首要检查 | 常见原因 | 优先修复 |
|---|---|---|---|
| reset 后立即爆炸 | collision、q0、root z、actuator | 碰撞重叠、脚陷地、索引错 | 修模型，不调 reward |
| 只能站立 | command observation、tracking budget | 站立奖励压过速度奖励 | 缩小任务并提高 tracking |
| 原地碎步 | actual pelvis vx、脚滑 | air-time 有收益但前进无收益 | 增强 pelvis tracking 和 undertracking |
| 双脚滑着前进 | foot contact/velocity | slide 太弱、足底碰撞错误 | 先修足底，再加 slide penalty |
| 只抬一只脚 | 左右 foot id、phase、reset | 顺序错或左右奖励不对称 | 强制 preserve_order，查 id |
| 同手同脚 | 关节轴、空间 arm phase | 只按关节名猜符号 | 用肘/前臂空间位置验证反相 |
| 手腕乱甩 | action scale、reward saturation | 腕部 scale 太大，exp 奖励接近 0 | 减 scale，改 deadband L1 + vel penalty |
| 胸转了但腿没转 | yaw reference | root 在胸部，腰部取巧 | pelvis+torso 共同跟踪 yaw |
| 身体高度不对 | target body 和尺寸 | 复制 PM01 height | 测量新机器人 q0 body height |
| 箭头方向错误 | heading offset | pelvis frame 的前向轴不是 +X | 标定固定 yaw offset |
| 走得更快但更晃 | std、action penalties、terrain | 探索过高或任务同时变难 | 平地巩固，降低 entropy |
| reward 上升但 replay 无变化 | weight 和 command 分布 | 总 reward 不可比 | 固定命令 benchmark |
| 中后期越来越容易摔 | curriculum、push | terrain level 持续提升 | 冻结地形，回退较早 checkpoint |
| clearance 很好但高抬腿 | target/weight 太高 | clearance 主导步态 | 降 target/weight，增 landing |
| 速度上不去 | tracking kernel、command curriculum | 大误差下 exp 接近 0 | 分级扩速 + dense undertracking |
| 低速好，高速一给就倒 | 速度跨度过大 | 没有逐级 checkpoint | 0.2 m/s 一级课程 |
| Play 比训练腕部动作大 | Train/Play action scale | 使用了错误 Play task | 为该阶段注册专用 Play cfg |
| 训练初期腕 J5 偏差很大 | reset scale、delay | 非零 q0 被乘法随机化 | 收窄 reset 或腕部单独 reset |

## 18. 奖励微调的正确顺序

当行为不对时，按以下顺序定位，避免同时改十个 weight。

### 18.1 第 1 层：确认测量对象正确

检查 body/joint/sensor IDs、frame、heading offset、foot order 和 contact threshold。测量错了，权重再大也无效。

### 18.2 第 2 层：确认奖励有数值

查看该项日志是否：

- 长期接近上限，说明可能太容易或无区分度。
- 长期接近 0，说明公式饱和或事件太稀疏。
- 数量级比其他项小 100 倍，说明几乎不起作用。
- 数量级大到主导总 reward，说明策略可能只优化这一项。

Gen2 的早期 speed stability 中，vertical 和 landing 项只有约 `-0.001`，相对 action smoothness 约 `-0.78` 几乎不起作用。增加 reward 名称并不代表它真的影响策略。

### 18.3 第 3 层：先改公式，再改 weight

常见公式问题：

- 大误差下 `exp(-error)` 接近 0，没有学习信号。
- touchdown-only 奖励太稀疏。
- L2 对大异常过强，对小误差过弱。
- 站立和移动使用同一目标。
- 原始 body frame 与期望 heading frame 不一致。

对应修复：

- 增加 linear/Huber undertracking term。
- 增加 dense air-time 辅助项。
- 使用 deadband L1 管理腕部等小动作。
- 根据 command 判断 stand/move。
- 统一转换到 pelvis heading frame。

### 18.4 第 4 层：小步调整 weight

一次调整约 1.5~3 倍，观察 100~300 iterations。不要从 `-0.1` 直接跳到 `-10`，否则已有 gait 很容易消失。

### 18.5 第 5 层：重新选择 checkpoint

最终 checkpoint 不一定最好。若 tracking 在中期已平台，而 terrain、fall 或 policy std 后期继续恶化，应固定条件比较中期和最终 checkpoint，再从更平衡的模型续训。

## 19. 常见失败案例的完整处理

### 19.1 base 在胸部导致“假转向”

错误做法：所有 tracking、orientation 和 height 都读取 root。

正确做法：

- pelvis 跟踪 planar velocity。
- pelvis 和 torso 共同跟踪 yaw rate。
- torso 控制 roll/pitch。
- pelvis 使用自己的高度 target。
- torso-pelvis alignment 防止腰部取巧。

### 19.2 复制 PM01 高度导致奖励长期异常

错误做法：直接把 PM01 `base_height=0.82` 用到新机器人。

正确做法：

1. 确定 PM01 base 是髋，而新机器人 root 是否也是髋。
2. 在 q0 下测新 pelvis 和 torso 高度。
3. 分别写 body height reward。
4. 检查 foot sole collision bottom，而不是只看视觉模型。

### 19.3 第二阶段“看起来没有提升”

可能同时发生：

- 实际速度提高。
- 脚滑增加。
- 前向速度 std 增加。
- terrain 难度升高。
- base contact 增加。

这不是单一结论。应在 deterministic plane 下固定 `0.2/0.4/0.6 m/s` 比较 actual vx、MAE、torso tilt、slide 和 reset。Gen2 的案例证明策略确实变快，但脚滑抵消了视觉收益，因此后续需要平地巩固而不是继续原配置。

### 19.4 自然摆臂出现同手同脚

先检查左右肩 pitch 的 URDF axis。左右镜像轴可能意味着：

```text
相同 joint delta -> 物理上相反的前后摆动
```

不要只看 joint sign。增加物理空间相位：

```text
leg_phase = x(left_foot) - x(right_foot)
arm_phase = x(right_elbow) - x(left_elbow)
```

希望两者同号。左脚在前时，右肘/前臂在前；右脚在前时，左肘/前臂在前。

### 19.5 高速目标奖励没有梯度

假设 command 为 `1.6 m/s`，实际只有 `0.6 m/s`，error 为 1。若使用 `exp(-5 * error^2)`，奖励约为 `0.0067`，继续变差或略微变好都很难区分。

修复：

- 用课程把 command 每次只提高约 `0.2 m/s`。
- 降低 tracking kernel 的 sigma。
- 增加 `max(command_x - actual_x, 0)` 形式的 dense undertracking penalty。
- 在最终目标速度保留足够训练时间。

## 20. Checkpoint 和实验记录规范

每次训练至少保存：

```text
run name
parent run
parent checkpoint
task ID
max iterations
command ranges
reward version
action scale version
terrain/push 状态
best visual checkpoint
final checkpoint
fixed-speed benchmark
```

推荐 run name：

```text
<robot>_stand_v1
<robot>_gait_low_speed_v1
<robot>_gait_clearance_v2
<robot>_speed_stability_v1
<robot>_speed_flat_consolidation_v2
<robot>_natural_arm_swing_stage3_v1
<robot>_fast_1p6_curriculum_v1
```

训练开始后，实际 `env.yaml` 和 `agent.yaml` 才是这一轮的事实来源。不要只凭当前代码猜旧 run 使用了什么配置。

## 21. 新机器人接入模板清单

### 21.1 模型清单

- [ ] URDF 能被 XML parser 和 Isaac converter 读取。
- [ ] mesh 单位和路径正确。
- [ ] 所有 movable joint 有合法 limit。
- [ ] 所有 link 有合理 mass 和正定 inertia。
- [ ] 足底 collision 平整。
- [ ] q0 下无严重非相邻 collision overlap。
- [ ] root z 使脚底不穿地。
- [ ] 左右关节 axis 和 limit 已人工核对。

### 21.2 机器人配置清单

- [ ] q0 在 limit 内且物理可站立。
- [ ] joint order 明确并 preserve_order。
- [ ] actuator stiffness/damping/effort/velocity 合理。
- [ ] pelvis、torso、feet、arm phase body 名明确。
- [ ] pelvis heading offset 已标定。
- [ ] pelvis/torso height 已测量。
- [ ] 默认 foot positions 和 sole RPY offsets 已测量。

### 21.3 环境清单

- [ ] Scene 能创建。
- [ ] observation/action dimension 正确。
- [ ] command 使用 pelvis body。
- [ ] contact sensor 匹配左右脚。
- [ ] base contact 不包含合法 foot body。
- [ ] reset 和 zero-action 全部 finite。
- [ ] Play task 与 Train task action mapping 一致。

### 21.4 训练阶段清单

- [ ] 站立通过后才训练低速。
- [ ] 低速左右交替后才扩 command。
- [ ] 速度和稳定性先在 plane 巩固。
- [ ] terrain 和 push 后加。
- [ ] 自然摆臂不改变 policy I/O dimension。
- [ ] 高速使用课程，不一次跳到目标上限。
- [ ] 每阶段都做固定指令 replay。

## 22. 最终推荐的工作习惯

1. 每次只提出一个可验证假设，例如“慢是因为 tracking 梯度不足”，不要先改十个参数。
2. 先检查日志数量级，再决定 weight。
3. 每次奖励改动都记录 parent checkpoint 和固定速度基准。
4. Train 和 Play 配置一起修改、一起验证。
5. 任何姿态、高度和速度问题都先问“当前读取的是哪个 body、哪个 frame”。
6. 任何左右协调问题都先检查 `preserve_order` 和 URDF axis。
7. 任何脚滑问题都先检查 collision 和 contact，再调 reward。
8. 任何高速问题都先做课程和 dense tracking，再放宽动作约束。
9. 策略能力和鲁棒性分阶段训练，先会做，再学会在扰动中做。
10. 最终评价标准永远是统一条件下的真实运动，而不是单个 TensorBoard 数字。

## 23. 当前 Gen2 参考实现

当前仓库中可以直接参考：

- [`robots/gen2.py`](../source/engineai_lab/robots/gen2.py)
- [`config/gen2/README.md`](../source/engineai_lab/tasks/velocity/config/gen2/README.md)
- [`config/gen2/common_env_cfg.py`](../source/engineai_lab/tasks/velocity/config/gen2/common_env_cfg.py)
- [`config/gen2/stages/`](../source/engineai_lab/tasks/velocity/config/gen2/stages)
- [`config/gen2/agents/`](../source/engineai_lab/tasks/velocity/config/gen2/agents)
- [`mdp/commands.py`](../source/engineai_lab/tasks/velocity/mdp/commands.py)
- [`mdp/rewards.py`](../source/engineai_lab/tasks/velocity/mdp/rewards.py)
- [`scripts/train.py`](../scripts/train.py)
- [`scripts/play.py`](../scripts/play.py)
- [`scripts/gen2_check_rl_readiness.py`](../scripts/gen2_check_rl_readiness.py)

这些文件展示了从 pelvis base 对齐、平地巩固、自然摆臂到 `1.6 m/s` 高速课程的完整实现。接入新机器人时，应复制结构和诊断方法，再用新机器人的 URDF 语义与尺寸重新计算参数。