Ch19 · 四足 + 机械臂：Loco-Manipulation

定位：Part V 第一章。将 Ch13（四足 Locomotion）和 Ch17（操作）的工程能力融合——当机器人需要同时"走"和"抓"时，locomotion 和 manipulation 的 MDP 设计如何融合？ 关键文献：VBC — Visual Whole-Body Control for Legged Loco-Manipulation（CoRL'24 Oral）；ETH Badminton — Learning Coordinated Badminton Skills for Legged Manipulators（Science Robotics'25） 关键框架：mjlab + Isaac Lab 双框架 · awesome-loco-manipulation URDF 库（Go2-Arx / B1-Z1） 机器人：Unitree Go2 + ARX L5 Pro · Unitree B1 + Z1 · ANYmal-D + DynaArm 累积项目：E（在 Ch17 的操作模块和 Ch18 的视觉模块基础上，增加移动操作整合） 前置要求：Ch13（四足 Locomotion）、Ch17（操作）、Ch18（视觉感知） 本章定位：复合形态的入口。Ch17 的操作是固定基座的（机械臂不动），本章让底座"活"起来——四足底盘提供移动性，机械臂提供操作性，两者同时工作产生的耦合效应是核心工程挑战。 阅读时间估计：精读约 6-8 小时（含动手实验），快速浏览约 2.5 小时。§19.7-19.8 的案例精读需要较深的 Ch13+Ch17 基础。

前置自测

📋 答不出 ≥ 3 题 → 先回前置章节复习

[Ch13] 四足 locomotion 的 velocity tracking reward 是什么形式？\(\sigma\) 参数如何影响跟踪精度？
[Ch17] Staged reward 的乘法门控为什么优于加法组合？从梯度传播的角度解释。
[Ch17] JointPositionAction、DiffIK 和 OSC 三种动作空间各适用于什么场景？操作任务中如何选择？
[Ch09] Teacher-student 蒸馏中，teacher 看到的 privileged 信息有哪些？student 如何从有限输入中恢复等价信息？
[Ch18] 视觉 locomotion 的三阶段蒸馏管线（state teacher → depth student → RGB student）每阶段解决什么问题？
[物理] 当四足机器人伸出机械臂抓取远处物体时，整体重心（CoM）如何变化？这对底盘稳定性有什么影响？
[工程] MJCF 中如何将两个独立的 XML 模型（底盘和机械臂）组合成一个完整的机器人？attach 和 include 的区别是什么？

本章目标

学完本章后，你应该能够：

分析移动操作中底盘运动与机械臂运动的三重耦合关系——逆运动学耦合、动力学耦合和感知耦合
组合建模四足底盘 + 机械臂的 MJCF/USD 资产，正确处理 mount frame、惯性参数和碰撞几何
设计混合动作空间——底盘速度命令（m/s）+ 臂关节位置（rad）+ 末端增量（m），统一量纲使梯度平衡
配置 observation group，正确融合 base state、arm joint state 和 object relative pose
工程实现 navigation reward + EE tracking reward + grasp reward + stability regularization 的多目标融合
执行 arm-constrained 四阶段课程——从冻结臂到全身联合训练的渐进过渡
精读 VBC 的分层架构（低层 whole-body tracker + 高层 visual policy）和 hot-start 技巧
理解 ETH 羽毛球的 constrained RL 方法论——感知噪声模型、关节约束和主动感知行为

本章知识全景图

Loco-Manipulation 知识树
├── 【本章核心】四足 + 机械臂
│   ├── 三重耦合（§19.1）
│   │   ├── 逆运动学耦合：底盘姿态改变 arm workspace
│   │   ├── 动力学耦合：arm 运动改变 CoM/角动量
│   │   └── 感知耦合：导航误差传递到操作精度
│   ├── 复合建模（§19.2）
│   │   ├── MJCF 组合（attach / include / mount frame）
│   │   └── Isaac Lab 多 Entity 挂载
│   ├── 混合动作空间（§19.3）
│   │   ├── 底盘速度 + 臂关节 + 末端增量
│   │   └── action scale 归一化
│   ├── Observation 融合（§19.4）
│   │   ├── base + arm + object state
│   │   └── 相对位姿 vs 绝对位姿
│   ├── 多目标 Reward 融合（§19.5）
│   │   ├── 导航 reward（base → object）
│   │   ├── 接近 reward（EE → object）
│   │   ├── 抓取 reward（grasp success）
│   │   └── 稳定性 reward（anti-tip regularization）
│   ├── 四阶段课程（§19.6）
│   │   ├── Phase 0：冻结臂 locomotion
│   │   ├── Phase 1：冻结底盘 arm reach
│   │   ├── Phase 2：全身 mobile reach + grasp
│   │   └── Phase 3：加 DR + 扰动
│   └── 精读案例
│       ├── VBC（CoRL'24，§19.7）
│       └── ETH 羽毛球（Science Robotics'25，§19.8）
├── 【回顾桥】Ch13 四足 locomotion · Ch17 操作 · Ch18 视觉
├── 【前瞻】Ch20 人形全身控制 · Ch21 轮式底盘 + 双臂
└── 平台参考
    ├── Go2 + ARX L5 Pro
    ├── B1 + Z1
    └── ANYmal-D + DynaArm

19.1 移动操作的三重耦合：为什么不是 locomotion + manipulation 的简单拼接 ⭐⭐

这一节解决什么问题：建立"移动操作不等于走路+抓东西"的认知。Ch13 教了四足怎么走，Ch17 教了机械臂怎么抓——但把两者放在一起时，新的工程挑战不是两者之和，而是两者之积。

动机：如果分别训练底盘和机械臂会怎样

最直觉的方案是"分离控制"：底盘用 Ch13 的 velocity tracking policy 走到物体附近，停下来，然后机械臂用 Ch17 的 lift cube policy 抓取物体。这个方案在以下条件下可以工作：

底盘能精确停在目标位置（±2cm）
机械臂的工作空间覆盖目标物体
抓取过程中底盘完全不动
物体是静态的（不会移动）

但在实际场景中，这四个条件很少同时满足。底盘的位置精度通常在 ±5-10cm（特别是在粗糙地面上），机械臂的工作空间半径只有 30-60cm（如 ARX L5 Pro 为 45cm），抓取过程中底盘会因为机械臂的反作用力而漂移，目标物体可能在移动（如传送带上的物体）。

更根本的问题是，分离控制丧失了"协调"的能力。一个人类在拿桌上的杯子时，不是"走到桌边 → 停下 → 伸手"——而是"边走边伸手，在接近桌子的最后几步同时完成定位和抓取"。这种协调让整个过程更快、更自然、更鲁棒。Loco-manipulation 的工程目标就是让机器人实现类似的协调。

用一个日常类比：想象你端着一盘食物穿过拥挤的餐厅。你的腿（locomotion）在导航避障，你的手臂（manipulation）在保持盘子水平，你的目光（perception）同时关注前方路线和盘子上的食物。这三者不是独立工作的——你会为了不撒食物而放慢脚步（动力学耦合），你会根据地面平整度调整手臂高度（逆运动学耦合），你会因为低头看食物而差点撞到柱子（感知耦合）。机器人的 loco-manipulation 面临完全相同的三重耦合。

耦合一：逆运动学耦合

底盘姿态改变了 arm 的可达工作空间。 当四足机器人在不平地面上行走时，底盘的 pitch/roll 角度在持续变化。对于安装在底盘上的机械臂来说，底盘的每一度倾斜都会改变臂末端在世界坐标系中的位置。

定量分析：假设机械臂安装在底盘中心上方 30cm 处，臂长 50cm。当底盘 pitch 10°时，臂底部的世界坐标在 x 方向偏移 \(0.3 \times \sin(10°) \approx 5.2\) cm，在 z 方向偏移 \(0.3 \times (1 - \cos(10°)) \approx 0.5\) cm。对于臂长只有 50cm 的轻量机械臂来说，5cm 的偏移占工作空间半径的 10%——这意味着底盘的姿态变化直接吃掉了机械臂 10% 的有效工作范围。

如果不处理这个耦合——即机械臂的 IK 不考虑底盘姿态——会怎样？末端位置会系统性地偏离目标，偏离量与底盘倾斜角成正比。在平地上这个问题不明显（底盘几乎水平），但在粗糙地面上底盘 pitch/roll 可达 ±15°，此时偏离可达 ±8cm——远超抓取的精度要求。

工程解决方案：在 observation 中包含 base orientation（作为 projected gravity 或 quaternion），让策略隐式学会补偿底盘倾斜。或者在 action 空间中使用 DiffIK（笛卡尔空间控制），让 IK 求解器自动补偿底盘姿态变化。VBC 使用后者——低层策略直接跟踪世界坐标系中的 EE 目标位姿，IK 求解器在每一步重新计算关节角度以补偿底盘变化。

耦合二：动力学耦合

机械臂的运动改变了整体的重心（CoM）和角动量。 一个 2-3 kg 的机械臂（如 ARX L5 Pro 含夹爪约 2.5 kg）安装在 Go2 底盘上（Unitree 官方整机带电池约 15 kg；若用某个无电池 URDF 的 inertial 质量可能约 12 kg，请以具体模型为准）。当臂完全伸展时，CoM 从底盘中心向臂伸出的方向偏移约 5-8 cm。

这个 CoM 偏移对底盘稳定性的影响：Go2 的支撑多边形（4 脚构成的四边形）对角线约 40cm。CoM 偏移 8cm 意味着 CoM 在支撑多边形内的"余量"从中心的 20cm 减小到 12cm——稳定性下降了 40%。如果机器人同时在行走（周期性地只有 2-3 脚着地），实际的稳定性余量更小。

更严重的是动态效应：当机械臂快速挥动时（如 ETH 羽毛球的 12 m/s 挥拍），角动量变化会在底盘上产生反作用力矩。如果底盘的 locomotion 策略没有预期到这个力矩，机器人会在挥臂的瞬间失去平衡。

如果不处理动力学耦合会怎样？两种典型失败模式：(1) 机械臂伸出时机器人向前倾倒（CoM 偏移超出支撑多边形），(2) 机械臂快速运动时机器人步态被打乱（角动量传递到底盘导致意外的 yaw/pitch 变化）。

定量分析 CoM 偏移的影响：以 Go2 + ARX L5 Pro 为例。设 Go2 底盘质量约 12 kg（取某无电池 URDF 模型值；官方整机约 15 kg），CoM 在几何中心。ARX L5 Pro 含夹爪 ~2.5 kg，臂长 45 cm。当臂完全水平伸出时：

关键：mount offset 不能直接相加到整体 CoM 偏移，它同样要按机械臂质量占总质量加权。设臂安装在底盘前方 x_mount = 0.15 m，臂自身 CoM 在臂坐标系约 l_arm/2 = 0.225 m 处，则臂整体 CoM 在 x_mount + l_arm/2 = 0.375 m。整体 CoM 偏移为：

\[\Delta x_{\text{CoM}} = \frac{m_{\text{arm}} \cdot (x_{\text{mount}} + l_{\text{arm}}/2)}{m_{\text{base}} + m_{\text{arm}}} = \frac{2.5 \times 0.375}{14.5} \approx 6.5 \text{ cm}\]

（若按 Unitree 官方 Go2 整机约 15 kg 计算，分母用 17.5，则约 \(2.5 \times 0.375 / 17.5 \approx 5.4\) cm。）这与前文"约 5-8 cm"的量级一致。Go2 的前脚支撑线到底盘中心约 20 cm——CoM 偏移约 6 cm 仍在支撑多边形内，但留给行走（swing phase 只有 3 脚着地、支撑多边形缩小）和动态挥臂的余量已经明显减少。注意这是静态估算，真实 CoM 还取决于臂各 link 的质量分布、底盘姿态和负载。

如果臂携带了额外负载（如抓起 0.5 kg 的物体），把负载并入臂端再加权，CoM 偏移会进一步增大，余量进一步压缩。策略必须学会用前腿更大的步幅和后腿的蹬力来补偿，或者让底盘微微后仰来平衡 CoM。

工程解决方案：

方案	实现	优点	缺点
Stability reward	惩罚 base pitch/roll 超过阈值	简单直接	限制了机械臂的工作范围
CoM tracking	在 obs 中包含 CoM 位置，reward 鼓励 CoM 在支撑多边形中心	物理正确	需要实时计算 CoM
全身联合训练	不分离底盘和臂，端到端训练整体策略	自动学会补偿	训练更难收敛
分层控制	低层策略跟踪 base + EE 目标，高层策略规划目标	模块化	两层之间可能有信息损失

VBC 使用"分层控制"，ETH 羽毛球使用"全身联合训练"。两者各有优劣——分层更容易训练（每层的问题更简单），全身联合训练性能上限更高（自动学会复杂的底盘-臂协调，如 ETH 论文中机器人学会在挥拍时用后腿蹬地来增加挥拍力度）。

耦合三：感知耦合

导航误差传递到操作精度。 如果底盘的位置估计有 ±5cm 误差，那么机械臂看到的"物体在末端坐标系中的位置"也有 ±5cm 误差——即使机械臂的 IK 完美准确。这意味着底盘的导航精度直接决定了操作精度的下限。

对于视觉系统，耦合更严重：安装在底盘上的相机在行走时持续抖动，产生运动模糊和视角变化。如果同一个相机同时服务于导航（看路）和操作（看物体），两个任务对相机姿态的需求可能矛盾——导航需要相机朝前看路，操作需要相机朝下看物体。

ETH 羽毛球论文发现了一个有趣的"主动感知"行为：机器人学会了在追踪羽毛球时主动调整底盘的 pitch 角度（后仰以抬高相机视角），从而保持羽毛球在相机视野内。这种 locomotion 服务于 perception 的协调行为完全是通过端到端训练自动涌现的——研究者没有显式设计这个行为。

本质洞察：移动操作的三重耦合意味着你不能把底盘和臂当作两个独立系统来设计——它们是一个整体。底盘的每一步都影响臂的末端位置，臂的每一个动作都影响底盘的稳定性，感知系统需要同时服务两者。这就是为什么"分别训练然后拼接"的方案在实际中很少工作——你必须在某种程度上联合训练，让策略学会协调。

Loco-Manipulation 方法谱

方法	底盘控制	臂控制	协调方式	代表工作
Sequential	velocity tracking policy（Ch13）	lift cube policy（Ch17）	时序切换（先走后抓）	—
Hierarchical	低层 whole-body tracker	高层 visual planner	低层自动协调	VBC (CoRL'24)
End-to-end	全身策略同时控制所有关节	同上	隐式协调	ETH 羽毛球 (Sci.Rob.'25)
Decoupled + feedback	locomotion policy + 臂 IK	IK 输出补偿底盘偏移	IK 在线补偿	SLIM (2025)

本章重点讲 Hierarchical（VBC, §19.7）和 End-to-end（ETH 羽毛球, §19.8）两种方案——它们是当前学术界的两个主流路径，分别代表了"工程简洁性"和"性能极限"的两端。

⚠️ 常见陷阱

⚠️ 编程陷阱：底盘策略和臂策略使用不同的控制频率

如果底盘 locomotion policy 以 50 Hz 运行，但机械臂 policy 以 20 Hz 运行，两者之间的频率不匹配会导致"底盘已经移动了一步但臂还在执行上一帧的命令"。这在快速运动时会导致末端位置的系统性偏差。正确做法：两者使用相同的控制频率，或者明确设计频率分层（如 VBC 的高层 10Hz + 低层 50Hz）。

💡 概念误区：认为"加一个机械臂就是在 obs 里多几个关节"

机械臂不只是"多几个关节"——它改变了系统的动力学（CoM、角动量）、改变了任务的结构（从纯 locomotion 变成多目标优化）、改变了 reward 的设计哲学（从单目标跟踪变成导航+接近+抓取+稳定的多阶段融合）。把臂当作"多几个维度"来处理是 loco-manipulation 最常见的工程失败根源。

🧠 思维陷阱：认为"端到端一定比分层好"

ETH 羽毛球的端到端训练确实展现了惊人的协调行为。但端到端训练需要精心设计的 reward、大量的训练计算和困难的超参调优。分层方案（如 VBC）虽然性能上限可能更低，但工程复杂度大大降低——低层策略可以从已有的 locomotion policy 热启动，高层策略只需要解决规划问题。对工程项目来说，"能用"比"最优"更重要。

练习

[分析题] Go2 的底盘质量为 12 kg，ARX L5 Pro 臂含夹爪质量为 2.5 kg，臂长 45 cm。当臂完全水平伸出时，计算 CoM 从底盘中心的偏移量。如果 Go2 的支撑多边形宽度为 20 cm（从中心到边缘），CoM 是否仍在支撑多边形内？
[设计题] 设计一个"分离控制"的 loco-manipulation 基线：底盘走到物体附近 → 停下 → 臂抓取。列出这个基线可能失败的 5 个场景。
[跨章综合题] 回顾 Ch17 的 staged reward 和 Ch13 的 velocity tracking reward。如果要设计一个从"走向物体"到"抓取物体"的连续 reward，staged reward 的门控信号应该是什么？

19.2 复合机器人组合建模：MJCF/USD 资产拼接 ⭐⭐

这一节解决什么问题：如何在仿真中把"一个四足底盘"和"一个机械臂"组合成一个可训练的机器人？两个框架的建模方式有什么差异？常见的建模错误如何避免？

动机：为什么需要"组合建模"

Ch13 使用的是完整的四足模型（如 Go2 的 MJCF），Ch17 使用的是完整的机械臂模型（如 YAM 的 MJCF 或 Franka 的 USD）。但 loco-manipulation 需要把两者拼成一个——底盘 + 臂 = 一个新的复合机器人。

这不是简单的"把两个 XML 文件粘贴在一起"——需要处理以下工程问题：

Mount frame：臂安装在底盘的什么位置？朝什么方向？安装点的坐标系是什么？
关节命名冲突：底盘和臂可能都有名为 joint_0 的关节——拼接后必须重命名以避免冲突
惯性参数一致性：臂的质量和惯性必须添加到底盘模型中，否则仿真的 CoM 计算不准确
碰撞几何：臂的碰撞几何不能与底盘的碰撞几何穿透——安装点附近需要特别检查
Actuator 配置：底盘关节和臂关节的 PD gain 范围差异很大（底盘需要大力矩，臂需要精细控制）

MJCF 组合：三种方式

方式一：<include> 直接包含。 最简单的方式——在底盘 MJCF 的 <worldbody> 中用 <include> 引入臂的 MJCF。

<!-- go2_arx.xml：Go2 + ARX L5 Pro 组合 -->
<!-- 注意：MJCF 的运动学树通过 XML body 嵌套表达，body 没有 URDF 风格的 parent 属性；
     要挂到 trunk 上，必须把 mount body 写成 trunk body 的子元素。
     此外被 include 的文件必须是合法的 MJCF 片段（不能把一个完整 <mujoco> 模型塞进 <body> 内）。 -->
<mujoco model="go2_arx">
  <!-- 底盘模型（include 的文件应为合法片段，而非完整 <mujoco> 顶层模型） -->
  <include file="go2_body.xml"/>
  <!-- 上面的 go2_body.xml 中需把臂 mount frame 嵌套在 trunk body 内，例如：
       <body name="trunk">
         ...
         <body name="arm_mount" pos="0.15 0.0 0.12">
           <include file="arx_l5_pro_fragment.xml"/>   片段，非完整模型
         </body>
       </body>
  -->

  <!-- 合并 actuator 列表（position actuator 的阻尼属性是 kv，不是 kd） -->
  <actuator>
    <!-- 底盘 12 个关节的 PD 控制 -->
    <position name="FR_hip" joint="FR_hip_joint" kp="40" kv="1"/>
    <!-- ... 省略其他 11 个底盘关节 ... -->

    <!-- 臂 6 个关节的 PD 控制——注意 kp 远小于底盘 -->
    <position name="arm_joint_1" joint="arx_joint_1" kp="10" kv="0.5"/>
    <!-- ... 省略其他 5 个臂关节 ... -->

    <!-- 夹爪 -->
    <position name="gripper" joint="arx_gripper" kp="5" kv="0.2"/>
  </actuator>
</mujoco>

<include> 的陷阱：(1) <include> 是解析期把文件内容原样插入，被包含文件必须是合法片段——若 arx_l5_pro.xml 是完整 <mujoco> 模型，直接塞进 <body> 内会把顶层 section 插到 body 中导致结构非法。(2) 被包含文件中的所有命名（body、joint、geom）进入同一个命名空间，名称冲突只能通过手动/脚本化加前缀或改用 MuJoCo 3.x 的 attach prefix 解决——<default class="..."> 只给元素补默认属性，不会重命名或加前缀。

方式二：<attach> 框架挂载（MuJoCo 3.x）。 MuJoCo 3.x 的 body/attach 是编译期 meta 元素：子模型需先在 <asset><model name="..." file="..."/></asset> 中定义，attach 的属性是 model、可选 body、必需 prefix——没有 site 定位属性；定位通过把 attach 放在目标 body/frame 下（用带 pos/quat 的 <frame> 或父 body）实现。这是编译期组合/保存后展开，而非"运行时组合"。

<!-- 使用 attach 方式（MuJoCo 3.x / mjlab 推荐） -->
<mujoco>
  <compiler meshdir="meshes/"/>

  <!-- 子模型先在 asset 中声明 -->
  <asset>
    <model name="arx_l5_pro" file="arx_l5_pro.xml"/>
  </asset>

  <include file="go2.xml"/>

  <!-- 把 attach 放在 trunk 下的 frame 中定位 -->
  <worldbody>
    <body name="trunk">
      <frame pos="0.15 0.0 0.12">
        <attach model="arx_l5_pro" body="base_link" prefix="arm_"/>
      </frame>
    </body>
  </worldbody>
</mujoco>

prefix="arm_" 自动解决命名冲突——臂的 joint_1 变成 arm_joint_1，臂的 base_link 变成 arm_base_link。这是 mjlab 推荐的方式。

方式三：代码中动态组合。 不在 XML 中静态组合，而是在 Python 代码中用 MuJoCo 的 API 动态修改模型。这种方式最灵活（可以在运行时改变安装位置），但代码复杂度最高。仅在需要在不同安装配置之间做消融实验时使用。

Isaac Lab 的多 Entity 挂载

Isaac Lab 使用 USD 格式，复合机器人的建模方式与 MJCF 不同：

# Isaac Lab: 复合机器人配置
from isaaclab.assets import ArticulationCfg

go2_arx_cfg = ArticulationCfg(
    # Go2 + ARX 的组合 USD 文件
    # 通常预先在 USD Composer 中组装好
    prim_path="{ENV_REGEX_NS}/Robot",
    spawn=UsdFileCfg(
        usd_path="assets/go2_arx.usd",
        activate_contact_sensors=True,
    ),
    actuators={
        # 底盘关节组——高力矩 PD
        "legs": ImplicitActuatorCfg(
            joint_names_expr=[".*hip_joint", ".*thigh_joint", ".*calf_joint"],
            stiffness=40.0,
            damping=1.0,
        ),
        # 臂关节组——低力矩精细 PD
        "arm": ImplicitActuatorCfg(
            joint_names_expr=["arm_joint_.*"],
            stiffness=10.0,
            damping=0.5,
        ),
        # 夹爪
        "gripper": ImplicitActuatorCfg(
            joint_names_expr=["gripper_.*"],
            stiffness=5.0,
            damping=0.2,
        ),
    },
)

Isaac Lab 的关键差异：Isaac Lab 的 USD 文件通常在 NVIDIA Isaac Sim 的 USD Composer 中预先组装——底盘和臂在 Composer 中图形化对齐、设置 fixed joint 连接、验证碰撞，然后导出为一个完整的 USD 文件。这比 MJCF 的 XML 编辑更直观，但需要 GUI 环境。

awesome-loco-manipulation 提供的预组装 URDF：开源仓库 github.com/aCodeDog/awesome-loco-manipulation 提供了多个预组装的 loco-manipulation URDF：

平台	底盘	臂	总 DOF	许可
Go2-Arx	Unitree Go2	ARX L5 Pro	12+6+1=19	BSD-3
B1-Z1	Unitree B1	Unitree Z1	12+6+1=19	BSD-3
Aliengo-Z1	Unitree Aliengo	Unitree Z1	12+6+1=19	BSD-3

这些 URDF 可以用 MuJoCo 官方 Python API（mujoco.MjModel.from_xml_path(...) 加载 + mujoco.mj_saveLastXML(...) 导出 MJCF；没有 mujoco.mjcf.load() 这个入口）转换为 MJCF，或用 Isaac Lab 的 URDF-to-USD 转换器转换为 USD。预组装模型提供了包含 meshes/inertias/collisions 的起点（仓库 README 勾选了这些项），可作为起点；但这不等同于训练级验证（无自碰撞、惯性正确、sim-to-real 验证），仍需按本节 checklist 验证安装、碰撞和动力学。

从 URDF 到 MJCF 的完整转换流程：

# Step 1: 加载 URDF 并导出为 MJCF（MuJoCo 官方 Python API）
import mujoco

# 加载（from_xml_path 会根据顶层元素 robot/mujoco 自动识别 URDF/MJCF）
model = mujoco.MjModel.from_xml_path("go2_arx.urdf")
# 注意：URDF 的 continuous joint 会被转为 unbounded joint，需手动加回 joint range

# 导出为 MJCF：把刚编译的模型保存为 XML
mujoco.mj_saveLastXML("go2_arx.xml", model)
# （没有 `python -m mujoco.tool convert` 这个官方命令；
#   C/C++ 侧也可用官方 compile code sample 做 URDF→MJCF。）

# Step 2: 后处理——修复常见的转换问题
# 2a: 添加关节限位（URDF continuous joint → MuJoCo 需要 range）
# 在转换后的 MJCF 中手动添加：
# <joint name="arm_joint_1" range="-3.14 3.14" .../>

# 2b: 设置不同关节组的 PD gain
# 底盘关节——需要大力矩支撑体重
leg_actuators = """
<position name="{name}" joint="{name}"
    kp="{kp}" kd="{kd}"
    ctrlrange="{range_low} {range_high}"/>
"""
# Go2 腿关节的推荐 PD gain：
# hip:   kp=40, kd=1.0
# thigh: kp=40, kd=1.0
# calf:  kp=40, kd=1.0

# 臂关节——需要精细控制
# ARX L5 Pro 的推荐 PD gain：
# joint_1 (base rotation): kp=15, kd=0.5
# joint_2 (shoulder):      kp=15, kd=0.5
# joint_3 (elbow):         kp=10, kd=0.3
# joint_4 (wrist_1):       kp=8,  kd=0.2
# joint_5 (wrist_2):       kp=8,  kd=0.2
# joint_6 (wrist_3):       kp=5,  kd=0.1
# gripper:                 kp=5,  kd=0.1

PD gain 的工程原理：为什么底盘和臂的 kp 差这么大？

底盘关节需要对抗重力（支撑 12+ kg 的体重）和产生行走所需的地面反力——这需要大的力矩（kp 高 → 位置偏差小 → 力矩大）。如果底盘 kp 太小（如 10），机器人会"软趴趴"地无法站立。

臂关节当然需要支撑自身重力和负载——尤其在水平伸展时，远端关节要承受整段臂加负载的重力力矩。只是臂的承载通常小于腿部（臂自重约 2-3 kg），且需要更精确的位置控制，因此需要"恰好够"的力矩与合适的 stiffness/damping，并通常配合重力补偿或积分/前馈项来消除 PD 的重力稳态误差。如果臂 kp 太大（如 40），微小的位置偏差会产生大力矩，导致臂运动过于"硬"和"抖动"。

PD gain 的 sim-to-real 影响：仿真中的 PD gain 不需要与真机电机的 PD gain 完全一致——仿真中的 PD 控制器只是一个"动作到力矩"的映射，真机上的电机控制器有自己的 PD 参数。但两者的比例关系应该相似——如果仿真中臂 kp / 腿 kp = 0.25，真机也应该是类似的比例。

Isaac Lab 的 actuator 分组配置：

Isaac Lab 使用正则表达式匹配来分组配置 actuator——这比 MJCF 的逐个配置更简洁：

# Isaac Lab: 使用正则表达式分组 actuator
actuators={
    "legs": ImplicitActuatorCfg(
        joint_names_expr=[
            "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint",
            "FL_hip_joint", "FL_thigh_joint", "FL_calf_joint",
            "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint",
            "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint",
        ],
        stiffness=40.0,
        damping=1.0,
        # 注意：该值应来自具体仿真模型/URDF，不是官方电机峰值——Unitree 官方 Go2 膝关节峰值约 45 N.m。
        # 另外 Isaac Lab 2.x/3.x 推荐用 effort_limit_sim 设置 solver 限制（effort_limit 对 implicit actuator 是兼容/弃用别名）。
        effort_limit_sim=33.5,  # 以你所用 URDF 的实际力矩为准
    ),
    "arm": ImplicitActuatorCfg(
        joint_names_expr=["arm_joint_.*"],
        stiffness={
            "arm_joint_[1-3]": 15.0,  # 近端大关节
            "arm_joint_[4-6]": 8.0,   # 远端小关节
        },
        damping={
            "arm_joint_[1-3]": 0.5,
            "arm_joint_[4-6]": 0.2,
        },
        effort_limit={
            "arm_joint_[1-3]": 10.0,
            "arm_joint_[4-6]": 5.0,
        },
    ),
    "gripper": ImplicitActuatorCfg(
        joint_names_expr=["gripper_.*"],
        stiffness=5.0,
        damping=0.1,
        effort_limit=2.0,
    ),
}

注意 Isaac Lab 支持 per-joint 的 stiffness/damping/effort_limit（通过字典匹配）——这比 MJCF 的全局 <default> 更灵活。effort_limit 限制了电机的最大力矩输出——这与 ETH 羽毛球的 constrained RL 思想一致，在仿真层面就防止了超出电机能力的动作。

从零组装 vs 使用预组装模型

方面	从零组装	使用预组装（awesome-loco-manipulation）
安装位置	需要自己确定	已验证
惯性参数	需要重新计算	已包含在 URDF 中
碰撞几何	需要手动检查自碰撞	已检查
关节命名	可以自定义	遵循原厂命名规范
灵活性	高（可以尝试不同安装位置）	低（固定安装配置）
推荐场景	研究新的安装配置	快速开始标准平台训练

推荐路径：先用预组装模型跑通整个训练管线（Phase 0-3），确认管线正确后，再根据需要修改安装位置或更换臂型号。

Mount Frame 的工程细节

安装位置的选择：臂通常安装在底盘的"背部前方"——前到足够接近前脚的工作空间，但不超出底盘支撑多边形的前缘。典型位置：Go2 trunk 的 (0.15, 0, 0.12) m（前方 15cm，高 12cm）。

安装方向：臂的 z 轴通常朝上（与世界 z 轴一致），base_link 的 x 轴朝前（与底盘前进方向一致）。如果安装方向旋转了（如臂侧装），所有关于末端工作空间的假设都会改变——DiffIK 的目标坐标系也需要相应调整。

安装刚度：理想情况下臂和底盘之间是 fixed joint（刚性连接）。但如果你想模拟现实中安装件的柔性（如 3D 打印的安装板），可以使用 ball joint + 高刚度弹簧来近似。

组合建模的验证清单

复合模型验证 Checklist（10 分钟）：
  □ 可视化：viser/meshcat 中模型正确显示，臂安装在底盘上方
  □ 自由度：验证 actuated joints 应看 model.nu、actuator/joint 名称列表；
            注意浮动基座 free joint 会让 nq/nv 各含 7/6 个基座广义坐标/速度，
            所以 nq 通常 ≠ 19（19 actuated joints 时，nq ≈ 19_hinge + 7_free）
  □ 关节命名：打印所有关节名称，确认无冲突且有清晰的前缀
  □ 零态：zero agent 下底盘保持站立、臂保持默认姿态
  □ CoM：打印 model.body_mass 的加权平均位置，确认在支撑多边形内
  □ 碰撞：运行 10 步 random agent，检查臂和底盘之间无自碰撞
  □ 执行器：分别给底盘和臂发送小命令，确认正确的关节在动
  □ 惯性：对比组合模型的总质量与单独模型质量之和

⚠️ 常见陷阱

⚠️ 编程陷阱：URDF→MJCF 转换时丢失 fixed joint

MuJoCo 在导入 URDF 时会将 fixed joint 合并到父 body 中（因为 MuJoCo 不支持 0-DOF joint）。如果臂通过 fixed joint 连接到底盘，转换后臂的 base body 会被合并到底盘的 trunk body——这通常是正确的，但如果你在代码中通过 body 名称引用臂的 base body，会找不到它。自检方法：转换后打印 model.body_names 确认所有预期的 body 都存在。

⚠️ 编程陷阱：底盘和臂的 PD gain 使用相同值

底盘关节（髋、膝）需要大力矩（kp=30-80）来支撑体重和行走，臂关节需要小力矩（kp=5-20）来实现精细控制。如果臂关节使用底盘的 kp 值，臂的运动会非常"硬"——微小的位置误差产生巨大的力矩，导致臂震荡。反之，如果底盘关节使用臂的 kp 值，底盘会"软"得站不住。

💡 概念误区：认为"用 MuJoCo Menagerie 的模型就不需要验证"

Menagerie 的模型经过精心调校，但它们是独立的底盘模型或独立的臂模型。组合后的惯性分布、碰撞几何和执行器配置需要重新验证。Menagerie 不提供预组装的 loco-manipulation 模型——awesome-loco-manipulation 填补了这个空白。

练习

[工程题] 从 awesome-loco-manipulation 下载 Go2-Arx URDF，用 mujoco.MjModel.from_xml_path(...) 加载、mujoco.mj_saveLastXML(...) 导出为 MJCF。打印关节名称列表，确认底盘和臂的关节都正确导入。
[验证题] 在 mjlab 中加载转换后的模型，运行 zero agent 和 random agent 各 100 步。保存一帧截图确认模型正确显示。random agent 是否触发了自碰撞？
[设计题] 如果你要把机械臂安装在底盘的侧面（而非背部），mount frame 的 pos 和 rot 应该怎么设置？画出坐标系示意图。

19.3 混合动作空间：量纲统一与梯度平衡 ⭐⭐⭐

这一节解决什么问题：Loco-manipulation 的动作空间包含多种物理量——底盘速度（m/s）、臂关节角度（rad）、末端增量（m）、夹爪开合（binary）。这些量的数值范围和物理意义完全不同。如何设计一个统一的动作空间使 PPO 能有效优化所有维度？

动机：为什么动作空间的设计比 locomotion 和 manipulation 更难

在 Ch13 的四足 locomotion 中，动作空间很均匀——12 个关节角度，全是 rad 单位，数值范围相似（±0.5 rad）。在 Ch17 的操作中，动作空间也较均匀——7 个臂关节角度 + 1 个夹爪。但 loco-manipulation 的动作空间是异构的：

动作维度	物理量	典型范围	控制目标
底盘 vx	线速度 (m/s)	[-1.0, 1.0]	前后移动
底盘 vy	线速度 (m/s)	[-0.5, 0.5]	左右移动
底盘 ωz	角速度 (rad/s)	[-1.5, 1.5]	原地转向
腿关节 ×12	角度偏移 (rad)	[-0.3, 0.3]	步态控制
臂关节 ×6	角度偏移 (rad)	[-0.15, 0.15]	精细操作
夹爪 ×1	开合度	[0, 1]	二值抓取

注意臂关节的 action scale（±0.15 rad）远小于腿关节（±0.3 rad）——因为操作需要更精细的控制。如果两者使用相同的 action scale，臂的每一步变化量太大，无法精确定位。

三种动作空间设计模式

模式一：统一关节空间（End-to-end）。 策略输出所有关节的位置偏移——底盘 12 + 臂 6 + 夹爪 1 = 19 维。每个维度有独立的 action scale。

# 模式一：统一关节空间
action_cfg = JointPositionActionCfg(
    asset_name="robot",
    joint_names=[".*"],  # 所有关节
    scale={
        ".*hip_joint": 0.25,        # 底盘髋关节
        ".*thigh_joint": 0.25,      # 底盘大腿关节
        ".*calf_joint": 0.25,       # 底盘小腿关节
        "arm_joint_[1-3]": 0.15,    # 臂近端关节
        "arm_joint_[4-6]": 0.10,    # 臂远端关节（更精细）
        "gripper": 0.5,             # 夹爪
    },
    offset=default_joint_pos,  # 从默认站立姿态偏移
)

这是 ETH 羽毛球使用的模式——策略直接控制所有关节，没有层级分离。优势：策略有最大的灵活性来协调底盘和臂。劣势：动作空间维度高（19D），训练更难收敛。

模式二：分层动作空间（Hierarchical）。 策略分两层：高层输出 base velocity command + EE target pose（6D 或 7D），低层策略接收高层命令并输出实际关节角度。

# 模式二：分层动作空间
# 高层策略输出（10 Hz）
high_level_action = {
    "base_vx": [-1.0, 1.0],       # m/s
    "base_vy": [-0.5, 0.5],       # m/s
    "base_wz": [-1.5, 1.5],       # rad/s
    "ee_dx": [-0.05, 0.05],       # m, 末端 x 增量
    "ee_dy": [-0.05, 0.05],       # m
    "ee_dz": [-0.05, 0.05],       # m
    "gripper": [0, 1],            # 开合
}
# 低层策略（50 Hz）：接收高层命令 → 输出 19 个关节角度

这是 VBC 使用的模式。优势：高层动作空间低维（7D），训练容易收敛；低层可以从 locomotion policy 热启动。劣势：两层之间的信息瓶颈可能限制最终性能。

模式三：混合模式。 底盘用 velocity command（高层），臂用关节角度（直接），两者拼接。

# 模式三：混合动作空间
action = concat(
    base_velocity_command,    # [vx, vy, wz] 3D
    arm_joint_position,       # 6D (rad)
    gripper,                  # 1D
)  # 总维度 = 10D

Action Scale 归一化：使梯度量级一致

为什么需要 action scale？ PPO 的策略网络输出的是标准正态分布的样本（均值约 0，方差约 1）。不同动作维度需要映射到不同的物理范围。如果底盘 vx 的物理范围是 [-1.0, 1.0] m/s 而臂 joint_1 的物理范围是 [-0.15, 0.15] rad，但两者都接收同样的策略输出范围 [-1, 1]：

\[a_{\text{physical}} = a_{\text{network}} \times \text{scale}\]

那么 vx 的 scale = 1.0，arm_joint_1 的 scale = 0.15。这意味着在策略网络中，arm_joint_1 对 reward 的梯度被 scale 缩小了 6.7 倍——PPO 在更新时会"忽视"臂的关节而过度关注底盘。

量纲归一化的工程方法：

# 归一化使所有维度的"一步变化量"物理意义相当
# 目标：策略输出 ±1 对应的物理变化量应在"合理范围"内

action_scales = {
    # 底盘：±1 对应 ±1 m/s
    "base_vx": 1.0,
    "base_vy": 0.5,
    "base_wz": 1.0,

    # 腿关节：±1 对应 ±0.25 rad（~15°）
    "leg_joints": 0.25,

    # 臂关节：±1 对应 ±0.1 rad（~6°）
    # 注意：比腿更小，因为操作需要精细控制
    "arm_joints": 0.1,

    # 夹爪：±1 对应完全开/合
    "gripper": 1.0,
}

如果 action scale 设置不当会怎样？两种典型失败：

臂 scale 太大（如 0.5 rad）：臂的运动过于粗暴，无法精确定位到物体——表现为"挥动臂但从不碰到物体"。
底盘 scale 太小（如 0.1 m/s）：底盘移动太慢，在 episode 超时前走不到物体附近——表现为"臂在努力伸但底盘不配合移动"。

调参经验法则：先独立确认底盘和臂各自的 action scale 合理（底盘：Ch13 的默认值；臂：Ch17 的默认值），然后在组合训练中微调——通常需要将臂的 scale 减小到独立训练时的 50-70%（因为组合训练中策略需要同时优化多个目标，每个目标的动作变化量应更保守）。

完整的混合动作空间实现（mjlab）：

# mjlab: 完整的 Loco-Manipulation 动作空间配置
class LocoManipActionsCfg:
    """Go2-Arx 的混合动作空间配置。

    三种动作分组：
    1. 底盘腿关节：JointPositionAction（从默认位姿偏移）
    2. 臂关节：JointPositionAction（从默认位姿偏移）
    3. 夹爪：BinaryGripperAction（连续输出做阈值化）

    各组使用不同的 scale——底盘大步幅、臂精细、夹爪二值。
    """

    # 底盘腿关节——与 Ch13 完全相同的配置
    legs = JointPositionActionCfg(
        asset_name="robot",
        joint_names=[
            "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint",
            "FL_hip_joint", "FL_thigh_joint", "FL_calf_joint",
            "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint",
            "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint",
        ],
        scale=0.25,            # ±0.25 rad 偏移（~15°）
        use_default_offset=True,
    )

    # 臂关节——比底盘更精细
    arm = JointPositionActionCfg(
        asset_name="robot",
        joint_names=[
            "arm_joint_1", "arm_joint_2", "arm_joint_3",
            "arm_joint_4", "arm_joint_5", "arm_joint_6",
        ],
        scale={
            "arm_joint_[1-3]": 0.15,  # 近端关节 ~9°
            "arm_joint_[4-6]": 0.10,  # 远端关节 ~6°
        },
        use_default_offset=True,
    )

    # 夹爪——二值控制（Isaac Lab 用 BinaryJointPositionActionCfg，
    #   字段是 open_command_expr / close_command_expr；阈值版用 AbsBinaryJointPositionActionCfg 才有 threshold）
    gripper = mdp.BinaryJointPositionActionCfg(
        asset_name="robot",
        joint_names=["gripper_left", "gripper_right"],
        open_command_expr={"gripper_left": 0.04, "gripper_right": 0.04},   # 打开关节角
        close_command_expr={"gripper_left": 0.0, "gripper_right": 0.0},    # 闭合关节角
    )

梯度平衡的数学分析：为什么 action scale 影响梯度？

设策略网络输出 \(a_{\text{net}} \in \mathbb{R}^{19}\)，物理动作 \(a_{\text{phys}} = s \odot a_{\text{net}}\)（\(s\) 是 scale 向量），reward \(R(a_{\text{phys}})\)。则：

\[\frac{\partial R}{\partial a_{\text{net},i}} = s_i \cdot \frac{\partial R}{\partial a_{\text{phys},i}}\]

如果 \(s_{\text{leg}} = 0.25\) 而 \(s_{\text{arm}} = 0.1\)，即使 \(\partial R / \partial a_{\text{phys}}\) 对底盘和臂同样大，策略网络接收到的梯度中底盘维度是臂维度的 2.5 倍。PPO 的自然梯度（natural gradient）部分缓解了这个问题（通过 Fisher 信息矩阵归一化），但在实践中 scale 差异超过 3-5 倍时仍会导致训练失衡。

实践验证：用以下代码在训练中监控各维度的梯度范数，确认平衡：

# 监控各动作维度的策略梯度范数
def monitor_action_gradients(policy, loss):
    """打印每组动作维度的梯度范数。

    如果 arm 梯度远小于 leg 梯度，
    说明 arm scale 太小或 arm reward 太弱。
    """
    loss.backward(retain_graph=True)

    # 获取策略输出层的梯度
    output_grad = policy.actor[-1].weight.grad  # [19, hidden_dim]

    leg_grad_norm = output_grad[:12].norm().item()
    arm_grad_norm = output_grad[12:18].norm().item()
    gripper_grad_norm = output_grad[18:].norm().item()

    print(f"Gradient norms — leg: {leg_grad_norm:.4f}, "
          f"arm: {arm_grad_norm:.4f}, "
          f"gripper: {gripper_grad_norm:.4f}")
    # 目标：arm / leg 比值在 0.3-1.0 范围内
    # 如果 < 0.1：增大 arm scale 或 arm reward weight
    # 如果 > 3.0：减小 arm scale 或 arm reward weight

如果不同动作组使用不同的 MLP 分支（不共享参数）：一种更激进的方案是为底盘和臂使用独立的 MLP 分支——底盘分支接收 base obs，臂分支接收 arm obs + object obs。两个分支的梯度完全独立，不会互相干扰。代价是参数量翻倍，且两者之间缺乏信息共享（底盘不知道臂在做什么）。VBC 的分层架构本质上就是这种"分支"模式的极端版本——两层完全独立的策略。

⚠️ 常见陷阱

⚠️ 编程陷阱：混合动作空间的关节顺序不一致

底盘关节和臂关节在 MJCF/USD 中的排列顺序可能与你在策略网络中假设的顺序不同。例如，MJCF 可能把所有底盘关节排在前面，臂关节排在后面——但如果你的 action_scale 向量假设了不同的排列，底盘的 scale 会被应用到臂上（或反过来）。自检方法：打印 env.action_space 的关节名称列表，确认与 action_scale 的顺序一致。

⚠️ 编程陷阱：夹爪动作的连续/离散混淆

真实夹爪通常是"开/合"二值控制。但如果 action 空间是连续的（PPO 输出正态分布），夹爪维度会输出 0-1 之间的连续值。需要在 env.step() 中加一个阈值化：gripper_cmd = 1.0 if raw_action > 0 else 0.0。如果不做阈值化，夹爪会一直保持"半开"状态——永远抓不住物体。

练习

[计算题] 一个 19-DOF 的 loco-manipulation 策略网络（MLP [256, 256, 128]）有多少参数？与 12-DOF 的纯 locomotion 策略相比增加了多少百分比？
[设计题] 为 Go2-Arx 设计一个分层动作空间。高层输出什么？低层输出什么？两层的控制频率分别是多少？
[实验题] 训练两个 reach 策略：一个使用 arm_scale=0.1，另一个使用 arm_scale=0.3。在 500 iteration 后比较末端到达精度。哪个更好？为什么？

19.4 Observation 设计：多源状态融合 ⭐⭐

这一节解决什么问题：Loco-manipulation 的 observation 需要包含底盘状态、臂状态和物体状态。如何设计 obs group？不同状态的表示方式（绝对 vs 相对）如何选择？

三类状态的组成

Loco-manipulation 的 actor observation 由三个部分组成（加上可选的视觉输入）：

Part 1：底盘本体感知（Proprioception）

与 Ch13 完全相同——关节位置、关节速度、base 角速度、projected gravity、commands。

# 底盘本体感知
base_proprio = {
    "joint_pos": 12,          # 腿关节角度 (rad)
    "joint_vel": 12,          # 腿关节角速度 (rad/s)
    "base_ang_vel": 3,        # IMU 角速度 (rad/s)
    "projected_gravity": 3,   # 投影重力方向
    "velocity_commands": 3,   # [vx_cmd, vy_cmd, wz_cmd]
    "last_action_legs": 12,   # 上一步的腿动作
}
# 小计：45 维

Part 2：臂本体感知

# 臂本体感知
arm_proprio = {
    "arm_joint_pos": 6,       # 臂关节角度 (rad)
    "arm_joint_vel": 6,       # 臂关节角速度 (rad/s)
    "ee_pos_base": 3,         # 末端位置（base 坐标系）
    "ee_quat_base": 4,        # 末端姿态（base 坐标系，quaternion）
    "gripper_state": 1,       # 夹爪开合度
    "last_action_arm": 7,     # 上一步的臂+夹爪动作
}
# 小计：27 维

Part 3：物体状态（任务相关）

# 物体状态——关键设计决策：绝对 vs 相对
object_state_absolute = {
    "object_pos_world": 3,    # 物体世界坐标
    "object_quat_world": 4,   # 物体世界姿态
}
# 或者
object_state_relative = {
    "object_pos_ee": 3,       # 物体相对于末端的位置
    "object_pos_base": 3,     # 物体相对于底盘的位置
    "object_quat_ee": 4,      # 物体相对于末端的姿态
}

绝对坐标 vs 相对坐标：关键设计决策

为什么相对坐标通常更好？ 考虑一个任务：底盘从 (0, 0) 走到 (2, 0) 处的物体，然后用臂抓取。如果 obs 使用世界坐标系的物体位置 (2, 0, 0.3)，策略需要学会"当 object_x = 2 且 base_x < 2 时向前走，当 base_x ≈ 2 时伸臂"。但如果物体在 (5, 3, 0.3)，策略看到的是完全不同的数字——它需要重新学习"当 object_x = 5 时怎么走"。

如果使用相对坐标 object_pos_base = object_pos_world - base_pos，无论物体在哪里，策略看到的都是"物体在我前方 2m"——同样的数值映射到同样的行为（向前走 2m）。这就是平移不变性——策略不需要为每个绝对位置分别学习。

VBC 的 observation 设计（从论文提取）：

# VBC 低层策略的 observation
vbc_low_level_obs = {
    # 本体感知
    "joint_pos": 18,           # 12 腿 + 6 臂
    "joint_vel": 18,
    "base_ang_vel": 3,
    "projected_gravity": 3,

    # 目标跟踪（相对坐标！）
    "ee_target_pos_base": 3,   # EE 目标位置（base 坐标系）
    "ee_target_quat_base": 4,  # EE 目标姿态（base 坐标系）
    "base_vel_cmd": 3,         # 底盘速度命令

    # 历史
    "last_action": 19,
}
# 总计：71 维

注意 VBC 的低层策略不看物体位置——它只看 EE 目标（由高层策略给出）。这是分层架构的核心思想：低层不需要知道"物体在哪"，只需要知道"末端应该去哪"。物体定位由高层视觉策略完成。

末端位姿的表示选择

表示	维度	连续性	工程难度	推荐场景
Quaternion (w,x,y,z)	4	有符号歧义（q 和 -q 同义）	需要归一化	大范围旋转
Rotation Matrix	9	连续，无歧义	需要正交化	数值稳定性要求高
Euler Angles (rpy)	3	有万向锁（gimbal lock）	最简单	小范围旋转
6D Rotation	6	连续，无歧义	需要额外投影	学习旋转的最新推荐

对 loco-manipulation 的 EE 目标姿态，推荐使用 quaternion——因为操作任务通常涉及大范围旋转（如从上方接近到侧方抓取），Euler angles 的万向锁是致命的。quaternion 的符号歧义可以通过"确保 w > 0"的约定来消除（如果 w < 0，取 -q）。

Quaternion 处理的完整工程代码：

import torch

def normalize_quat(q):
    """归一化四元数，确保 ||q|| = 1。

    为什么需要这一步？MLP 输出的"四元数"不保证单位范数，
    如果直接用于旋转计算会产生缩放伪影。
    """
    return q / (q.norm(dim=-1, keepdim=True) + 1e-8)

def standardize_quat(q):
    """确保 w > 0（消除符号歧义）。

    四元数 q 和 -q 表示同一个旋转——
    如果 obs 中交替出现 q 和 -q，MLP 会困惑。
    统一到 w > 0 消除这个问题。
    """
    return torch.where(q[..., 0:1] < 0, -q, q)

def quat_distance(q1, q2):
    """计算两个四元数之间的旋转角度差（rad）。

    用于 EE orientation tracking reward 的计算。
    """
    q1 = normalize_quat(standardize_quat(q1))
    q2 = normalize_quat(standardize_quat(q2))
    dot = (q1 * q2).sum(dim=-1).clamp(-1.0, 1.0)
    return 2.0 * torch.acos(dot.abs())  # abs 处理 q/-q 歧义

def compute_relative_pose(pos_world, quat_world, base_pos, base_quat):
    """将世界坐标系的位姿转换为 base 坐标系的相对位姿。

    这是 obs 中使用相对坐标的核心函数。

    Args:
        pos_world: [B, 3] 目标在世界坐标系的位置
        quat_world: [B, 4] 目标在世界坐标系的姿态 (w,x,y,z)
        base_pos: [B, 3] 底盘在世界坐标系的位置
        base_quat: [B, 4] 底盘在世界坐标系的姿态
    Returns:
        pos_base: [B, 3] 目标相对于底盘的位置
        quat_base: [B, 4] 目标相对于底盘的姿态
    """
    # 位置：世界位置差 → 旋转到 base 坐标系
    delta_pos = pos_world - base_pos
    base_rot_inv = quat_conjugate(base_quat)
    pos_base = quat_rotate(base_rot_inv, delta_pos)

    # 姿态：相对旋转
    quat_base = quat_multiply(base_rot_inv, quat_world)
    quat_base = standardize_quat(quat_base)

    return pos_base, quat_base

def quat_conjugate(q):
    """四元数共轭（等价于旋转的逆）。"""
    return torch.cat([q[..., 0:1], -q[..., 1:4]], dim=-1)

def quat_multiply(q1, q2):
    """四元数乘法。"""
    w1, x1, y1, z1 = q1.unbind(-1)
    w2, x2, y2, z2 = q2.unbind(-1)
    return torch.stack([
        w1*w2 - x1*x2 - y1*y2 - z1*z2,
        w1*x2 + x1*w2 + y1*z2 - z1*y2,
        w1*y2 - x1*z2 + y1*w2 + z1*x2,
        w1*z2 + x1*y2 - y1*x2 + z1*w2,
    ], dim=-1)

def quat_rotate(q, v):
    """用四元数旋转向量。"""
    q_v = torch.cat([torch.zeros_like(v[..., :1]), v], dim=-1)
    q_conj = quat_conjugate(q)
    result = quat_multiply(quat_multiply(q, q_v), q_conj)
    return result[..., 1:4]

Observation 归一化的工程细节：

不同 obs 项的数值范围差异很大——关节角度在 [-2, 2] rad，关节速度在 [-10, 10] rad/s，物体位置在 [-3, 3] m。如果不做归一化，MLP 的输入值域不一致，训练效率降低。

两种归一化策略：

用一个类比来理解 obs 归一化的重要性：想象你在一个混合了厘米和千米的坐标系中导航——"前方 50 厘米有一个台阶"和"前方 0.5 千米有一个建筑"在数值上分别是 50 和 0.5——但前者对即时决策更重要。如果 MLP 看到的原始数字是 50 vs 0.5，它会认为 50 的那个维度"更重要"（因为梯度更大）。归一化就是把所有维度放到同一个"度量衡"下——让 MLP 公平对待每一个输入。

Per-term scaling（推荐）：在 ObservationGroupCfg 中为每个 term 设置 scale 参数。

# mjlab: 带 scale 的 observation 配置
observations = ObservationsCfg(
    policy=ObservationGroupCfg(
        terms={
            "joint_pos": JointPositionObsCfg(scale=1.0),       # 值域 ~[-2,2]
            "joint_vel": JointVelocityObsCfg(scale=0.05),      # 值域 ~[-10,10] → ×0.05 → [-0.5,0.5]
            "base_ang_vel": BaseAngularVelocityObsCfg(scale=0.25),  # 值域 ~[-4,4] → ×0.25 → [-1,1]
            "object_pos_base": RelativePositionObsCfg(scale=0.5),   # 值域 ~[-3,3] → ×0.5 → [-1.5,1.5]
            "object_quat_base": RelativeQuaternionObsCfg(scale=1.0),  # 值域 [-1,1]，无需缩放
        },
    ),
)

Running/经验归一化（RSL-RL 内置）：注意 RSL-RL/Isaac Lab 用的不是 Gym/SB3 的 VecNormalize wrapper，而是 RSL-RL 的 EmpiricalNormalization 模块（Isaac Lab 侧通过 empirical_normalization 或 actor/critic obs normalization 配置暴露，且该字段随版本变化、部分版本已弃用，应以当前 isaaclab_rl.rsl_rl cfg 为准）。它在线累计 obs 的 mean/std 做归一化。优势：不需要手动设定 scale。劣势：训练初期 mean/std 估计不准，可能引入不稳定。对 loco-manipulation 推荐使用 per-term scaling（更可控）或明确配置经验归一化。

Critic Observation 的 Privileged 信息

与 Ch17 类似，asymmetric actor-critic 的 critic 可以使用额外的 privileged 信息：

Privileged Term	维度	来源	Actor 不能用的原因
object_velocity	6	仿真器	真实场景中难以实时估计
true_friction	1	DR 参数	未知地面的摩擦
true_arm_mass	1	DR 参数	负载质量未知
contact_force_gripper	6	仿真器	真实夹爪可能没有力传感器
terrain_height_scan	~200	raycast	只在仿真中可用

ETH 羽毛球的 asymmetric critic：teacher（critic）看到精确的羽毛球 3D 位置和速度；actor 只看到带噪声的相机检测。这使得 critic 能准确估计值函数，为 actor 提供更好的 advantage 估计——即使 actor 面对的是噪声很大的感知信息。

⚠️ 常见陷阱

⚠️ 编程陷阱：物体位置使用世界坐标而非相对坐标

使用 object_pos_world 而非 object_pos_base 作为 obs 输入——策略会学到"物体在 (2,0,0.3) 时向前走"但无法泛化到物体在 (5,3,0.3) 的情况。解决方案：始终使用相对于 base 或 EE 的坐标。

💡 概念误区：认为"obs 维度越高策略越好"

把所有可获得的状态都塞进 obs 不一定有帮助——噪声和冗余信息会增加 MLP 的学习难度。例如，同时包含 ee_pos_base 和 ee_pos_world + base_pos_world 提供的信息是冗余的（前者等于后者的差），但后者增加了 6 维无用的绝对位置信息。只包含"最小充分"的 observation——策略需要什么信息就给什么，不多给。

练习

[设计题] 为 Go2-Arx 的抓取任务设计完整的 actor 和 critic observation group。列出每个 term 的名称、维度和坐标系。actor 和 critic 的 obs 有什么区别？
[实验题] 训练两个策略：一个用绝对坐标 object_pos_world，一个用相对坐标 object_pos_base。在 2000 iteration 后比较两者在随机初始位置上的抓取成功率。
[分析题] VBC 的低层策略为什么不需要看到物体位置？如果低层策略也看到物体位置，会发生什么？

19.5 多目标 Reward 融合：从导航到抓取的连续奖励 ⭐⭐⭐

这一节解决什么问题：Loco-manipulation 任务有多个目标——走到物体附近、伸出臂接近物体、闭合夹爪抓取、保持底盘稳定。如何设计一个统一的 reward function 使策略依次完成这些目标？

动机：Ch17 的 staged reward 在这里如何扩展

回顾 Ch17：操作任务的 staged reward 有两个阶段（reaching → bringing），用乘法门控编码因果顺序。Loco-manipulation 的阶段更多：

操作（Ch17）：reaching → bringing
Loco-manipulation：navigation → reaching → grasping → bringing + stability（全程）

navigation 阶段是 loco-manipulation 独有的——底盘需要先移动到物体附近，这是 Ch13 的 velocity tracking 的一个变体。关键区别是：Ch13 的 velocity command 是外部随机采样的，而 loco-manipulation 的"速度命令"由物体位置隐式决定——"向物体方向移动"。

Reward 的四个组件

组件一：Navigation Reward（底盘→物体）

def navigation_reward(base_pos, object_pos, sigma_nav=0.5):
    """鼓励底盘移动到物体可达范围内。

    sigma_nav 控制"多近算到达"——
    对臂长 45cm 的 ARX，sigma_nav = 0.3-0.5 m 是合理的。
    不需要精确到位，只需要让物体进入臂的工作空间。
    """
    dist_base_to_obj = torch.norm(
        base_pos[:, :2] - object_pos[:, :2], dim=-1  # 只看 xy 平面距离
    )
    return torch.exp(-dist_base_to_obj**2 / sigma_nav**2)

组件二：EE Reaching Reward（末端→物体）

def ee_reaching_reward(ee_pos, object_pos, sigma_reach=0.1):
    """鼓励末端接近物体。

    与 Ch17 的 reaching reward 相同——
    但这里是在底盘移动的过程中同时追踪。
    sigma_reach 应该比 sigma_nav 小——
    末端需要比底盘更精确地定位。
    """
    dist_ee_to_obj = torch.norm(ee_pos - object_pos, dim=-1)
    return torch.exp(-dist_ee_to_obj / sigma_reach)

组件三：Grasp Reward（抓取成功）

def grasp_reward(gripper_state, object_height, table_height, threshold=0.05):
    """判断是否成功抓起物体。

    成功条件：夹爪闭合 AND 物体高度 > 桌面 + threshold
    """
    gripper_closed = gripper_state < 0.1  # 夹爪几乎闭合
    object_lifted = object_height > table_height + threshold
    return (gripper_closed & object_lifted).float()

组件四：Stability Regularization（防翻倒）

def stability_reward(base_pitch, base_roll, pitch_limit=0.3, roll_limit=0.3):
    """惩罚底盘过度倾斜——防止伸臂时翻倒。

    pitch_limit 和 roll_limit 应该比纯 locomotion 更严格——
    因为 CoM 偏移让翻倒风险更高。
    """
    pitch_penalty = torch.clamp(torch.abs(base_pitch) - pitch_limit, min=0)
    roll_penalty = torch.clamp(torch.abs(base_roll) - roll_limit, min=0)
    return -(pitch_penalty + roll_penalty) * 10.0

多目标融合：加权组合 vs 乘法门控

加权组合：各 reward 独立计算，乘以权重后相加。

\[r_{\text{total}} = w_{\text{nav}} \cdot r_{\text{nav}} + w_{\text{reach}} \cdot r_{\text{reach}} + w_{\text{grasp}} \cdot r_{\text{grasp}} + w_{\text{stab}} \cdot r_{\text{stab}}\]

乘法门控（Ch17 模式的扩展）：

\[r_{\text{total}} = r_{\text{nav}} \cdot r_{\text{reach}} + r_{\text{grasp}} + w_{\text{stab}} \cdot r_{\text{stab}}\]

乘法门控编码了"先导航后抓取"的因果关系——如果 \(r_{\text{nav}} \approx 0\)（底盘还没到），\(r_{\text{reach}}\) 的贡献被屏蔽，策略不会浪费时间在远处伸臂。

用一个做菜的类比来理解乘法门控：你不会在锅还没热的时候就放食材（导航没完成就开始操作）。乘法门控就像"锅热了才开始"的条件——\(r_{\text{nav}}\) 是"锅的温度"，只有它足够高时 \(r_{\text{reach}}\) 的"烹饪 reward"才有意义。如果用加法组合（不门控），相当于"一边加热一边放食材"——表面上节省了时间，但做出来的菜可能不熟。在 loco-manipulation 中，"不熟"就是"底盘还在远处但臂已经在乱伸"——既浪费了臂的运动（可能碰到障碍物），也没有导航到位的效果。

推荐方案：对大多数 loco-manipulation 任务，混合使用乘法门控（导航→接近）+ 加权组合（稳定性全程生效）：

# 推荐的 reward 融合方案
def compute_total_reward(env_state):
    r_nav = navigation_reward(env_state)
    r_reach = ee_reaching_reward(env_state)
    r_grasp = grasp_reward(env_state)
    r_stab = stability_reward(env_state)
    r_action_rate = action_rate_penalty(env_state)

    # 乘法门控：导航到位后才计算接近 reward
    r_approach = r_nav * r_reach

    # 加权组合
    total = (
        1.0 * r_approach +
        5.0 * r_grasp +          # 抓取成功给大 reward
        0.5 * r_stab +           # 稳定性全程生效
        0.02 * r_action_rate     # 动作平滑
    )
    return total

Reward 权重平衡的经验法则

组件	推荐权重范围	调参方向
r_nav	1.0（基准）	底盘不动→增大，底盘走太快→减小
r_reach	1.0-2.0	臂不伸→增大，臂乱挥→减小
r_grasp	5.0-10.0	不抓→增大（这是终极目标）
r_stab	0.3-1.0	翻倒→增大，臂活动范围太小→减小
r_action_rate	0.01-0.05	震荡→增大
r_alive	0.5-1.0	自杀策略→增大

权重调参的系统方法论：不要盲目猜测权重——使用以下三步流程：

Step 1：确认每个 reward 项的值域。 在训练开始前，用 random agent 跑 100 步，打印每个 reward 项的均值和方差。如果 r_nav 的均值是 0.3 但 r_stab 的均值是 -5.0，两者的梯度量级差 15 倍——PPO 会被 r_stab 主导。

# 打印 reward 各项的值域（训练前诊断）
def diagnose_reward_scales(env, num_steps=100):
    """在 random agent 下统计每个 reward 项的值域。"""
    reward_stats = {name: [] for name in env.reward_names}
    for _ in range(num_steps):
        action = env.action_space.sample()
        _, rewards_dict, _, _ = env.step(action)
        for name, val in rewards_dict.items():
            reward_stats[name].append(val.mean().item())

    print("Reward diagnostics (random agent):")
    for name, vals in reward_stats.items():
        print(f"  {name}: mean={np.mean(vals):.4f}, "
              f"std={np.std(vals):.4f}, "
              f"range=[{np.min(vals):.4f}, {np.max(vals):.4f}]")

Step 2：归一化使各项的加权贡献大致相等。 设 \(r_i\) 的均值为 \(\bar{r}_i\)，权重为 \(w_i\)。调整 \(w_i\) 使 \(w_i \cdot |\bar{r}_i| \approx 1.0\) 对所有 \(i\)。这确保每个 reward 项在总 reward 中的贡献量级相当——PPO 不会因为某个项的贡献太大而忽略其他项。

Step 3：按任务优先级微调比例。 grasp success 是最终目标，所以 r_grasp 的加权贡献应该是最大的（2-5 倍于其他项）。stability 是约束而非目标，所以 r_stab 的加权贡献应该"刚好够用"——太大会限制策略的探索空间。

完整的 mjlab RewardManager 配置：

# mjlab: Loco-Manipulation 的完整 Reward 配置
class LocoManipRewardsCfg:
    """Go2-Arx 移动抓取的 reward 配置。

    设计原则：
    1. 乘法门控编码因果顺序（nav → reach → grasp）
    2. stability 全程生效（不随阶段变化）
    3. stop_during_grasp 在接近物体时激活
    4. action_rate 抑制所有关节的震荡
    """

    # === 导航阶段 ===
    navigation = NavigationRewardCfg(
        func=gaussian_distance_reward,
        params={"sigma": 0.5},
        body_name="base",
        target_name="object",
        coordinate="xy",          # 只看 xy 平面距离
        weight=1.0,
    )

    # === 接近阶段（被 navigation 门控）===
    ee_reaching = EEReachingRewardCfg(
        func=exponential_distance_reward,
        params={"sigma": 0.1},
        ee_name="gripper_center",
        target_name="object",
        gate_by="navigation",     # 乘法门控
        weight=2.0,
    )

    # === 抓取阶段 ===
    grasp_success = GraspSuccessRewardCfg(
        func=binary_grasp_reward,
        params={
            "gripper_threshold": 0.1,
            "lift_threshold": 0.05,
        },
        weight=10.0,
    )

    # === 稳定性（全程） ===
    base_stability = BaseStabilityRewardCfg(
        func=pitch_roll_penalty,
        params={
            "pitch_limit": 0.25,   # rad (~14°)
            "roll_limit": 0.25,
        },
        weight=-2.0,              # 负号表示惩罚
    )

    base_height = BaseHeightRewardCfg(
        func=height_tracking_reward,
        params={"target_height": 0.34, "sigma": 0.05},
        weight=0.5,
    )

    # === 动作平滑（全程） ===
    action_rate_legs = ActionRateRewardCfg(
        joint_names=[".*hip.*", ".*thigh.*", ".*calf.*"],
        weight=-0.01,
    )
    action_rate_arm = ActionRateRewardCfg(
        joint_names=["arm_joint_.*"],
        weight=-0.02,              # 臂的平滑更重要
    )

    # === 操作时底盘冻结 ===
    stop_during_grasp = StopDuringGraspRewardCfg(
        func=conditional_velocity_penalty,
        params={
            "ee_to_obj_threshold": 0.15,  # m
            "velocity_weight": 5.0,
        },
        weight=-1.0,
    )

    # === 存活奖励 ===
    alive = AliveRewardCfg(weight=0.5)

"Reward Up Task Metric Flat"在 Loco-Manipulation 中的表现：

这个 Ch17 的黄金诊断原则在 loco-manipulation 中有一个特别常见的变种——策略学会了"高速冲向物体"获取大的 navigation reward，但到达后不停下也不抓取。这是因为 navigation reward 在每一步都给正信号（越近越大），而 grasp reward 只在抓取成功时给一次信号。如果 grasp 的权重不够大（或者策略还没学会抓取），PPO 会发现"反复冲向物体然后 reset"比"尝试抓取但可能失败"的 expected return 更高。

诊断方法：独立记录 base_to_object_distance 的最小值——如果这个值在 episode 中反复达到很小（如 0.2 m）然后又变大（机器人冲过物体），说明策略在做"冲刺循环"而非"导航-停下-抓取"。解决方案：增大 r_grasp 的权重到 10-20，或加入 stop_during_grasp reward。

操作阶段的底盘冻结 Reward

一个 loco-manipulation 的独有挑战：抓取阶段底盘应该保持静止——如果底盘在夹爪即将闭合时继续移动，末端位置会偏移，抓取失败。

def stop_during_grasp_reward(base_vel, ee_to_object_dist, threshold=0.15):
    """当末端接近物体时，惩罚底盘运动。

    threshold：末端距物体多近时开始要求底盘停止。
    """
    near_object = (ee_to_object_dist < threshold).float()
    base_speed = torch.norm(base_vel[:, :2], dim=-1)
    return -near_object * base_speed * 5.0  # 近物体时强惩罚底盘速度

这个"stop during grasp"reward 编码的工程直觉是接触窗口的速度约束——不是"导航和操作不能同时发生"。与本章主线一致：粗导航与预伸臂可以并行（边走边把臂伸向目标），只有在夹爪闭合/接触丰富的精细抓取窗口才要求底盘低速/稳定。是否需要完全停下取决于任务、物体和控制精度；过强的 stop 约束反而会退化成"先导航-停下-再抓"的 sequential baseline，丧失 loco-manipulation 的协调优势。

本质洞察：Loco-manipulation 的 reward 设计核心不是"怎么让 reward 更大"，而是"怎么编码正确的行为顺序"。乘法门控编码了"先导航后操作"的因果关系，stop reward 编码了"操作时底盘保持静止"的约束。这些不是直觉上显然的——如果只靠 reward shaping 的"添加更多项"，策略很容易学到 shortcut。

⚠️ 常见陷阱

⚠️ 编程陷阱：Navigation reward 和 EE reaching reward 的 sigma 混淆

如果 \(\sigma_{\text{nav}}\) 设得太小（如 0.1 m），底盘必须精确到 10cm 内才能获得 navigation reward——这对四足底盘来说太苛刻了。正确设置：\(\sigma_{\text{nav}} \approx\) 臂工作空间半径的 2/3。

💡 概念误区：认为"grasp reward 应该是稀疏的"

纯稀疏 grasp reward（成功=1, 失败=0）在高维 loco-manipulation 中几乎不可能收敛——因为策略需要先导航到位、再伸臂、再闭合夹爪、再抬起物体，这个序列中的每一步都需要梯度信号。Dense reward（navigation + reaching + grasp 的组合）比稀疏 reward 收敛快几个数量级。

练习

[设计题] 为一个"Go2-Arx 捡起地上的球"任务设计完整的 reward function。列出所有 reward 项、权重和 sigma 参数。
[实验题] 比较两种融合方式：(a) 纯加权组合，(b) 乘法门控 + 加权组合。在 3000 iteration 后比较 grasp success rate。
[分析题] "stop during grasp" reward 的 threshold 应该设为多少？太大和太小分别会导致什么行为？

19.6 Arm-Constrained 四阶段课程训练 ⭐⭐⭐

这一节解决什么问题：直接训练全身 loco-manipulation 策略很难收敛——19 维动作空间 + 多目标 reward 让 PPO 的探索效率极低。四阶段课程（arm-constrained curriculum）如何分解训练难度？

为什么需要课程

Loco-manipulation 的训练难度来自两个方面的"组合爆炸"：

动作空间维度：19D（12 腿 + 6 臂 + 1 夹爪）比纯 locomotion（12D）高 58%。PPO 在高维空间中的探索效率随维度指数下降。
多目标 reward：策略需要同时学会走路（locomotion skill）和抓取（manipulation skill）。如果两个 skill 的 reward 信号相互干扰，策略可能学到"走得好但不抓"或"抓得好但不走"的局部最优。

四阶段课程通过"先学简单的再学复杂的"来分解这个难度——每个阶段只引入一个新的挑战，让策略在已有能力的基础上逐步扩展。

Phase 0：冻结臂，只训练底盘 Locomotion

# Phase 0 配置：臂关节冻结在默认位姿
phase0_cfg = dict(
    arm_frozen=True,                # 臂关节输出被忽略
    arm_default_pos=[0, -0.5, 0.5, 0, 0, 0],  # 收起姿态
    locomotion_reward=True,         # 只开启 velocity tracking reward
    manipulation_reward=False,      # 关闭所有操作相关 reward
    num_envs=4096,
    max_iterations=5000,
)

Phase 0 的目标：让策略学会在"背上有一个固定的额外质量"的条件下稳定行走。这与纯 locomotion 的区别是 CoM 偏移——臂虽然不动，但它的质量（2-3 kg）改变了系统的惯性分布。策略需要适应这个新的动力学。

Phase 0 的验收标准：velocity tracking error < Ch13 的基线 + 20%。如果误差增大超过 20%，说明底盘策略无法适应额外质量——可能是臂安装位置导致 CoM 偏移过大，需要调整 mount frame 或增加 base height reward。

Hot-start 技巧（VBC 的核心工程贡献）：不从零训练 Phase 0——直接加载 Ch13 中已训练好的纯 locomotion policy 作为初始化。这叫"hot-start"——策略已经会走路了，Phase 0 只需要微调以适应额外质量。实测：hot-start 的 Phase 0 在 500-1000 iteration 内就能收敛（vs 从零训练需要 5000+ iteration）。

# Hot-start：从纯 locomotion policy 加载权重
import torch

# 加载 Ch13 训练好的 locomotion policy
loco_policy = torch.load("go2_velocity_tracking.pt")

# 初始化 Phase 0 的策略
# 注意：Go2-Arx 动作维度是 19（12 腿 + 6 臂 + 1 夹爪），
# 但 locomotion policy 只有 12 维输出——
# 对于多出来的 7 个维度（6 臂 + 1 夹爪），用零初始化
phase0_policy = LocoManipPolicy(action_dim=19)
# 复制 locomotion 的前 12 维动作权重
phase0_policy.actor.load_partial(loco_policy.actor, match_prefix="leg_")
# 臂的动作权重保持随机初始化（Phase 0 中被冻结，不影响训练）

Phase 1：冻结底盘，训练臂 Reach

# Phase 1 配置：底盘冻结，只训练臂
phase1_cfg = dict(
    base_frozen=True,               # 底盘不动
    arm_frozen=False,               # 解冻臂
    locomotion_reward=False,        # 关闭 locomotion reward
    manipulation_reward=True,       # 开启 reaching reward
    ee_goal_sampling="sphere",     # EE 目标在以 base 为中心的球面上采样
    goal_sphere_radius=0.35,       # 臂工作空间内
    num_envs=2048,
    max_iterations=3000,
)

Phase 1 的目标：让策略学会用臂关节到达任意 EE 目标位姿——这是 Ch17 的 reach 任务在移动底盘上的翻版。底盘冻结确保策略只需要学习臂的控制（6D 问题），不需要同时学习底盘（12D + 3D velocity）。

EE 目标采样策略：VBC 使用"height-invariant sphere"——目标点均匀采样在以机器人 base 为中心的球面上，但球面的高度范围受限（不会太高也不会太低）。这确保了训练数据覆盖臂的整个可达工作空间。

Phase 1 的验收标准：EE position error < 3 cm，EE orientation error < 10°。如果达不到，检查臂的 PD gain（太低导致精度不足）或 action scale（太大导致过冲）。

Phase 2：全身联合训练 Mobile Reach + Grasp

# Phase 2 配置：全部解冻，联合训练
phase2_cfg = dict(
    base_frozen=False,              # 解冻底盘
    arm_frozen=False,               # 臂已解冻
    locomotion_reward=True,         # 开启 velocity tracking
    manipulation_reward=True,       # 开启 reaching + grasping
    navigation_reward=True,         # 开启导航 reward
    stability_reward=True,          # 开启稳定性正则化
    stop_during_grasp=True,         # 近物体时惩罚底盘运动
    num_envs=2048,
    max_iterations=10000,

    # 关键：从 Phase 0 + Phase 1 的权重初始化
    resume_from="phase1_best.pt",
    # Phase 1 的权重包含了 Phase 0 hot-start 的底盘权重
    # + Phase 1 训练的臂权重
)

Phase 2 的目标：策略学会"走到物体附近 → 伸臂接近 → 闭合夹爪 → 抬起物体"的完整序列。这是最难的阶段——19D 动作空间 + 多目标 reward 同时生效。

Phase 2 的关键工程决策：

决策	推荐选择	替代方案	原因
是否从 Phase 0+1 热启动	✅ 是	从零训练	热启动节省 5000+ iteration
底盘和臂是否共享 MLP	❌ 不共享	共享	不同物理量的梯度方向不同
物体初始位置范围	0.5-2.0 m	0-3.0 m	太远底盘走不到，太近不需要导航
Episode 长度	10-20 s	5-30 s	太短来不及完成全序列，太长浪费无效步

Phase 2 的收敛曲线特征：

0-2000 iter: navigation reward 快速上升（底盘学会走向物体）
             reaching reward 缓慢上升（臂开始伸出但不准确）
             grasp reward 几乎为零
2000-5000 iter: navigation reward 饱和
               reaching reward 快速上升（策略学会在近处精确伸臂）
               grasp reward 开始出现非零值
5000-10000 iter: reaching reward 饱和
                grasp reward 稳步上升
                stability reward 在抓取时偶有下降（CoM 偏移）

如果 grasp reward 在 5000 iteration 后仍然为零——问题通常在 reaching 阶段：臂没有精确到位。检查 EE-to-object distance 是否在下降。如果在下降但未到达抓取阈值，减小 sigma_reach 或增大 arm action scale。

Phase 2 的详细训练配方：

# Phase 2 完整训练命令
uv run train LocoManip-Go2Arx-Phase2 \
  --env.scene.num-envs 2048 \
  --agent.max-iterations 10000 \
  --agent.learning-rate 1e-4 \
  --agent.gamma 0.99 \
  --agent.clip-param 0.2 \
  --agent.entropy-coeff 0.005 \
  --agent.num-mini-batches 4 \
  --agent.num-epochs 5 \
  --agent.logger wandb \
  --resume-from phase1_best.pt  # 从 Phase 1 热启动

Phase 2 的关键 lr 选择：为什么用 1e-4 而非 Phase 0/1 的 3e-4？因为策略已经有了好的初始化（hot-start 的 locomotion 权重 + Phase 1 的 arm reach 权重），大 lr 会破坏这些已学到的能力。用一个类比：hot-start 像是在一块已经粗加工好的石头上做精细雕刻——你需要小号刻刀（小 lr），不是大锤子（大 lr）。

Phase 2 中期的关键检查点（5000 iter）：

# Phase 2 中期健康检查脚本
def phase2_midpoint_check(env, policy, num_episodes=100):
    """在 5000 iter 时评估策略的四个关键指标。"""
    metrics = {
        "nav_success": 0,      # 底盘走到物体 0.5m 内
        "reach_success": 0,    # EE 到物体 0.1m 内
        "grasp_success": 0,    # 成功抓取并抬起
        "stability_ok": 0,     # base pitch < 0.3 rad 且未摔倒
    }

    for _ in range(num_episodes):
        obs = env.reset()
        for step in range(500):  # 10s episode @ 50Hz
            action = policy(obs)
            obs, reward, done, info = env.step(action)
            if done:
                break

        # 收集终态指标
        base_to_obj = info["base_to_object_dist"]
        ee_to_obj = info["ee_to_object_dist"]
        grasp_ok = info["grasp_success"]
        pitch = info["base_pitch_abs"]

        metrics["nav_success"] += (base_to_obj < 0.5).float().mean()
        metrics["reach_success"] += (ee_to_obj < 0.1).float().mean()
        metrics["grasp_success"] += grasp_ok.float().mean()
        metrics["stability_ok"] += (pitch < 0.3).float().mean()

    for k in metrics:
        metrics[k] /= num_episodes

    print("Phase 2 midpoint check (5000 iter):")
    print(f"  Navigation success:  {metrics['nav_success']:.1%} (target: >80%)")
    print(f"  Reaching success:    {metrics['reach_success']:.1%} (target: >50%)")
    print(f"  Grasp success:       {metrics['grasp_success']:.1%} (target: >10%)")
    print(f"  Stability OK:        {metrics['stability_ok']:.1%} (target: >90%)")

    # 诊断建议
    if metrics["nav_success"] < 0.5:
        print("  ⚠ Navigation too weak — increase nav reward weight or check base velocity command")
    if metrics["reach_success"] < 0.2:
        print("  ⚠ Reaching too weak — increase arm scale or ee_reaching weight")
    if metrics["grasp_success"] < 0.01:
        print("  ⚠ No grasps at all — check gripper threshold and object reachability")
    if metrics["stability_ok"] < 0.7:
        print("  ⚠ Frequent tipping — increase stability weight or reduce arm mass")

    return metrics

Phase 2 的 Episode 长度选择：Episode 太短（如 5s / 250 steps），策略来不及完成"导航→接近→抓取"的完整序列。Episode 太长（如 30s / 1500 steps），策略在抓取成功后的剩余步骤中"无事可做"，浪费训练计算。推荐 10-15 秒（500-750 steps @ 50 Hz）——足够完成全序列，且不过度浪费。

如果任务需要"抓取后搬运到目标位置"，episode 需要更长（20-30s）。如果只需要"抓取并抬起"，8-10s 就够了。

Phase 3：加 Domain Randomization 和扰动

# Phase 3 配置：加入 DR 准备 sim-to-real
phase3_cfg = dict(
    # 继承 Phase 2 的所有配置
    **phase2_cfg,
    resume_from="phase2_best.pt",

    # 物理 DR
    dr_body_mass=(-0.5, 0.5),      # 底盘质量 ±0.5 kg
    dr_arm_mass=(-0.3, 0.3),       # 臂负载质量 ±0.3 kg
    dr_friction=(0.3, 1.5),        # 地面摩擦
    dr_com_offset=(-0.02, 0.02),   # CoM 偏移 ±2 cm

    # 臂特有 DR（来自 SLIM 2025）
    dr_arm_mount_pos=(-0.01, 0.01),  # 安装位置 ±1 cm
    dr_arm_mount_yaw=(-3.0, 3.0),    # 安装朝向 ±3°
    dr_arm_control_noise=0.02,       # 臂控制信号噪声

    # 外部扰动
    push_robot_interval=10.0,       # 每 10 秒推一次
    push_force_range=(10, 50),      # N

    max_iterations=5000,
)

Phase 3 的目标：在不显著降低 Phase 2 性能的前提下，提高策略对物理参数变化的鲁棒性。典型的性能损失：grasp success rate 从 Phase 2 的 85% 降到 Phase 3 的 70-75%。如果降低超过 20%，DR 范围太大——需要缩小。

臂安装 DR（SLIM 2025 的贡献）：真实世界中，臂的安装位置和朝向与 CAD 设计不完全一致——3D 打印的安装板有公差，螺丝拧紧后可能有毫米级偏移。dr_arm_mount_pos 和 dr_arm_mount_yaw 模拟这种安装误差。如果不做这个 DR，策略会过拟合到完美的安装位置——真机上的微小偏移就会导致抓取失败。

四阶段的完整训练时间预算

阶段	GPU	num_envs	Iterations	Wall-clock	从哪里初始化
Phase 0	1× RTX 4090	4096	500-1000	30 min-1 h	Ch13 locomotion policy
Phase 1	1× RTX 4090	2048	2000-3000	1-3 h	Phase 0
Phase 2	1× RTX 4090	2048	5000-10000	5-10 h	Phase 1
Phase 3	1× RTX 4090	2048	3000-5000	3-5 h	Phase 2
总计				~10-20 h

对比直接端到端训练（不分阶段）：通常需要 20000+ iteration（30-50 h），且收敛不稳定——Phase 2 的初始化质量是整个管线成功的关键。

本质洞察：四阶段课程的核心思想是"先学子技能，再学组合"——这与人类学习运动技能的方式一致。你不会一上来就学"一边跑步一边接球"——你先学跑步，再学接球，最后学边跑边接。机器人训练也需要同样的渐进过程。hot-start 是这个过程的工程加速器——它让策略不需要从零学习已有的子技能。

用一个更具体的类比：想象训练一个杂技演员同时骑独轮车和抛接球。如果直接让他"边骑边抛"，他很可能两样都学不会——因为他的注意力被分散在两个完全不同的技能上。正确的训练方法是：Phase 0——先学会骑独轮车（locomotion），Phase 1——站在地上先学会抛接球（manipulation），Phase 2——骑着独轮车慢慢加入抛接（联合训练），Phase 3——在颠簸地面上练习（加 DR）。这正是 arm-constrained 四阶段课程的工程逻辑。

⚠️ 常见陷阱

⚠️ 编程陷阱：Phase 2 从 Phase 1 恢复时忘记调整 action mask

Phase 1 中底盘动作被 mask 为零（冻结），Phase 2 需要取消这个 mask。如果忘记，底盘在 Phase 2 中仍然不动——策略只学到了 reach 但没有 navigation。自检方法：Phase 2 开始后打印前 10 步的底盘动作输出，确认非零。

⚠️ 编程陷阱：Hot-start 时网络维度不匹配

Ch13 的 locomotion policy 输出 12D，Phase 0 的策略输出 19D。直接 load_state_dict 会因为维度不匹配而报错。正确做法：用 load_partial 只加载匹配的维度，或手动复制前 12 维的权重。

💡 概念误区：认为"Phase 3 的 DR 越多越好"

过度 DR（如臂安装偏移 ±5cm）会让策略学到"保守到什么都不做"——因为在极端 DR 下，任何积极的动作都可能失败。DR 的范围应该反映真实的物理不确定性，而不是"越大越鲁棒"。

练习

[设计题] 设计 Phase 0-3 的完整配置文件。包括每个阶段的 reward 开关、action mask、num_envs 和 max_iterations。
[实验题] 对比两种训练路径：(a) 四阶段课程（Phase 0→1→2→3），(b) 直接端到端训练（从零开始，全部 reward 同时生效）。在 10000 iteration 后比较 grasp success rate。
[分析题] 如果 Phase 1 的 EE position error 始终 > 5 cm（不达标），你应该修改什么？列出 3 个可能的原因和对应的修复方案。

19.7 精读：Visual Whole-Body Control (VBC, CoRL'24) ⭐⭐⭐

这一节解决什么问题：通过 VBC 的完整架构精读，理解分层 loco-manipulation 的工程实现——低层 whole-body tracker + 高层 visual policy 的三阶段训练。

VBC 系统概览

VBC（Visual Whole-Body Control，Liu et al., CoRL 2024 Oral）是当前最完整的开源 loco-manipulation 框架。在 Unitree B1+Z1 平台上实现了视觉引导的移动抓取——机器人看到 depth 图像中的目标物体，走过去，伸臂抓取。

核心架构：三阶段层级训练。

平台：B1 + Z1 + gripper，共 19 DoF（论文事实）

Stage 1: Low-level whole-body policy (RL)
  跟踪：root velocity command + target EE pose
  Input:  90D observation（论文）——含 base state、arm/leg state、last action、
          environment extrinsic、foot timing/contact 等
  Output: 腿部 12 个 joint angle targets；机械臂由 IK 把 EE pose 转成关节目标
          （不是直接输出 18/19 个原始关节位置）
  Key trick: hot-start from locomotion policy

Stage 2: High-level privileged teacher (RL)
  Input:  proprio + object_state(privileged，含相对 arm base 的 local object pose)
  Output: root velocity + EE pose/velocity + gripper 类命令
  Training: PPO with low-level frozen

Stage 3: Visuomotor student (DAgger)
  Input:  proprio + object masks + segmented depth images（两路视觉）
  网络:   CNN + GRU + MLP
  Output: same as Stage 2
  Training: DAgger from Stage 2 teacher
  部署频率：50 Hz / 10 Hz / 500 Hz（论文）

Stage 1 的 Hot-Start 细节：VBC 的关键工程贡献是"hot-start"——低层策略从纯 locomotion policy 初始化。具体做法：

先训练一个纯 locomotion velocity tracking policy（Ch13 的标准流程）
在 locomotion policy 的 obs 和 action 维度上扩展——增加 arm 关节的 obs 和 EE 目标的 obs
新增的维度用零初始化（MLP 的输入权重）和随机初始化（MLP 的输出权重的新维度部分）
开始 Phase 1 训练——MLP 的 locomotion 部分已经有好的权重，arm 部分从零开始学习

Stage 1 的 EE 目标采样：目标点在"height-invariant sphere"上均匀采样——球心在 base 上方，半径等于臂的可达距离。"height-invariant"意味着球面上的高度范围被限制在 [0.05, 0.5] m——不会要求末端到达地面以下或头顶以上的位置。

VBC 的低层策略使用 ROA（Regularized Online Adaptation）——与 Ch09 的 RMA 类似，在低层策略中内嵌一个 adaptation module，使策略能在部署时自动适应不同的物理参数（地面摩擦、负载质量等）。这是 VBC 实现 sim-to-real 的关键——低层策略不仅跟踪目标，还能适应真实世界的物理差异。

VBC 的 Reward 设计（Stage 1）

# VBC Stage 1 低层策略的 reward
def vbc_low_level_reward(env):
    # 1. Base velocity tracking（与 Ch13 相同）
    vel_error = env.base_vel - env.base_vel_cmd
    r_vel = torch.exp(-vel_error.norm(dim=-1) / 0.25)

    # 2. EE position tracking
    ee_error = env.ee_pos - env.ee_target_pos
    r_ee_pos = torch.exp(-ee_error.norm(dim=-1) / 0.05)

    # 3. EE orientation tracking（可选）
    quat_error = quat_distance(env.ee_quat, env.ee_target_quat)
    r_ee_quat = torch.exp(-quat_error / 0.1)

    # 4. Regularization
    r_action_rate = -torch.sum((env.action - env.last_action)**2, dim=-1) * 0.01
    r_joint_limit = -torch.sum(
        torch.clamp(torch.abs(env.joint_pos) - env.joint_limit * 0.9, min=0), dim=-1
    ) * 5.0

    # 5. 组合
    total = 1.0 * r_vel + 2.0 * r_ee_pos + 0.5 * r_ee_quat + r_action_rate + r_joint_limit
    return total

注意 r_ee_pos 的 sigma = 0.05 m（5 cm）——这比 Ch17 的 reach sigma（0.15 m）小很多，因为低层策略需要精确跟踪高层给出的 EE 目标。

VBC 低层策略的完整 mjlab 配置示例：

# mjlab: VBC Stage 1 低层策略的完整 env_cfg
class VBCLowLevelEnvCfg(ManagerBasedRLEnvCfg):
    """VBC 低层 whole-body tracker 的环境配置。

    低层策略接收 base velocity command + EE target pose，
    输出所有关节的位置偏移。
    """

    # 场景配置
    scene = SceneCfg(
        num_envs=4096,
        env_spacing=2.5,
        robot=ArticulationCfg(
            prim_path="/World/Robot",
            spawn=MJCFFileCfg(file_path="go2_arx.xml"),
        ),
    )

    # Observation 配置
    observations = ObservationsCfg(
        policy=ObservationGroupCfg(
            terms={
                # 底盘本体感知（与 Ch13 相同）
                "base_ang_vel": BaseAngularVelocityObsCfg(scale=0.25),
                "projected_gravity": ProjectedGravityObsCfg(),
                "leg_joint_pos": JointPositionObsCfg(
                    joint_names=[".*hip.*", ".*thigh.*", ".*calf.*"],
                    scale=1.0,
                ),
                "leg_joint_vel": JointVelocityObsCfg(
                    joint_names=[".*hip.*", ".*thigh.*", ".*calf.*"],
                    scale=0.05,
                ),
                # 臂本体感知
                "arm_joint_pos": JointPositionObsCfg(
                    joint_names=["arm_joint_.*"],
                    scale=1.0,
                ),
                "arm_joint_vel": JointVelocityObsCfg(
                    joint_names=["arm_joint_.*"],
                    scale=0.1,
                ),
                # 命令（由高层策略给出）
                "base_vel_cmd": CommandObsCfg(command_name="base_velocity"),
                "ee_target_pos": CommandObsCfg(command_name="ee_target_pos"),
                "ee_target_quat": CommandObsCfg(command_name="ee_target_quat"),
                # 历史
                "last_action": LastActionObsCfg(),
            },
        ),
    )

    # 动作配置——统一关节空间
    actions = JointPositionActionCfg(
        asset_name="robot",
        joint_names=[".*"],
        scale={
            ".*hip.*": 0.25,
            ".*thigh.*": 0.25,
            ".*calf.*": 0.25,
            "arm_joint_[1-3]": 0.15,
            "arm_joint_[4-6]": 0.10,
            "gripper.*": 0.5,
        },
    )

    # Reward 配置
    rewards = RewardsCfg(
        terms={
            "base_vel_tracking": VelocityTrackingRewardCfg(
                sigma=0.25, weight=1.0,
            ),
            "ee_pos_tracking": EEPositionTrackingRewardCfg(
                sigma=0.05, weight=2.0,
            ),
            "ee_quat_tracking": EEOrientationTrackingRewardCfg(
                sigma=0.1, weight=0.5,
            ),
            "action_rate": ActionRateRewardCfg(weight=-0.01),
            "joint_limit_penalty": JointLimitRewardCfg(
                threshold=0.9, weight=-5.0,
            ),
        },
    )

VBC Stage 1 训练配方：

# Stage 1: 低层 whole-body tracker
uv run train VBC-LowLevel-Go2Arx \
  --env.scene.num-envs 4096 \
  --agent.max-iterations 10000 \
  --agent.learning-rate 3e-4 \
  --agent.logger wandb \
  --resume-from go2_locomotion.pt  # Hot-start from Ch13 policy

训练参数	推荐值	调参方向
num_envs	4096	内存不足→2048
max_iterations	10000	EE error 未收敛→15000
learning_rate	3e-4	不稳定→1e-4
ee_sigma	0.05	EE 不准→减小到 0.03
base_vel_sigma	0.25	底盘不动→减小到 0.15
action_rate_weight	0.01	震荡→增大到 0.03

Stage 1 的 TensorBoard 关键指标：

需要监控的 5 个指标：
1. base_vel_tracking_reward: 应在 2000 iter 内达到 > 0.8
   → 如果 < 0.5：检查 hot-start 是否成功加载

2. ee_pos_tracking_reward: 应在 5000 iter 内达到 > 0.7
   → 如果停滞不涨：检查 EE 目标是否在臂工作空间内

3. ee_to_target_distance (cm): 应收敛到 < 3 cm
   → 如果 > 5 cm：增大 ee_pos weight 或减小 arm action scale

4. base_pitch_max (rad): 应 < 0.3 rad (~17°)
   → 如果 > 0.3：增大 stability weight

5. episode_length: 应接近 max_episode_length
   → 如果远小于 max：策略不稳定，频繁摔倒

Stage 2 的高层 Teacher

高层 teacher 使用 privileged 信息（物体精确位姿）来生成低层策略的目标。它的输出是：

base_vel_cmd（3D）：底盘应该以什么速度移动（方向指向物体，速度随距离减小）
ee_target_pos（3D）：末端应该去哪里（逐步接近物体的抓取位姿）
gripper_cmd（1D）：什么时候闭合夹爪

高层 teacher 的 reward 是任务级的——grasp_success + navigation_progress + stability。它不需要关心"关节怎么动"——那是低层策略的工作。

Stage 3 的 Visuomotor Student

Stage 3 把高层 teacher 蒸馏为视觉 student——用 depth 图像替代 privileged 物体状态。蒸馏使用 DAgger（与 Ch18 §18.5 完全相同的流程）。

Stage 3 数据流：
  depth(320×240) → resize(64×64) → CNN → visual_feature(128D)
  proprio(48D) → concat(visual_feature) → MLP → high_level_action(7D)
  high_level_action → low_level_policy(frozen) → joint_positions(18D)

向双框架的迁移

VBC 原始代码基于 IsaacGym。迁移到 mjlab 或 Isaac Lab 的关键步骤：

VBC (IsaacGym)	mjlab 对应	Isaac Lab 对应
B1+Z1 URDF	awesome-loco-manipulation URDF → MJCF 转换	URDF → USD 转换
IsaacGym 相机	MuJoCo Warp Batch Renderer	TiledCameraCfg
RSL-RL PPO	RSL-RL PPO（相同）	RSL-RL PPO（相同）
ROA adaptation	需要自行实现（从 VBC 代码移植）	需要自行实现
Hot-start 脚本	load_partial + zero-init 新维度	同左

⚠️ 常见陷阱

⚠️ 编程陷阱：Stage 2 训练时低层策略未冻结

Stage 2 训练高层 teacher 时，低层策略的权重应该被冻结（requires_grad=False）。如果不冻结，高层和低层同时更新会导致"移动目标"问题——低层在变，高层也在变，两者互相干扰导致训练不收敛。

💡 概念误区：认为"VBC 的三阶段等同于 §19.6 的四阶段"

VBC 的三阶段是"低层 → 高层 teacher → 视觉 student"（功能分层），§19.6 的四阶段是"冻结臂 → 冻结底盘 → 全身 → DR"（能力课程）。两者可以组合使用——先用 §19.6 的课程训练低层，再用 VBC 的 Stage 2-3 训练高层。

练习

[源码阅读题] 在 VBC 的代码（github.com/Ericonaldo/visual_wholebody）中找到 hot-start 的实现。它如何处理 locomotion policy（12D）到 loco-manip policy（18D）的维度扩展？
[设计题] VBC 的高层策略输出 7D（3D base_vel + 3D ee_pos + 1D gripper）。如果你想让高层也控制 EE orientation（+4D quaternion），Stage 2 的 reward 需要怎么修改？
[对比题] VBC 的分层架构 vs ETH 羽毛球的端到端架构——在什么任务条件下前者更适合？什么条件下后者更适合？

19.8 精读：ETH 羽毛球方法论（Science Robotics'25）⭐⭐⭐

这一节解决什么问题：通过 ETH 羽毛球的方法论精读，理解端到端 loco-manipulation 的极限工程——感知噪声模型、constrained RL 和主动感知行为的涌现。

系统概览

ETH 羽毛球（Ma et al., Science Robotics 2025）在 ANYmal-D + DynaArm 平台上实现了自主羽毛球对打——机器人用立体相机追踪羽毛球，预测飞行轨迹，移动到拦截位置，挥拍击球。挥拍速度最高达 12.06 m/s，连续对打最多 10 拍。

与 VBC 的关键区别：ETH 羽毛球使用端到端训练（不分层），一个策略同时控制所有关节（4 腿 × 3 关节 + 6 臂关节 = 18 DOF）。论文明确指出"the key reason for natural arm-leg compensation"——不分层让策略自动学会了复杂的底盘-臂协调（如挥拍时用后腿蹬地增加力量、击球前主动后仰以保持视觉追踪）。

核心工程贡献一：感知噪声模型

ETH 羽毛球最精细的工程贡献是"perception noise model"——用真实相机数据标定仿真感知噪声。

为什么需要这个？ 如果仿真中策略看到的羽毛球位置是完美的（零噪声），但真实世界中相机检测有 ±5 cm 的噪声，策略在真机上会因为"看到的球位置不稳定"而行为异常。简单的 DR（加高斯噪声）可以缓解这个问题，但噪声的大小和分布需要与真实相机匹配。

ETH 的做法： 1. 在真实环境中用双目相机检测羽毛球位置，同时用高精度 motion capture 记录真实位置 2. 计算两者的差值 → 得到"相机检测噪声"的统计分布（均值、方差、与距离的关系） 3. 在仿真中将这个噪声分布注入到策略的 observation 中

\[\text{obs}_{\text{noisy}} = \text{obs}_{\text{true}} + \mathcal{N}(\mu_{\text{cam}}, \Sigma_{\text{cam}}(d))\]

其中 \(\Sigma_{\text{cam}}(d)\) 是距离相关的协方差矩阵——远处的球检测噪声更大。

这个噪声模型的工程效果：策略在训练时就"习惯了"与真机一样的感知噪声水平。更重要的是，它导致了"主动感知"行为的涌现——策略学会了在追踪球时主动调整底盘的 pitch 角度以保持球在相机视野的中心区域（中心区域噪声更小）。这种行为不是被 reward 显式鼓励的——而是策略自动发现的"如何在噪声感知下最大化击球成功率"的策略。

核心工程贡献二：Constrained RL

标准 RL 通过 reward 惩罚"不良行为"（如关节速度过大）。但 reward 惩罚是"软约束"——策略可能发现在某些情况下"承受惩罚但获取更大 reward"的捷径。Constrained RL 把某些限制作为更强的约束来处理。

ETH 论文的实际做法（重要更正）： ETH 羽毛球用的约束算法是 N-P3O（一种 constrained PPO 变体），其关键硬约束是机械臂电流消耗 ≤ 8 A（真机上由保险丝限制）；而关节 torque 和 velocity 在论文中是 soft penalties（因为硬件上它们也不是立即失败的硬约束），不是文中此前写的"关节速度/力矩硬约束"。下面的 Lagrangian 实现作为通用 constrained RL 教学保留，但请注意它不是 ETH 采用的具体算法。

\[\max_\pi \mathbb{E}[R] \quad \text{s.t.} \quad \text{(ETH: arm current} \le 8\,\text{A，硬约束)};\quad \tau_i,\dot{q}_i \text{ 作为软惩罚}\]

为什么操作比 locomotion 更需要约束？ 挥拍时的关节速度（可达 12 m/s 的末端速度 → 对应极高的关节角速度）与电流会逼近电机/供电的物理极限。如果仿真中不约束电流，策略可能学到"超出供电能力"的动作——真机上保险丝会切断或电机过载。约束式 RL 确保策略在训练时就在安全范围内操作。

通用实现方式（非 ETH 专用）：一种常见做法是 Lagrangian relaxation——将约束转化为拉格朗日乘子项添加到 reward 中，乘子在训练过程中自适应调整。

# Constrained RL 的完整实现
class ConstrainedPPO:
    """PPO + Lagrangian 约束。

    与标准 PPO 的区别：
    1. reward 中加入约束违反的惩罚
    2. 拉格朗日乘子在每个 epoch 后自适应更新
    3. 乘子初始为零——约束从松到紧渐进生效
    """
    def __init__(self, constraint_names, target_violations):
        # 每个约束一个拉格朗日乘子
        self.lambdas = {name: 0.0 for name in constraint_names}
        self.targets = target_violations  # 每个约束的容忍违反量
        self.lambda_lr = 0.01             # 乘子更新速率
        self.lambda_max = 10.0            # 防止乘子爆炸

    def augmented_reward(self, base_reward, violations):
        """计算加入约束惩罚后的 augmented reward。

        Args:
            base_reward: [B] 基础 reward（不含约束）
            violations: dict[str, Tensor[B]] 每个约束的违反量
        Returns:
            augmented: [B] 最终 reward
        """
        penalty = torch.zeros_like(base_reward)
        for name, v in violations.items():
            penalty += self.lambdas[name] * v
        return base_reward - penalty

    def update_lambdas(self, mean_violations):
        """每个 PPO epoch 后更新拉格朗日乘子。

        如果平均违反量 > 目标值，增大 lambda（加强约束）
        如果 < 目标值，减小 lambda（放松约束）
        """
        for name, mean_v in mean_violations.items():
            gradient = mean_v - self.targets[name]
            self.lambdas[name] += self.lambda_lr * gradient
            self.lambdas[name] = max(0.0, min(self.lambdas[name], self.lambda_max))

# 使用示例
constrained_ppo = ConstrainedPPO(
    constraint_names=["joint_vel", "joint_torque"],
    target_violations={
        "joint_vel": 0.01,    # 容忍 1% 的步超速
        "joint_torque": 0.01, # 容忍 1% 的步超力矩
    },
)

# 在 training loop 中：
for epoch in range(num_epochs):
    # 计算约束违反量
    violations = {
        "joint_vel": compute_vel_violation(joint_vel, vel_limit),
        "joint_torque": compute_torque_violation(joint_torque, torque_limit),
    }
    # 计算 augmented reward
    reward = constrained_ppo.augmented_reward(base_reward, violations)
    # 标准 PPO 更新（使用 augmented reward）
    ppo_update(reward, ...)
    # 更新乘子
    constrained_ppo.update_lambdas({k: v.mean().item() for k, v in violations.items()})

Constrained RL 与 reward penalty 的关键区别：

维度	Reward Penalty	Constrained RL (Lagrangian)
约束强度	固定权重（人工设定）	自适应乘子（自动调整）
能否保证约束满足	不能（权重可能不够大）	渐近保证（乘子会一直增大直到约束满足）
对 reward 的影响	固定的梯度干扰	自适应的梯度干扰
工程复杂度	低（加一个 penalty 项）	中（需要乘子更新逻辑）
推荐场景	软约束（如"尽量不震荡"）	硬约束（如"绝对不超过电机力矩极限"）

对 locomotion 的 action_rate penalty 使用 reward penalty 就够了（软约束）。但对机械臂的关节速度/力矩限制，constrained RL 更安全——特别是在高速操作（如挥拍、投掷）中，超出电机极限会造成硬件损坏。

ETH 羽毛球的约束（论文可核验项）：

约束	类型	说明
机械臂电流消耗 ≤ 8 A	硬约束（N-P3O 强制）	真机上由保险丝限制；这是论文的关键硬约束
关节 torque	软惩罚	作为 reward penalty，硬件上也按软约束处理
关节 velocity	软惩罚	同上

注意：此前给出的"ANYmal ±15/±80、DynaArm ±20/±30、球拍 tip ≤15 m/s"这类具体数值未在论文中核验到（论文重点是 8 A 电流硬约束 + torque/velocity 软惩罚）；如需具体电机限制，请查对应硬件手册。

核心工程贡献三：Shuttlecock Prediction Model

策略不直接使用相机检测到的球的当前位置——而是通过一个预测模块估计球的飞行轨迹和拦截时间/位置。

Prediction Module:
  Input: 最近若干帧的球位置检测（含噪声）
  Output: 预测的拦截时间 t_intercept + 拦截位置 p_intercept

策略的输入：
  proprio + predicted_intercept_time + predicted_intercept_pos

重要更正（论文事实）： ETH 的轨迹预测不是一个离线训练的小 MLP，而是 EKF + 基于物理的羽毛球动力学模型——shuttlecock trajectory generation 与 EKF 都遵循同一个 shuttlecock dynamics model（论文用 measured aerodynamic length L = 4.1 m），且训练与部署复用同一套 EKF/预测模块。下面用物理模型生成轨迹的代码仅作为"如何用空气动力学模型生成/预测羽毛球轨迹"的教学示例（与论文的物理建模思路一致），不要理解为"训练一个学习型 MLP predictor"。

预测模型的训练数据生成：

# 生成 shuttlecock 预测模型的训练数据
def generate_shuttle_trajectories(num_trajs=10000):
    """在仿真中生成大量带有空气阻力的羽毛球轨迹。

    物理模型：
    - 初速度：5-20 m/s，方向随机（但总是朝对面场地）
    - 空气阻力：F_drag = 0.5 * rho * Cd * A * v^2
    - 重力：g = 9.81 m/s^2
    - 添加风扰动：随机水平加速度 ±0.5 m/s^2
    """
    trajectories = []
    for _ in range(num_trajs):
        # 初始条件
        v0 = np.random.uniform(5, 20)
        angle = np.random.uniform(10, 60)  # 度
        azimuth = np.random.uniform(-30, 30)  # 度

        # 数值积分（Euler method, dt=0.01s）
        pos, vel = [start_pos], [v0_vec]
        for t in range(500):
            drag = 0.5 * 1.225 * 0.6 * 0.003 * np.linalg.norm(vel[-1])**2
            drag_dir = -vel[-1] / (np.linalg.norm(vel[-1]) + 1e-6)
            acc = np.array([0, 0, -9.81]) + drag * drag_dir
            acc[:2] += np.random.normal(0, 0.5, 2)  # 风扰动
            new_vel = vel[-1] + acc * 0.01
            new_pos = pos[-1] + new_vel * 0.01
            pos.append(new_pos)
            vel.append(new_vel)
            if new_pos[2] < 0:  # 触地
                break

        # 添加感知噪声（匹配真实相机特性）
        noisy_pos = [p + np.random.normal(0, 0.02 + 0.01 * np.linalg.norm(p - camera_pos))
                     for p in pos]

        trajectories.append({
            "true_pos": np.array(pos),
            "noisy_pos": np.array(noisy_pos),
            "intercept_time": find_intercept_time(pos, robot_reachable_region),
            "intercept_pos": find_intercept_pos(pos, robot_reachable_region),
        })

    return trajectories

空气阻力模型使用简化的二次阻力公式——精确的空气动力学（如 Magnus 效应、风的影响）通过训练数据的 DR 覆盖。这种"简化物理 + DR"的方法论比精确建模更鲁棒——因为真实世界中风的影响是不可预测的。

ETH 羽毛球的涌现行为

论文报告了三种未被 reward 显式鼓励但自然涌现的行为：

时间约束响应：当球飞来的时间紧迫时，机器人自动加快移动速度和挥拍速度；当时间充裕时，动作更从容准确。这来自 reward 中的"击球成功"目标——时间紧迫时只有快速动作才能成功，时间充裕时策略优化准确性。
击球后自动恢复：挥拍后机器人自动回到场地中央（"恢复姿态"）。这来自 reward 中的多目标击球训练——每个 episode 包含多次击球，策略发现回到中央可以为下次击球做更好的准备。
主动感知：在追踪球时主动调整底盘 pitch 角度以保持球在视野中心。这来自感知噪声模型——相机视野边缘的检测噪声更大，策略学会了"把球放在视野中心"以获得更准确的位置估计。

本质洞察：ETH 羽毛球证明了一个重要的工程原则——如果仿真环境的物理和感知特性足够接近真实世界，端到端 RL 可以自动发现人类工程师不会显式设计的策略（如主动感知）。这些涌现行为不是免费的——它们需要精心标定的感知噪声模型、正确的物理参数和足够的训练计算。但一旦条件满足，RL 的策略搜索能力可以超越人类的直觉。

向通用 Loco-Manipulation 的工程启示

ETH 羽毛球虽然是一个特定的任务（打羽毛球），但其工程方法论可以直接迁移到通用的 loco-manipulation：

ETH 羽毛球方法	通用 Loco-Manipulation 对应
Perception noise model	任何视觉 sim-to-real 都需要标定感知噪声
Constrained RL	任何高速操作（如工业流水线 pick-and-place）都需要约束关节速度
End-to-end whole-body	需要复杂底盘-臂协调的任务（如搬运大件物品）
Shuttlecock predictor	任何需要预测动态目标的任务（如传送带抓取）

⚠️ 常见陷阱

⚠️ 编程陷阱：Constrained RL 的 lambda 初始值太大

如果 lambda 初始值太大（如 100），约束惩罚主导了 reward——策略会学到"什么都不做"以避免违反约束。正确做法：lambda 从 0 开始，通过自适应更新逐步增大到合理值。

💡 概念误区：认为"ETH 羽毛球的方法可以直接用于任何任务"

ETH 羽毛球的端到端训练之所以成功，部分原因是任务的"清晰度"——击球成功/失败是一个明确的二值信号，不需要复杂的 staged reward。对于更复杂的任务（如在杂乱桌面上整理物品），端到端训练可能因为 reward 设计的困难而失败——分层方案（VBC 模式）更适合。

练习

[分析题] ETH 羽毛球的 asymmetric actor-critic 中，critic 看到了什么 actor 看不到的信息？这种不对称如何帮助训练？
[设计题] 如果你要将 ETH 羽毛球的 constrained RL 应用到一个 Go2-Arx 的搬运任务中，需要约束什么关节的什么物理量？约束值应该从哪里获取？
[跨章综合题] ETH 羽毛球的感知噪声模型与 Ch18 的 depth DR 有什么异同？两者的标定方法有什么区别？

19.9 训练诊断与调试 ⭐⭐

这一节解决什么问题：Loco-manipulation 训练失败的模式比纯 locomotion 或纯 manipulation 更多。如何系统性定位问题？

Loco-Manipulation 的六层诊断流程

Layer 1：底盘 locomotion 可行（5 min）
  □ Phase 0 的 velocity tracking error < 基线 + 20%
  → 不通过：回到 Ch13 检查底盘策略

Layer 2：臂 reach 可行（5 min）
  □ Phase 1 的 EE position error < 3 cm
  → 不通过：检查 arm action scale, PD gain, EE 目标采样

Layer 3：联合训练收敛（训练中）
  □ Navigation reward 在 2000 iter 内上升
  □ EE reaching reward 在 5000 iter 内上升
  □ Grasp success rate 在 8000 iter 内 > 0
  → 不通过：检查 reward 权重平衡、action scale、物体初始位置

Layer 4：行为质量（play 可视化）
  □ 底盘走向物体方向正确
  □ 臂在接近物体时伸出
  □ 夹爪在正确时机闭合
  □ 抓取后底盘不翻倒
  → 不通过：逐项检查 reward 的分项贡献

Layer 5：鲁棒性（DR 后）
  □ Phase 3 的 success rate > Phase 2 × 70%
  → 不通过：减小 DR 范围

Layer 6：Sim-to-Real（部署时）
  □ 底盘和臂的关节命令在合理范围
  □ 真机上底盘能走到物体附近
  □ 臂能伸出并闭合夹爪
  → 不通过：检查关节顺序映射、action scale 一致性、通信延迟

典型失败模式与根因

症状	可能原因	快速检查
底盘走到物体旁但臂不伸	arm action scale 太小 / arm reward 权重太低	打印 arm action 的绝对值
臂在远处就开始挥动	navigation → reaching 的门控缺失	检查是否用了乘法门控
抓取时底盘漂移	stop_during_grasp reward 缺失或权重太低	play 中观察底盘速度
机器人伸臂后翻倒	stability reward 太弱 / 臂太重	打印 base pitch/roll
grasp success 始终为零	夹爪 action 的阈值化缺失	打印 gripper 输出值
reward 上涨但 success rate 不变	reward hacking（底盘快速靠近获取 nav reward 但不抓）	独立记录 task metric
Phase 2 不收敛	Phase 0/1 热启动失败	检查权重加载是否成功
action 震荡	action_rate penalty 不足 / 底盘和臂的 scale 失衡	增大 action_rate weight

"底盘到了但臂不动"的详细诊断树

底盘走到物体旁，但臂不伸出
├── arm action 输出为零
│   ├── arm 被冻结（Phase 0 的 mask 未取消）
│   │   → 检查 Phase 2 配置的 arm_frozen 参数
│   └── arm action scale = 0
│       → 检查 action_scale 配置
├── arm action 输出非零但很小
│   ├── arm reward 权重太低（被 locomotion reward 压制）
│   │   → 增大 r_reach 权重到 2.0-3.0
│   └── arm action scale 太小
│       → 增大到 0.15-0.2
├── arm 伸出但方向错误
│   ├── object_pos 坐标系错误（使用了世界坐标而非 base 相对坐标）
│   │   → 切换到 object_pos_base
│   └── mount frame 旋转错误（臂朝侧面安装但 obs 假设朝前）
│       → 检查 mount frame 的 rot 参数
└── arm 伸出方向正确但到不了物体
    ├── 物体不在臂的工作空间内（底盘停得太远）
    │   → 减小 sigma_nav，让底盘更靠近
    └── DiffIK 的目标超出可达范围
        → 在 reward 中加入 manipulability 惩罚

⚠️ 常见陷阱

⚠️ 编程陷阱：Reward 各项的量级差异导致梯度失衡

如果 r_nav 的典型值域是 [0, 1] 但 r_stab 的典型值域是 [-10, 0]（大的惩罚值），PPO 的 advantage 估计会被 r_stab 主导——策略只学会"不摔倒"但不学"走向物体"。解决方案：确保所有 reward 项的值域大致相当（如都在 [-1, 1] 或 [0, 1] 范围内），通过权重而非值域来控制重要性。

练习

[诊断题] 你的策略在 play 中表现为"底盘围着物体转圈但不停下来"。列出 3 个可能的原因和对应的检查步骤。
[实验题] 训练一个完整的 Go2-Arx 抓取策略。在训练过程中记录 6 个指标：total_reward、nav_reward、reach_reward、grasp_success、base_pitch_max、ee_to_object_dist。画出 6 条曲线，分析每个阶段的学习进展。

TensorBoard 指标解读指南

训练 Loco-Manipulation 策略时，TensorBoard 中有几十条曲线。以下是按优先级排列的关键指标及其正常模式：

第一层（5 秒检查——训练是否在正确方向上？）
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
episode_reward_mean:
  正常：持续上升或阶梯状上升
  异常：持续震荡或下降 → 检查 lr、reward 设计、hot-start

grasp_success_rate (task metric):
  正常：0-5k iter 为零，5k+ 开始出现，10k+ > 30%
  异常：10k iter 后仍为零 → EE 到不了物体（检查 reach reward）

第二层（30 秒检查——各子技能是否在学？）
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
navigation_reward:
  正常：最先涨（0-2k iter），饱和在 0.7-0.9
  异常：不涨 → 底盘不动（检查 base action scale）

ee_reaching_reward:
  正常：2k-5k iter 开始涨
  异常：滞后太多 → arm action scale 太小，或 Phase 1 没做好

base_to_object_dist (m):
  正常：随训练逐步减小
  异常：减小后又增大（震荡）→ 策略在"冲刺循环"

ee_to_object_dist (m):
  正常：在 nav 收敛后开始减小
  异常：始终 > 0.2m → arm 够不到物体

第三层（诊断时按需查看）
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
base_pitch_max (rad):
  正常：< 0.3 rad
  异常：频繁 > 0.4 → 翻倒风险，增大 stability weight

arm_action_norm:
  正常：随训练逐步增大（臂越来越活跃）
  异常：始终接近零 → arm 被冻结或 scale=0

gripper_action_mean:
  正常：在接近物体时从 1.0(开) 变到 0.0(合)
  异常：始终 ~0.5 → 夹爪未做阈值化

policy_entropy:
  正常：缓慢下降
  异常：过快下降 → 探索不足，增大 entropy_coef

如何从 TensorBoard 曲线判断"卡在哪个阶段"：

现象	卡在哪一步	首先检查什么
nav reward 低，其他全低	导航阶段	base action scale, base velocity command
nav reward 高，reach reward 低	接近阶段	arm action scale, EE 目标是否在工作空间内
nav + reach 高，grasp 为零	抓取阶段	gripper 阈值化, grasp 成功判定条件
nav + reach + grasp 都不错但波动大	稳定性	stability weight, CoM 偏移

训练过程中的常见"假阳性"

"Reward 涨了但策略在作弊"：以下三种常见的 reward hacking 模式：

冲刺循环：策略反复高速冲向物体（获取大的 nav reward），但到达后不停下——因为减速意味着瞬时 nav reward 下降。解决：加 stop_during_grasp reward。
悬浮抓取：策略发现了某个关节配置能让夹爪"卡住"物体（不是真正的力闭合抓取），然后用这个配置获取 grasp success reward。在仿真中看起来成功，但真机上因为接触模型不同而失败。解决：在 grasp 判定中加入"物体必须在两个手指之间"的几何检查。
原地旋转：策略发现原地旋转可以让 base_to_object_dist 在某些角度变小（因为底盘不是点，而是有长度的矩形）。解决：用 base 中心而非 base 前端来计算距离。

19.10 双框架 API 映射与视觉 Loco-Manipulation 管线 ⭐⭐

这一节解决什么问题：如何在 mjlab 和 Isaac Lab 中分别配置 loco-manipulation 环境？视觉输入如何整合到 loco-manipulation 管线中？

双框架 API 映射表

功能	mjlab (MuJoCo Warp)	Isaac Lab (PhysX/Omniverse)
复合模型加载	`attach` / `include` in MJCF	USD Composer 组装 + `ArticulationCfg`
Actuator 分组	`<default class="legs">` + `<default class="arm">`	`ImplicitActuatorCfg` with `joint_names_expr`
动作空间	`JointPositionActionCfg` with per-joint scale	`JointPositionActionCfg` with regex scale dict
底盘 obs	base 角速度等 obs term	概念对应（具体类名以各自版本文档为准）
臂 obs	关节位置 obs term（`joint_names=["arm_.*"]`）	同左（概念对应）
物体 obs	物体状态 obs term	物体状态 obs term
Height scan	`RayCastSensorCfg`（mjlab，BVH ray tracing）	`RayCasterCfg`（Isaac Lab，基于 Warp，非 PhysX）
Depth camera	`CameraSensorCfg`（MuJoCo Warp Batch Renderer）	`TiledCameraCfg`（RTX）
Reward	`RewardManager` with `RewardTermCfg`	`RewardManager` with `RewardTermCfg`
Terrain	`hfield` in MJCF	Procedural terrain generator
DR	`EventManager` (startup/reset/interval)	`EventTermCfg` (same 4 modes)
PPO 后端	RSL-RL	RSL-RL / rl_games

两个框架都基于 Manager-based 架构，observation/reward 的设计模式相近，但"同名/几乎一致"需逐类核验——相机、raycast、asset、event、action 等具体类名会随版本变化（上表已把概念映射与精确类名分开标注）。主要差异在渲染管线（mjlab 的 MuJoCo Warp vs Isaac Lab 的 RTX）和模型格式（MJCF vs USD）。迁移时除了资产加载和渲染配置，资产、关节顺序、IK、相机/mask pipeline 也需逐项重建，不是简单改两行就能直接复用。

Isaac Lab 内置的 Loco-Manipulation 环境

Isaac Lab 2.3 提供了 G1 humanoid 的 in-place loco-manipulation 环境（官方 task ID Isaac-PickPlace-Locomanipulation-G1-Abs-v0），可以作为配置结构参考——但要注意它偏向 Mimic/teleop/IK workflow，不是现成的 RL reward/curriculum 环境：

# Isaac Lab 内置环境（官方源码类名是 LocomanipulationG1EnvCfg）
# 位置: manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py
# 注意：这是 humanoid (G1) 的环境，不是四足的
from isaaclab_tasks.manager_based.locomanipulation.pick_place.locomanipulation_g1_env_cfg \
    import LocomanipulationG1EnvCfg

# 重要：官方源码中该环境 rewards = None、curriculum = None，
#   更偏 IK / Mimic / teleop 数据生成 workflow，
#   不要把它当作"navigation + reaching + grasping + stability + adaptive terrain"的 RL 环境复用；
#   可参考其上半身/下半身控制器与 IK workflow 的配置结构，而非 reward/curriculum。
# 对于四足 + 臂，需要自定义 RL 环境（自己设计 reward 与 curriculum）。

视觉 Loco-Manipulation 管线：从 Ch18 到 Ch19 的桥接

Ch18 建立了视觉 locomotion 的管线（depth → CNN → locomotion 策略），Ch17 建立了视觉操作的管线（depth/RGB → CNN → 操作策略）。视觉 loco-manipulation 需要融合两者——一个视觉系统同时服务于导航和操作。

VBC 的视觉管线（Stage 3）：

前视 depth camera (320×240)
    → resize (64×64)
    → CNN encoder → visual_feature (128D)
    → concat(proprio, visual_feature)
    → MLP → high_level_action
        ├── base_vel_cmd (3D): 底盘往哪走
        ├── ee_target_pos (3D): 末端去哪里
        └── gripper_cmd (1D): 什么时候抓

    → 传给 low_level_policy (frozen)
    → joint_positions (18D)

视觉编码器的设计决策：对 loco-manipulation，CNN 需要同时提取两类信息——地形几何（用于导航）和物体位置（用于操作）。这比纯 locomotion 的 CNN（只提取地形）或纯操作的 CNN（只提取物体）更难。

两种视觉编码策略：

策略一：共享编码器。 一个 CNN 同时编码地形和物体信息。优势：参数共享、计算高效。劣势：两类信息在同一个 feature space 中可能互相干扰。

# 共享视觉编码器
class SharedVisualEncoder(nn.Module):
    def __init__(self):
        super().__init__()
        self.cnn = SmallCNN(in_channels=1, out_dim=128)
        # 128D 特征同时编码地形和物体信息

    def forward(self, depth):
        return self.cnn(depth)  # [B, 128]

策略二：分支编码器。 两个 CNN 分别编码地形（低分辨率全局特征）和物体（高分辨率局部特征），然后融合。优势：两类信息不干扰。劣势：参数量和计算量翻倍。

# 分支视觉编码器
class BranchedVisualEncoder(nn.Module):
    def __init__(self):
        super().__init__()
        # 地形分支：大感受野，低分辨率
        self.terrain_cnn = SmallCNN(in_channels=1, out_dim=64,
                                    kernel_sizes=[7, 5, 3])
        # 物体分支：小感受野，高分辨率
        self.object_cnn = SmallCNN(in_channels=1, out_dim=64,
                                   kernel_sizes=[3, 3, 3])

    def forward(self, depth):
        terrain_feat = self.terrain_cnn(depth)  # [B, 64]
        object_feat = self.object_cnn(depth)    # [B, 64]
        return torch.cat([terrain_feat, object_feat], dim=-1)  # [B, 128]

VBC 使用共享编码器（更简单）。"分支编码器"是一种可选的自定义架构设计，本节作为工程选项介绍，需要消融验证——它并非某个具体论文的既定做法。（顺带更正：VIRAL 是 Unitree G1 人形上的 RGB-based loco-manipulation 工作，其 student 用 RGB 图像编码器如 DINOv3 与 proprioception 融合，并不是"地形/物体双 CNN 分支编码器"，也不是四足 Go2-Arx 场景，不要据此当作分支编码器的来源。）对初学者推荐共享编码器——先跑通管线再优化架构。

多相机方案：如果场景中地形和物体在不同方向（如地形在脚下、物体在桌上），一个前视相机可能无法同时看到两者。解决方案：

前视 + 下视双相机（与 Ch18 §18.2 的多相机方案一致）
广角相机：FOV > 90° 的鱼眼相机可以同时看到地面和前方
头部相机 + 手腕相机：分别服务导航和操作（类似人类的"远视+近视"）

从 State Teacher 到 Visual Student 的 Loco-Manipulation 蒸馏

将 Ch18 的三阶段蒸馏管线应用到 loco-manipulation：

Stage 1: State Teacher (privileged)
  Input:  proprio + height_scan + object_state_privileged
  Output: base_vel + ee_target + gripper
  → 训练方法：PPO + §19.5 的多目标 reward

Stage 2: Depth Student (DAgger)
  Input:  proprio + depth_image(64×64)
  Output: base_vel + ee_target + gripper
  → 蒸馏方法：Ch18 §18.5 的 DAgger

Stage 3: RGB Student (optional)
  Input:  proprio + rgb_image(64×64×3)
  Output: base_vel + ee_target + gripper
  → 蒸馏方法：DAgger + 视觉 DR (Ch18 §18.6)

Loco-Manipulation 蒸馏与纯 Locomotion 蒸馏的关键差异：

维度	纯 Locomotion 蒸馏 (Ch18)	Loco-Manipulation 蒸馏
Teacher 的 privileged info	height_scan	height_scan + object_state
Student 需要提取的信息	地形几何	地形几何 + 物体位置/姿态
CNN 的学习难度	中（单一信息类型）	高（两类信息同时提取）
DAgger 收敛速度	较快（5k iter）	较慢（8-15k iter）
MTS 是否需要	可选（heading prediction）	强烈推荐（navigation + reaching 的联合预测）

工程建议：先在 state teacher（无视觉）上完整调通 loco-manipulation 管线（§19.1-19.9），确认 task metric（grasp success rate）达标后，再加入视觉蒸馏。不要一开始就从视觉端到端训练——调试难度太大，无法区分"策略问题"和"视觉问题"。

⚠️ 常见陷阱

⚠️ 编程陷阱：视觉 Student 蒸馏时底盘和臂的 DAgger 更新不同步

如果 DAgger 的学习率对底盘相关的输出维度和臂相关的输出维度是相同的，但臂的输出更难学（因为需要精确定位），训练可能出现"底盘已经学会导航但臂还在挣扎"的不平衡。解决方案：对臂相关的 DAgger loss 使用更大的权重（2-3 倍于底盘）。

💡 概念误区：认为"loco-manipulation 的视觉管线等于 locomotion 视觉 + manipulation 视觉的简单叠加"

两者的视觉需求是矛盾的——locomotion 需要低分辨率全局地形感知（CNN 的大感受野），manipulation 需要高分辨率局部物体感知（CNN 的小感受野）。一个 CNN 很难同时满足两者。分支编码器或多相机方案是更可靠的解决方案。

练习

[设计题] 为 Go2-Arx 的视觉 loco-manipulation 设计完整的三阶段蒸馏管线。列出每个阶段的 input/output 维度、训练方法和预期训练时间。
[对比题] 比较共享编码器和分支编码器在以下两个任务上的适用性：(a) 平地上的桌面抓取（地形简单），(b) 粗糙地面上的地面物体拾取（地形复杂）。
[跨章综合题] 回顾 Ch18 的 depth DR 配置。如果将其应用到 loco-manipulation 的视觉蒸馏中，需要增加什么 DR 维度？（提示：物体的外观变化）

19.11 全阶段参数汇总与工程快查表 ⭐⭐

这一节解决什么问题：提供一张可快速查阅的参数表，当 loco-manipulation 策略出现特定现象时应该调什么。

全阶段 PPO 超参数推荐

阶段	num_envs	lr	gamma	clip	entropy	iterations	Wall-clock
Phase 0	4096	3e-4	0.99	0.2	0.01	500-1000	30 min
Phase 1	2048	3e-4	0.99	0.2	0.01	2000-3000	1-3 h
Phase 2	2048	1e-4	0.99	0.2	0.005	5000-10000	5-10 h
Phase 3 (DR)	2048	5e-5	0.99	0.15	0.005	3000-5000	3-5 h
VBC Stage 2	2048	1e-4	0.995	0.2	0.01	5000	3-5 h
Visual Student	512-1024	1e-4	—	—	—	5000-10000	5-15 h

Phase 2 的 lr 比 Phase 0/1 低——因为策略已经有了好的初始化（hot-start），大 lr 会破坏已学到的子技能。Phase 3 进一步降低 lr——DR 只需要微调鲁棒性，不需要学习新行为。

Reward 权重推荐与调参方向

Reward 项	推荐权重	值太大的后果	值太小的后果
navigation	1.0	底盘冲太快、不停下	底盘不动
ee_reaching	2.0	臂在远处就乱伸	臂不伸
grasp_success	10.0	策略只追求抓取忽略步态	不尝试抓取
stability	2.0	臂活动范围受限	频繁翻倒
stop_during_grasp	5.0	底盘过早停下	边走边抓导致精度低
action_rate_legs	0.01	底盘步态太保守	腿部震荡
action_rate_arm	0.02	臂运动太慢	臂部震荡
alive	0.5	策略太保守（不做任何事避免失败）	自杀策略
base_height	0.5	姿态太僵硬	底盘高度不稳定

Action Scale 推荐

关节组	推荐 scale	物理含义	调参指引
底盘 hip	0.25 rad	每步最大偏移 ~15°	步态太保守→增大到 0.3
底盘 thigh	0.25 rad	每步最大偏移 ~15°	同上
底盘 calf	0.25 rad	每步最大偏移 ~15°	同上
臂 joint 1-3	0.15 rad	每步最大偏移 ~9°	精度不够→减小到 0.1
臂 joint 4-6	0.10 rad	每步最大偏移 ~6°	精度不够→减小到 0.07
夹爪	0.5	每步最大开合变化 50%	夹爪反应慢→增大到 0.8
底盘 vx (分层)	1.0 m/s	最大前进速度	太慢→增大到 1.5
底盘 vy (分层)	0.5 m/s	最大侧移速度	同上
底盘 wz (分层)	1.0 rad/s	最大转向角速度	转向不灵→增大到 1.5

症状→参数的快速对照

症状	最可能的原因	首先调整的参数
底盘完全不动	base action scale = 0 / base velocity 未解冻	action_scale, Phase 2 配置
臂完全不动	arm_frozen 未取消 / arm scale = 0	Phase 2 arm_frozen 参数
底盘走到了但臂不伸	arm reward 权重太低	ee_reaching weight: 2.0→4.0
臂在远处就乱挥	乘法门控缺失 / nav 没有门控 reach	reward 融合方式
底盘围物体转圈	sigma_nav 太大 / stop_during_grasp 缺失	sigma_nav: 0.5→0.3
伸臂后翻倒	stability weight 太低 / CoM 偏移过大	stability weight: 2.0→5.0
抓取成功率始终为零	gripper 未阈值化 / 物体太远	gripper 实现, sigma_nav
reward 涨但 success rate 不涨	reward hacking (冲刺循环)	stop_during_grasp, grasp weight
Phase 2 初期大量摔倒	hot-start 失败	检查 --resume 权重加载
DR 后 success rate 降 > 30%	DR 范围太大	缩小 DR 到 50%
训练 20k iter 不收敛	未用课程训练	切换到四阶段课程
夹爪始终半开	连续 action 未做 threshold	加 threshold(action > 0 → close)

性能预算参考

配置	GPU	Phase 0 (steps/s)	Phase 2 (steps/s)	Visual Student (steps/s)
最小	RTX 3060	~20k	~12k	~3k
标准	RTX 3090/4090	~50k	~30k	~8k
高端	A100/H100	~100k	~60k	~15k

Phase 2 比 Phase 0 慢约 40%——因为动作维度更高（19D vs 12D）且 reward 计算更复杂（多目标融合）。Visual Student 进一步慢 3-5 倍——渲染是瓶颈（与 Ch18 一致）。

本章小结

知识点	核心结论	重要程度
三重耦合	IK 耦合 + 动力学耦合 + 感知耦合，不可分离处理	⭐⭐⭐
CoM 偏移量化	Go2+ARX 臂伸出时 CoM 偏移约 5-7cm（mount offset 须按臂质量加权，不能直接相加）	⭐⭐
MJCF 组合建模	attach + prefix 是推荐方式，awesome-loco-manipulation 提供预组装 URDF	⭐⭐
URDF→MJCF 转换	fixed joint 被合并、PD gain 需要按关节组分别配置	⭐⭐
PD gain 分组	底盘 kp=40 vs 臂 kp=10-15，差异来自承重/精度的不同需求	⭐⭐⭐
混合动作空间	action scale 归一化使梯度平衡，臂 scale 通常为底盘的 50-70%	⭐⭐⭐
三种动作模式	End-to-end / Hierarchical / Mixed，各有适用场景	⭐⭐
相对坐标 obs	object_pos_base 比 object_pos_world 泛化性好得多（平移不变性）	⭐⭐⭐
Quaternion 表示	推荐 (w,x,y,z) + "确保 w>0" 消除符号歧义	⭐⭐
多目标 reward 融合	乘法门控编码因果顺序 + 加权组合处理并行目标	⭐⭐⭐
stop_during_grasp	近物体时惩罚底盘运动——防止"边走边抓"导致的精度下降	⭐⭐
Reward 值域平衡	各项 weighted contribution 应大致相等，通过权重而非值域控制重要性	⭐⭐⭐
Reward hacking 检测	独立记录 grasp_success_rate，reward 涨但 metric 不涨 = hacking	⭐⭐⭐
四阶段课程	冻结臂→冻结底盘→全身→DR，hot-start 节省 5000+ iter	⭐⭐⭐
Hot-start 技巧	从 locomotion policy 初始化低层，零初始化新维度	⭐⭐⭐
VBC 分层架构	低层 whole-body tracker + 高层 visual policy，三阶段训练	⭐⭐⭐
VBC Stage 1 training	base_vel + ee_pos 联合跟踪，sigma_ee = 0.05m	⭐⭐
ETH 羽毛球	端到端全身训练 + perception noise model + constrained RL	⭐⭐⭐
感知噪声模型	用真实相机数据标定仿真噪声，涌现主动感知行为	⭐⭐⭐
Constrained RL (ETH)	N-P3O，硬约束=机械臂电流 ≤ 8 A；torque/velocity 为软惩罚（Lagrangian 是通用教学，非 ETH 算法）	⭐⭐
Shuttlecock Predictor (ETH)	EKF + 物理羽毛球动力学模型（L=4.1m），训练与部署复用同一模块（非学习型 MLP）	⭐⭐
涌现行为	精确的仿真环境可以让 RL 发现人类想不到的策略	⭐⭐⭐
双框架 API 映射	obs/reward/action API 一致，主要差异在渲染和模型格式	⭐⭐
视觉 loco-manip	共享 vs 分支编码器；先 state 调通再加视觉	⭐⭐⭐
TensorBoard 解读	nav→reach→grasp 的阶段性收敛模式，各指标的正常范围	⭐⭐

本章建立的三个核心心智模型

模型一：耦合不可忽略。 底盘和臂不是两个独立系统——它们通过 IK、动力学和感知紧密耦合。任何"分离然后拼接"的方案都必须在某种程度上处理这些耦合。VBC 通过分层架构隐式处理（低层自动学会协调），ETH 羽毛球通过端到端训练显式处理。

模型二：渐进式课程。 四阶段课程把"同时学走路和抓东西"分解为"先学走路（Phase 0）→ 再学伸臂（Phase 1）→ 再学边走边抓（Phase 2）→ 最后加鲁棒性（Phase 3）"。每一步只引入一个新挑战，让策略在已有能力基础上扩展。Hot-start 是这个过程的加速器。

模型三：仿真精度决定涌现行为。 ETH 羽毛球的主动感知行为不是被 reward 设计出来的——它从"足够精确的感知噪声模型"中自然涌现。这说明 sim-to-real 的精度不仅影响最终性能，还影响策略能否发现人类工程师想不到的巧妙行为。

累积项目：本章新增模块

累积项目 E 在本章增加"移动操作整合"模块。你应该能够：

加载 Go2-Arx 的组合模型，验证关节、碰撞和 CoM 正确
配置混合动作空间（底盘 velocity + 臂 joint position + 夹爪）
实现四阶段课程训练，每个阶段达到验收标准
训练完整的 mobile reach + grasp 策略，grasp success rate > 50%
对比分层（VBC 模式）和端到端训练的性能差异

完成标准：

验收项	预期结果	验证方法
模型加载	19 个 actuated joints 正确，zero play 不碰撞	可视化 + 打印 model.nu / joint 名称（不是 model.nq，因浮动基座会让 nq 含基座坐标）
Phase 0	velocity tracking error < Ch13 基线 + 20%	TensorBoard
Phase 1	EE position error < 3 cm	TensorBoard
Phase 2	grasp success rate > 50%	独立 metric
Phase 3	success rate > Phase 2 × 70%	DR 下评估

推荐的工程执行顺序（3 周）：

Week 1: 模型与单技能 (Phase 0 + 1)
  Day 1: 下载 awesome-loco-manipulation URDF
         → URDF→MJCF 转换 → 验证 Checklist (10 min)
  Day 2: 配置 Phase 0 环境 → hot-start from Ch13 locomotion
         → 训练 500-1000 iter → 确认 velocity tracking
  Day 3: 配置 Phase 1 环境 → 冻结底盘 → 训练 arm reach
         → 确认 EE error < 3 cm
  Day 4-5: 调试 Phase 0/1 问题，确认两个子技能稳定

Week 2: 联合训练 (Phase 2 + 3)
  Day 1: 配置 Phase 2 环境 → 多目标 reward
         → 从 Phase 1 热启动
  Day 2-3: 训练 Phase 2 (5000-10000 iter)
           → 监控 nav/reach/grasp 的阶段性收敛
  Day 4: 如果 grasp > 50%，进入 Phase 3 (加 DR)
         如果 grasp < 30%，回检 reward 权重和 action scale
  Day 5: Phase 3 训练完成，DR 下评估鲁棒性

Week 3 (可选): 消融实验 + 视觉
  Day 1-2: 消融实验（乘法门控 vs 加权组合）
  Day 3-4: 加入 depth 视觉蒸馏（如果基础管线已调通）
  Day 5: 撰写实验报告

如果 Week 1 Day 2 就卡住了（Phase 0 不收敛）：回到 Ch13 确认底盘策略在纯 locomotion 场景下工作正常。如果纯 locomotion 正常但加了臂后不正常，检查臂的质量是否过大（导致 CoM 偏移超出支撑多边形），或者臂的 PD gain 是否过高（导致臂的微小运动产生大的反作用力矩）。

如果 Week 2 Day 3 后 grasp 仍为零：按 §19.9 的六层诊断流程逐层排查。最常见的三个原因：(1) 夹爪 action 未做阈值化（夹爪半开半合），(2) 物体初始位置不在臂工作空间内（底盘走到了但臂够不到），(3) reaching reward 的 sigma 太大（臂缺乏精确定位的梯度信号）。

延伸阅读

资料	难度	推荐原因
Liu et al. 2024, "Visual Whole-Body Control for Legged Loco-Manipulation" (CoRL Oral)	⭐⭐⭐	分层 loco-manipulation 的标杆工作，代码开源
Ma et al. 2025, "Learning Coordinated Badminton Skills for Legged Manipulators" (Science Robotics)	⭐⭐⭐⭐	端到端全身训练的工程极限，constrained RL + perception noise model
awesome-loco-manipulation (GitHub)	⭐⭐	预组装 URDF 库（Go2-Arx, B1-Z1, Aliengo-Z1）
Gu et al. 2023, "Multi-skill Mobile Manipulation for Object Rearrangement" (ICLR 2023 notable top 25%；方法名 M3)	⭐⭐⭐	多技能移动操作与 region-goal 导航
Sferrazza et al. 2024, "HumanoidBench" (RSS)	⭐⭐	27-task benchmark，层级 RL vs 端到端 RL 的对比
SLIM 2025, "SLIM: Sim-to-Real Legged Instructive Manipulation via Long-Horizon Visuomotor Learning"（arXiv 2501.09905；Go1 + WidowX-250S）	⭐⭐⭐	足式长程移动操作的 sim-to-real，特别是 arm mount perturbation DR
Isaac Lab PickPlace-Locomanipulation-G1 环境	⭐⭐	Isaac Lab 2.3 内置的 loco-manipulation 环境

阅读顺序建议：先读 VBC（理解分层架构的工程实现），再读 ETH 羽毛球（理解端到端训练的极限工程），然后读 M3（理解 reward 设计的通用原则）。awesome-loco-manipulation 作为实验平台的参考。

VBC 论文精读建议：重点关注 Section III-A（低层策略的 hot-start 实现——如何从 12D 输出扩展到 18D 输出且不破坏已有的 locomotion 能力）和 Section III-C（高层到视觉 student 的蒸馏——DAgger 如何在分层架构中工作）。代码中的 whole_body/envs/ 目录包含完整的环境配置（obs、action、reward 的每一个参数），whole_body/algorithms/ 目录包含 ROA（Regularized Online Adaptation）和 DAgger 的实现。建议先阅读 envs/config.py 理解环境配置，再看 algorithms/ppo_roa.py 理解训练逻辑。

ETH 羽毛球论文精读建议：重点关注 perception noise model 的标定方法（如何从真实相机数据回归 detection probability/measurement error 并注入仿真，变量含 shuttle distance、robot/camera 角速度、是否在 FOV 内）和其 constrained RL（N-P3O，硬约束是机械臂电流 ≤ 8 A，torque/velocity 为软惩罚——不是 Lagrangian PPO），以及 EKF + shuttlecock dynamics model（aerodynamic length L=4.1m）的轨迹预测。注意论文的"whole-body asymmetric actor-critic with no architectural split"——这是端到端训练成功的关键，不要误读为"需要分离底盘和臂的网络"。

M3 论文精读建议：M3（"Multi-skill Mobile Manipulation for Object Rearrangement"，ICLR 2023 notable top 25%）的核心贡献是 region-goal navigation reward——不要求机器人精确到达一个点，而是到达一个"区域"就给正 reward。这个思想对 loco-manipulation 很重要——底盘不需要精确到位，只需要让物体进入臂的工作空间即可。重点关注 Section 3.2 的 reward 设计和 Table 2 的消融实验——论文用消融证明了 region-goal 比 point-goal 在移动操作中收敛更快。M3 报告的 cross-configuration 成功率为 71.2%，cross-layout 为 55.0%——这些数值可以作为你自己实验的参考基准。

SLIM 论文精读建议：SLIM（2025，Go1 + WidowX-250S）的独特贡献之一是 arm mount perturbation DR——在 reset 时随机扰动臂的安装位置和朝向。这是一个在其他论文中很少提到但对 sim-to-real 很重要的 DR 维度。论文还讨论了用 PID 而非纯 PD 控制臂——因为纯 PD 在未做重力补偿/积分补偿时会存在重力导致的稳态跟踪误差（并非"任何情况下都有"，加重力补偿或积分项即可消除）。如果你的真机臂使用 PD 控制，这个观察值得注意。

如果时间有限只读一篇：读 VBC。它是最完整的开源 loco-manipulation 框架，代码质量高，工程模式与 mjlab/Isaac Lab 高度兼容。理解了 VBC 的三阶段架构，你就掌握了 loco-manipulation 的核心工程模式。

故障排查手册

症状	可能原因	排查步骤	相关章节
模型加载时关节数不对	URDF→MJCF 转换丢失 fixed joint	1. 打印 model.nu 和 joint/actuator 名称（注意 free joint 对 nq/nv 的贡献） 2. 对比 URDF 的 joint 数量 3. 检查转换日志	19.2
底盘和臂的 PD gain 完全相同	未按关节组分别配置 actuator	1. 打印每个 actuator 的 kp/kd 2. 确认腿组 kp=40 臂组 kp=10-15	19.2
底盘走向物体但方向偏	object_pos 坐标系错误	1. 检查是否用相对坐标 2. 打印 obs 中的物体方向 3. 可视化	19.4
臂不伸出	arm 被冻结 / action scale=0	1. 打印 arm action 值 2. 检查 Phase 2 的 arm_frozen 设置	19.6
臂乱挥（不朝物体方向）	obs 中缺少 object 相对位置 / 坐标系不对	1. 打印 obs group 内容 2. 确认 object_pos_base 在 actor obs 中	19.4
底盘围着物体转圈	navigation reward 的 sigma 太大 / 缺少 stop reward	1. 减小 sigma_nav 到 0.3 2. 添加 stop_during_grasp reward	19.5
底盘冲向物体不停	stop_during_grasp 缺失 / nav reward 权重太大	1. 添加 stop reward 2. 降低 nav reward weight	19.5
伸臂后翻倒	stability reward 太弱 / CoM 偏移过大	1. 增大 stability weight 到 5.0 2. 检查臂质量和安装位置	19.1, 19.5
grasp success 始终为零	夹爪输出未阈值化 / 物体不在可达范围	1. 打印 gripper action 值 2. 打印 EE-to-object 距离	19.5
夹爪始终半开	连续 action 未做 threshold	1. 在 env.step 中加 threshold(action > 0 → close)	19.3
Phase 2 不收敛	hot-start 权重加载失败	1. 打印 Phase 2 初始 reward 是否 > 0 2. 检查维度匹配	19.6
Phase 2 初期大量摔倒	Phase 0 locomotion 不稳定 / 臂初始姿态不对	1. 回到 Phase 0 确认步态稳定 2. 检查 arm 默认位姿	19.6
底盘和臂抖动	action_rate penalty 不足 / scale 失衡	1. 增大 action_rate weight 2. 检查梯度范数比值	19.3
臂梯度远小于底盘梯度	arm action scale 太小 / arm reward weight 太低	1. 用 §19.3 的梯度监控代码诊断 2. 增大 arm scale 或 weight	19.3
reward 涨但 success rate 不涨	reward hacking（快速导航但不抓取）	1. 独立记录 grasp_success_rate 2. 检查乘法门控是否生效	19.5, 19.9
DR 后 success rate 降 > 30%	DR 范围太大	1. 缩小 DR 到原来的 50% 2. 逐项消融确认哪个 DR 影响最大	19.6
arm mount DR 后精度暴降	mount 偏移范围太大（>2cm）	1. 减小到 ±1cm 2. 确认 IK 或策略能补偿此范围的偏移	19.6
端到端训练 20k iter 不收敛	任务太难 / reward 设计不当	1. 切换到四阶段课程 2. 检查 reward 各项的值域是否平衡	19.6, 19.5
VBC Stage 2 高层不学	低层未冻结 / 低层性能不足	1. 确认 low_level.requires_grad=False 2. 检查 Stage 1 EE error	19.7
VBC Stage 3 视觉蒸馏不收敛	depth 预处理不对 / CNN lr 太大	1. 保存 depth 图可视化 2. 降低 CNN lr 到 1e-5	19.10, Ch18
quaternion obs 导致策略不稳定	符号歧义（q 和 -q 交替出现）	1. 加 standardize_quat(w>0) 2. 打印 obs 中 quat 的 w 分量	19.4
constrained RL 策略"什么都不做"	lambda 初始值太大	1. 将 lambda 初始值设为 0 2. 减小 lambda_lr	19.8
预测模型在真机上不准	训练数据未包含真实噪声分布	1. 用真实相机数据标定噪声 2. 重新生成训练数据	19.8

写在最后：本章将 Ch13 的四足 locomotion 和 Ch17 的操作能力融合为一个整体——loco-manipulation。核心工程挑战不是"底盘怎么走"或"臂怎么抓"，而是"两者如何协调"。VBC 的分层架构和 ETH 羽毛球的端到端训练代表了两种截然不同的工程哲学——前者通过分层隐式处理耦合（工程简单但性能有限），后者通过端到端显式学习耦合（工程复杂但性能卓越）。掌握了这两种方法论，你就拥有了设计任何 loco-manipulation 系统的工程基础——无论是四足搬运、人形整理，还是轮式服务机器人。

本章与后续章节的联系：

向后回顾：Ch13 的 velocity tracking 策略在本章成为 Phase 0 的热启动源。Ch17 的 staged reward 设计模式在本章扩展为 navigation → reaching → grasping 的多阶段融合。Ch18 的视觉蒸馏管线在 §19.10 中被整合到 loco-manipulation 的视觉版本。这三章构成了本章的工程基础——如果在本章中遇到困难，回溯到相应的前置章节通常能找到答案。

向前预告：Ch20（人形全身控制）将把 loco-manipulation 的概念扩展到人形机器人——19 DOF 变成 19-29 DOF，稳定性约束更严格（双足比四足更容易翻倒），操作能力更丰富（双臂 + 灵巧手）。本章建立的分层架构模式、四阶段课程和 reward 融合方法将直接迁移到人形平台。Ch21（轮式底盘 + 双臂）是 loco-manipulation 的另一个变体——底盘从四足变成轮式，引入非完整约束（差速轮无法侧移）和底盘漂移补偿的新挑战。Ch22（DIY 实战）将为你提供从零搭建自定义 loco-manipulation 系统的完整指南。

一个总结性的工程经验法则：Loco-manipulation 的 80% 工作量不在"训练策略"上，而在"验证管线的每一环正确"上。在开始任何训练之前，完成以下工程健康检查：

text □ 模型：19 DOF 正确，zero play 不碰撞，CoM 在支撑多边形内 □ 动作：打印每组动作的 scale 值，确认腿>臂>夹爪的顺序 □ 观测：打印 obs 向量，确认物体使用相对坐标（base frame） □ 奖励：random agent 下打印各 reward 项的均值，确认值域平衡 □ 课程：Phase 0 locomotion 稳定后再进入 Phase 1 □ 指标：独立记录 grasp_success_rate，不只看 total reward

这 6 项检查能在训练开始前排除 90% 的配置错误。做了这些检查后再开始训练——这是本章最重要的工程建议。