第16章 灵巧操作案例研究：Sim-to-Real 强化学习与柔顺操控

原课程: Robots That Learn, UC Berkeley CS294-291, Spring 2026 本章基于 Lecture 11 (L11): Dexterous Manipulation Case Studies 精读论文 1: Toru Lin, Kartik Sachdev, Linxi “Jim” Fan, Jitendra Malik, Yuke Zhu — “Sim-to-Real Reinforcement Learning for Vision-Based Dexterous Manipulation on Humanoids,” CoRL 2025 精读论文 2: Hojung Choi, Yifan Hou, Chuer Pan, Seongheon Hong, Austin Patel, Xiaomeng Xu, Mark R. Cutkosky, Shuran Song — “In-the-Wild Compliant Manipulation with UMI-FT,” ICRA 2026

一、本章概要

核心问题: 多指灵巧手 (multi-fingered dexterous hands) 的操控是机器人学中最具挑战性的问题之一。与二指平行夹爪 (parallel-jaw gripper) 不同，灵巧手拥有 6 个以上的自由度，其接触模式 (contact pattern) 的组合爆炸使得手工编程几乎不可能。本章聚焦两个互补的前沿方案：(1) 用 Sim-to-Real 强化学习让仿人机器人从零开始学习灵巧操作，无需人类演示；(2) 用指端力/力矩传感器 (finger-level F/T sensor) 实现柔顺操控 (compliant manipulation)，让机器人安全地处理需要精确力调制的接触密集型任务。

与前后章节的逻辑关系: 第11章 (强化学习) 建立了 MDP、Policy Gradient、PPO 的数学基础。本章将 RL 从理论推向工程实践：Lin et al. 的 “Recipe” 展示了如何系统性地克服 sim-to-real RL 在灵巧操作中的四大挑战 (环境建模、奖励设计、策略学习、视觉迁移)。第13章 (视觉模仿学习) 介绍的 UMI 范式在 Choi et al. 的 UMI-FT 中得到关键扩展——指端力传感使模仿学习从纯运动层面进化到力-运动联合层面。第5章 (人手与灵巧操作) 的人手解剖学知识为理解本章的力觉传感与控制提供了生物学参照。

本章路线图: 我们从阻抗/导纳控制的严格数学模型出发 (§2.1-2.2)，引入力/力矩传感的伴随变换理论 (§2.3)，然后对 sim-to-real gap 进行形式化分解与理论分析 (§2.4)。接着逐篇精读两篇核心论文的方法论与实验发现 (§3.1-3.3)。最后用 PyTorch 代码实现导纳控制器与 sim-to-real RL 训练框架 (§4.1-4.3)。

二、数学基础：阻抗/导纳控制与 Sim-to-Real 分析

2.1 Hogan 阻抗控制 (Impedance Control)

机器人操控的核心困境是力-位置耦合：在自由空间中，纯位置控制表现良好；一旦接触物体，无穷小的位置误差就可产生极大的接触力，导致系统不稳定或物体损坏。Neville Hogan 于 1985 年提出的阻抗控制 [Hogan, 1985] 通过赋予机器人一个可调的机械阻抗来从根本上解决这个问题。

核心思想: 不直接指令力或位置，而是规定机器人末端执行器与环境之间的动态关系——即阻抗 \(Z(s) = F(s)/V(s)\)。机器人在自由空间中表现为纯位置控制 (低阻抗)，在接触时自然过渡为力-位置柔顺行为。

数学模型 (质量-弹簧-阻尼系统，在 SE(3) 任务空间中):

\[\boxed{\mathbf{M}_d(\ddot{\mathbf{x}} - \ddot{\mathbf{x}}_d) + \mathbf{D}_d(\dot{\mathbf{x}} - \dot{\mathbf{x}}_d) + \mathbf{K}_d(\mathbf{x} - \mathbf{x}_d) = \mathbf{F}_{\text{ext}}}\]

其中:

物理直觉:

关节空间实现: 在关节空间中，阻抗控制通过计算力矩控制实现。机器人动力学方程为:

\[\mathbf{H}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau} + \mathbf{J}(\mathbf{q})^T \mathbf{F}_{\text{ext}}\]

其中 \(\mathbf{H}\) 是惯性矩阵，\(\mathbf{C}\) 是 Coriolis 矩阵，\(\mathbf{g}\) 是重力向量，\(\mathbf{J}\) 是雅可比矩阵。将任务空间的期望阻抗行为映射到关节力矩:

\[\begin{aligned} \boldsymbol{\tau} &= \mathbf{H}(\mathbf{q}) \mathbf{J}^{-1}(\mathbf{q}) \Big[\ddot{\mathbf{x}}_d - \mathbf{M}_d^{-1}\big(\mathbf{D}_d \dot{\tilde{\mathbf{x}}} + \mathbf{K}_d \tilde{\mathbf{x}} - \mathbf{F}_{\text{ext}}\big) - \dot{\mathbf{J}}(\mathbf{q})\dot{\mathbf{q}}\Big] \\ &\quad + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) \end{aligned}\]

其中 \(\tilde{\mathbf{x}} = \mathbf{x} - \mathbf{x}_d\) 是位置跟踪误差。该控制律在动力学模型精确的情况下，使末端执行器对外呈现指定的阻抗特性。

2.2 导纳控制 (Admittance Control)

导纳控制 [Maples & Becker, 1986] 是阻抗控制的对偶形式 (dual formulation)，因果关系完全相反。对于高刚度、非反向可驱 (non-backdrivable) 的工业机器人 (如 UR5e)，直接实现阻抗控制在数值上不稳定——因为微小的力矩误差会被高传动比放大为巨大的位置误差。导纳控制采用外环力/内环位置的级联架构来解决此问题。

外环 (导纳环) — 以力为输入，输出柔顺位姿修正:

\[\mathbf{M}_a \ddot{\mathbf{x}}_c + \mathbf{D}_a \dot{\mathbf{x}}_c + \mathbf{K}_a (\mathbf{x}_c - \mathbf{x}_d) = \mathbf{F}_{\text{ext}}\]

以 Euler 离散形式实现 (步长 \(\Delta t = 1 / f_{\text{adm}}\)):

\[\begin{aligned} \ddot{\mathbf{x}}_c[t] &= \mathbf{M}_a^{-1}\big[\mathbf{F}_{\text{ext}}[t] - \mathbf{D}_a \dot{\mathbf{x}}_c[t-1] - \mathbf{K}_a(\mathbf{x}_c[t-1] - \mathbf{x}_d[t])\big] \\ \dot{\mathbf{x}}_c[t] &= \dot{\mathbf{x}}_c[t-1] + \ddot{\mathbf{x}}_c[t] \cdot \Delta t \\ \mathbf{x}_c[t] &= \mathbf{x}_c[t-1] + \dot{\mathbf{x}}_c[t] \cdot \Delta t \end{aligned}\]

内环 (位置环): 将柔顺位姿 \(\mathbf{x}_c[t]\) 作为高带宽位置控制器 (如 PID) 的设定点。UMI-FT 在 UR5e 上使用 500 Hz 的导纳控制外环 + 内嵌的位置控制。

选择阻抗 vs 导纳控制的决策表:

特性 阻抗控制 导纳控制
因果方向 运动 \(\to\) 力 力 \(\to\) 运动
适合的机器人 轻量、反向可驱 (backdrivable) 高刚度、高减速比
力传感器 不必须 (可通过关节力矩估计) 必须在末端安装 F/T 传感器
自由空间表现 优 (精确位置跟踪) 依赖内环位置控制器精度
接触稳定性 依赖精确动力学模型 更稳定 (通过导纳参数主动调节)
接触刚度上限 受限于反馈增益和传感器噪声 可达机器人结构刚度上限

2.3 力/力矩传感的坐标变换 (伴随变换理论)

六轴 F/T 传感器测量的是其本地坐标系 \(\{S\}\) 中的力旋量 (wrench):

\[\mathcal{W}_S = [F_x, F_y, F_z, \tau_x, \tau_y, \tau_z]^T \in \mathbb{R}^6\]

要将其用于机器人工具坐标系 \(\{T\}\) 的控制，需通过伴随变换 (adjoint transformation) 进行坐标变换。这是刚体力学的基本运算，也是 UMI-FT 双指力觉汇总的数学基础 [Choi et al., 2026, Eq. 1]。

设坐标系 \(\{S\}\) 相对于 \(\{T\}\) 的位姿由齐次变换矩阵 \(g_{ST} \in SE(3)\) 给出:

\[g_{ST} = \begin{bmatrix} \mathbf{R} & \mathbf{p} \\ \mathbf{0}^T & 1 \end{bmatrix} \in SE(3)\]

其中 \(\mathbf{R} \in SO(3)\) 是旋转矩阵，\(\mathbf{p} \in \mathbb{R}^3\) 是平移向量。则伴随变换矩阵为:

\[\boxed{\text{Ad}_{g_{ST}} = \begin{bmatrix} \mathbf{R} & \mathbf{0}_{3 \times 3} \\ \hat{\mathbf{p}} \mathbf{R} & \mathbf{R} \end{bmatrix} \in \mathbb{R}^{6 \times 6}}\]

其中 \(\hat{\mathbf{p}}\) 是平移向量 \(\mathbf{p}\) 的反对称矩阵 (skew-symmetric matrix，即叉乘矩阵):

\[\hat{\mathbf{p}} = \begin{bmatrix} 0 & -p_z & p_y \\ p_z & 0 & -p_x \\ -p_y & p_x & 0 \end{bmatrix}\]

使得 \(\hat{\mathbf{p}} \mathbf{v} = \mathbf{p} \times \mathbf{v}\) 对任意 \(\mathbf{v} \in \mathbb{R}^3\)。

力旋量的伴随变换规则 (注意: 与运动旋量相反，力旋量使用 \(\text{Ad}^T\)):

\[\mathcal{W}_T = \text{Ad}_{g_{ST}}^T \cdot \mathcal{W}_S = \begin{bmatrix} \mathbf{R}^T & \mathbf{R}^T \hat{\mathbf{p}}^T \\ \mathbf{0} & \mathbf{R}^T \end{bmatrix} \begin{bmatrix} \mathbf{F}_S \\ \boldsymbol{\tau}_S \end{bmatrix}\]

物理含义: \(\mathbf{F}_T = \mathbf{R}^T \mathbf{F}_S\) (力的方向随坐标系旋转)，\(\boldsymbol{\tau}_T = \mathbf{R}^T \boldsymbol{\tau}_S + \mathbf{R}^T (\mathbf{p} \times \mathbf{F}_S)\) (力矩不仅随坐标系旋转，还因力的作用点平移而产生附加力矩——即杠杆效应)。

在 UMI-FT 中，两个手指的 CoinFT 传感器的 wrench 分别经过伴随变换到工具坐标系 (TCP 设为两指尖中心) 后求和，得到手腕导纳控制的总反馈:

\[\boxed{\mathcal{W}_{\text{wrist}} = \text{Ad}_{S_1 T}^T \mathcal{W}_{S_1} + \text{Ad}_{S_2 T}^T \mathcal{W}_{S_2}}\]

2.4 CoinFT 非线性标定模型

与商用 F/T 传感器 (如 ATI Gamma) 的线性响应不同，CoinFT 由于电容式传感的物理特性 (介质 pillar 变形后电容变化非线性)，需要学习一个非线性映射 [Choi et al., 2025]。标定在传感器安装到 UMI-FT 指尖后原位进行 (in situ calibration)，以确保标定模型捕获完整手指结构的影响。

设 \(\mathbf{c} \in \mathbb{R}^{16}\) 为 16 通道电容读数 (8 个电容通道 \(\times\) 2 个差分方向)，\(\mathbf{w} \in \mathbb{R}^6\) 为目标 wrench。标定通过监督学习完成:

\[\hat{\mathbf{w}} = f_{\boldsymbol{\phi}}(\mathbf{c}) = \mathbf{W}_5 \cdot \sigma(\mathbf{W}_4 \cdot \sigma(\cdots \sigma(\mathbf{W}_1 \mathbf{c} + \mathbf{b}_1) \cdots) + \mathbf{b}_4) + \mathbf{b}_5\]

其中 \(\sigma = \text{ReLU}\)，隐藏层维度为 \([128, 64, 36, 24, 12]\)。训练目标为最小化与参考传感器 ATI Gamma 的均方误差:

\[\mathcal{L}_{\text{calib}}(\boldsymbol{\phi}) = \frac{1}{N} \sum_{i=1}^{N} \|\hat{\mathbf{w}}_i - \mathbf{w}_i^{\text{GT}}\|_2^2\]

标定后的各轴精度 (MSE): \(F_x\): 0.18 N, \(F_y\): 0.15 N, \(F_z\): 0.58 N, \(\tau_x\): 159 mNm, \(\tau_y\): 231 mNm, \(\tau_z\): 17 mNm。

2.5 抓取力控制律

UMI-FT 中的抓取力控制器采用速度分解导纳控制 (velocity-resolved admittance control)，将力误差转化为夹爪速度指令 [Choi et al., 2026, Eq. 2]:

\[\boxed{v_G = K_p (x_G^* - x_G) + K_f (f_G^* - f_G)}\]

其中:

参数调节的物理含义:

2.6 Sim-to-Real Gap 的形式化分析

Sim-to-Real 迁移的核心问题可以形式化为域偏移 (domain shift) 下的策略性能退化。令 \(\eta(\pi) = \mathbb{E}_{\tau \sim p(\cdot\vert \pi)}[\sum_t \gamma^t R_t]\) 为策略 \(\pi\) 在环境分布 \(p\) 下的期望折扣回报，则:

\[\eta_{\text{real}}(\pi) = \eta_{\text{sim}}(\pi) - \Delta(\pi)\]

其中 \(\Delta(\pi)\) 是 sim-to-real gap，可分解为三个独立贡献项 [Lin et al., 2025, Sec III-A/D]:

\[\boxed{\Delta(\pi) = \Delta_{\text{dyn}} + \Delta_{\text{perc}} + \Delta_{\text{emb}}}\]

(1) 动力学 gap (Dynamics Gap): 仿真物理引擎与真实物理的差异。形式化地:

\[\Delta_{\text{dyn}} = \mathbb{E}_{\mathbf{s}_0}\left[D_f\big(p_{\text{sim}}(\mathbf{s}'\vert \mathbf{s}, \mathbf{a}) \;\|\; p_{\text{real}}(\mathbf{s}'\vert \mathbf{s}, \mathbf{a})\big)\right]\]

其中 \(D_f\) 是 \(f\)-divergence。动力学 gap 的物理来源包括:

Lin et al. 通过 autotune 模块将动力学 gap 的 MSE 最小化。消融实验 (Table I) 给出了最直接的证据: 最低 MSE 的参数集对应 8/10 真实成功率，最高 MSE 的参数集对应 0/10 真实成功率——精确的 system identification 是 sim-to-real 成功的必要条件。

(2) 感知 gap (Perception Gap): 仿真渲染与真实相机图像的分布差异:

\[\Delta_{\text{perc}} = \mathbb{E}_{\mathbf{s}, \boldsymbol{\xi}}\big[\|\phi(\mathbf{o}_{\text{sim}}(\mathbf{s}; \boldsymbol{\xi}_{\text{vis}})) - \phi(\mathbf{o}_{\text{real}}(\mathbf{s}))\|_2\big]\]

关键困境: 高维物体表示 (RGB 图像, dense point cloud) 编码更丰富的信息 \(\Rightarrow\) 仿真中策略性能更高，但 sim-to-real gap \(\Delta_{\text{perc}}\) 也更大。低维表示 (3D 质心位置) gap 小但信息不足。Lin et al. 的解决方案是混合表示: 低维 3D 位置 (gap 小) + 高维分割深度图 (信息丰富)，并通过模态特定的数据增强 (深度图: GroupNorm 替代 BatchNorm) 缩小高维侧的 gap。

(3) 本体 gap (Embodiment Gap): 控制信号与实际执行的差异:

\[\Delta_{\text{emb}} = \mathbb{E}\big[\|\mathbf{a}_{\text{cmd}} - \mathbf{a}_{\text{exec}}\|_2\big]\]

来源: PD 增益随机性、关节传感器噪声 (\(\mathcal{N}(0, 0.4)\) rad)、动作延迟 (10% 概率)、帧延迟 (10% 概率)。

域随机化作为鲁棒最优控制: 域随机化可以被理解为在参数分布 \(\Xi\) 上的极小极大 (minimax) 或分布鲁棒优化:

\[\pi^* = \arg\max_{\pi} \min_{\boldsymbol{\xi} \in \Xi} \eta_{\text{sim}}(\pi; \boldsymbol{\xi}) \quad \text{或} \quad \pi^* = \arg\max_{\pi} \mathbb{E}_{\boldsymbol{\xi} \sim \Xi}\big[\eta_{\text{sim}}(\pi; \boldsymbol{\xi})\big]\]

如果真实环境的参数 \(\boldsymbol{\xi}_{\text{real}}\) 落在训练分布的支撑集内 (\(\boldsymbol{\xi}_{\text{real}} \in \text{supp}(\Xi)\))，域随机化后的策略在真实环境中的性能有概率下界保证。Lin et al. 的域随机化配置 (Appendix C, Table IV) 覆盖了质量 [0.03, 0.1] kg、摩擦 [0.5, 1.5]、PD gain \(\times[0.7, 1.2]\)、观测噪声、外力扰动、相机位姿/FOV 等 15+ 个参数维度。

三、论文精读

3.1 论文一: Recipe — 人形机器人灵巧操作的 Sim-to-Real RL

Lin, T., Sachdev, K., Fan, L., Malik, J., Zhu, Y. “Sim-to-Real Reinforcement Learning for Vision-Based Dexterous Manipulation on Humanoids.” CoRL 2025. https://toruowo.github.io/recipe/

3.1.1 问题定义与研究动机

该论文要回答一个驱动性问题: 强化学习为什么尚未在基于视觉的灵巧操作中普及？ 作者识别出灵巧操作与其他 RL 应用领域 (游戏、语言、四足行走) 之间的本质差异:

  1. 高维动作空间: 双臂 14 DoF (7+7) + 双手 12 DoF (6+6) = 26 维连续控制
  2. 接触密集型: 每个任务都涉及手-物体的复杂接触模式，接触模式的组合随手指数量指数爆炸
  3. 视觉依赖: 物体识别、定位、跟踪全部依赖视觉，而视觉本身存在巨大的 sim-to-real gap
  4. 长程任务: 双手交接 (handover) 涉及多个阶段，稀疏奖励下的探索几乎不可能

论文的哲学立场是: 工程化的 sim-to-real RL 配方 (recipe) 足以克服这些困难，关键在于系统性地解决四个子问题 (Fig. 2 的 A-D)。

3.1.2 组件 A — Real-to-Sim 自动调参模块 (Algorithm 1)

传统 real-to-sim 建模中，工程师需要反复手工调整数十个 URDF 参数 (关节刚度、阻尼、惯量、连杆原点、关节限位等)，每次调整后需在真实机器人上采集数据并人工评估。Lin et al. 将这一过程自动化:

算法流程:

  1. 从 3D 模型文件 (URDF) 中提取可调参数空间 \(\mathcal{P}\)
  2. 从 \(\mathcal{P}\) 中随机采样 \(K\) 组参数组合 \(\{\boldsymbol{\phi}_i\}_{i=1}^K\)，初始化 \(K\) 个并行仿真环境
  3. 在真实机器人上运行 \(N\) 条关节目标序列 (正弦扫描等校准轨迹)，记录关节角度跟踪数据 \(R_{\text{track}}\)
  4. 在所有仿真环境中并行运行相同轨迹，记录仿真跟踪数据 \(\{S_{\text{track}}^i\}_{i=1}^K\)
  5. 选择最小化 MSE 的参数组合:
\[\boldsymbol{\phi}^* = \arg\min_{\boldsymbol{\phi}_i} \frac{1}{N \cdot T} \sum_{n=1}^{N} \sum_{t=1}^{T} \|\mathbf{q}_{\text{sim}}^i(t) - \mathbf{q}_{\text{real}}(t)\|_2^2\]

关键工程细节:

3.1.3 组件 B — 通用奖励设计: 接触目标 + 物体目标

从人类操作活动的大量观察中 (引用 Ego-Exo4D 数据集 [Grauman et al., 2024])，Lin et al. 提炼出一个通用模式: 任何灵巧操作任务都可以分解为接触状态 (hand-object contact) 和物体状态 (object state) 的交错序列。

接触标记 (Contact Markers): 在仿真环境中，每个物体表面自动生成一组”接触贴纸” (contact stickers)，代表潜在的期望接触点。接触目标奖励定义为指尖位置与目标接触标记之间距离的平滑反比函数 (Eq. 1):

\[\boxed{r_{\text{contact}} = \sum_{i} \left(\frac{1}{1 + \alpha \cdot d(\mathbf{X}_L, \mathbf{F}_L^i)} + \frac{1}{1 + \beta \cdot d(\mathbf{X}_R, \mathbf{F}_R^i)}\right)}\]

其中 \(d(A, x) = \min_i \|A_i - x\|_2\) (Eq. 2) 是点到点集的最小欧氏距离，\(\mathbf{F}_L, \mathbf{F}_R \in \mathbb{R}^{4 \times 3}\) 是左右手指尖位置，\(\mathbf{X}_L, \mathbf{X}_R\) 是对应接触标记位置，\(\alpha, \beta\) 是缩放超参数控制奖励陡峭度。

该奖励的关键性质: (1) 平滑 — 从 0 连续增大至 1，避免离散奖励的稀疏性问题; (2) 可解释 — 改变标记位置直接改变策略涌现的接触行为 (Fig. 5: 标记在侧面 \(\to\) 双手侧夹，标记在顶部 \(\to\) 顶部下压); (3) 可泛化 — 同一框架适用于 grasp, lift, handover 三种不同性质的任务。

双臂交接任务的阶段奖励 (Eq. 4): 通过阶段变量 \(a \in \{0, 1\}\) 自动切换左右手的接触/物体目标:

\[r = (1 - a) \cdot (r_{\text{contact}}^A + r_{\text{goal}}^A) + a \cdot (r_{\text{contact}}^B + r_{\text{goal}}^B)\]

每个阶段完成时给予递增的 bonus 奖励，激励策略持续推进任务进程。

3.1.4 组件 C — 分治蒸馏 (Divide-and-Conquer Distillation)

对于 26 维动作空间 + 多物体泛化的硬探索问题，Lin et al. 提出两个互补技术:

(a) 任务感知手部姿态初始化: 人类操作员在仿真中用遥操作手柄”随意摆弄” (play around) 任务目标 (每任务少于 30 秒)。收集的关节状态和物体位姿作为 RL 训练的初始状态分布采样源。与传统需完整演示轨迹的方法 [Bauza et al., 2024] 不同，操作员无需完成任务——只需将手放在物体的”附近”即可大幅缩小探索空间。消融实验 (Table II) 显示: 有人类初始化 \(\to\) 抓取 80% vs 60%, 交接 30% vs 0%。

(b) 分治蒸馏: 将多物体任务分解为子任务，训练专精策略后用 Diffusion Policy 蒸馏为通才策略。10 个物体的 grasp-and-reach 任务被分解为:

核心发现 (Fig. 4 + Sec IV-E):

single 虽然训练最快，但 sim-to-real 成功率最低，原因是 过拟合到特定几何形状。mix 在不同形状组之间训练，习得更鲁棒的跨形状特征，实现了训练效率与迁移性能的最佳折中。all 的探索难度最高，连仿真训练收敛都未达到。

蒸馏架构 (Appendix D): 去除了仿真特权信息 (物体速度、角速度、精确摩擦系数)，仅保留真实机器人可获取的观测 (关节状态 + 3D 物体位置 + 自中心深度图)。通才策略使用: ResNet-18 (GroupNorm) 编码深度 \(\to\) 3 层 MLP [512, 512, 512] 编码本体系 + 位置 \(\to\) 拼接 \(\to\) 扩散策略 (100 步, square cosine schedule)，AdamW 优化器 (lr=\(10^{-4}\), weight decay=\(10^{-5}\), batch size=128)。

3.1.5 组件 D — 视觉 Sim-to-Real: 混合物体表示

稀疏特征 (3D CoM Position): 从第三视角 RGB-D 相机 + SAM2 实时追踪获得。首帧由 SAM2 生成分割掩码，其后利用 SAM2 的追踪能力自动传播掩码到所有剩余帧。通过掩码中心的像素坐标和对应深度值恢复近似的 3D 物体质心位置:

\[(x, y, z)_{\text{CoM}} \approx \left(u_{\text{center}} \cdot \frac{d}{f_x}, \; v_{\text{center}} \cdot \frac{d}{f_y}, \; d\right)\]

感知管线以 5 Hz 运行 (匹配策略控制频率)。

稠密特征 (Segmented Depth): 从自中心 (egocentric) RGB-D 相机获取。利用 SAM2 分割掩码去除背景，仅保留物体区域的深度值。使用 ResNet-18 (BatchNorm 替换为 GroupNorm，因 BatchNorm 在 sim-to-real 中因分布偏移导致性能退化) 编码。

消融实验 (Table III) 给出了最具说服力的证据:

Task Pickup (Depth+Pos) Pickup (Depth-only) Full Success (Depth+Pos) Full Success (Depth-only)
Grasp 10/10 2/10 10/10 2/10
Lift 10/10 0/10 10/10 0/10
HandoverA 10/10 0/10 9/10 0/10
HandoverB 10/10 0/10 5/10 0/10

gap 的大小随任务对完整物体几何信息的依赖程度增加而急剧扩大: 简单 pickup 中纯深度仍有 2/10，复杂 handover 中纯深度彻底崩溃 (0/10)。

3.1.6 最终系统能力与批判性审视

成功率 (10 个物体 \(\times\) 10 次试验): grasp-and-reach 62.3%, box lift 80%, bimanual handover 52.5%。

鲁棒性: 学到的策略对多种力扰动 (knock, pull, push, drag) 保持稳定，体现了 RL 从域随机化中习得的泛化能力 (Fig. 6)。

局限性 (Sec V):

  1. 动力学 sim-to-real gap 的动态分量未被充分解决: 仅依赖 naive domain randomization，handover 52.5% 的真实成功率暗示交接中的接触动力学仍未充分建模
  2. 灵巧手硬件的自由度瓶颈: Fourier 手仅 6 主动 DoF，Inspire 手也是 6+6，与人类手的 20+ DoF 有数量级差距
  3. 物体表示假设: 仅用柱体、方块、球等几何基元的随机化训练，对日常物品的泛化性有待验证
  4. 离”通用操作”仍有很大距离: 系统可以处理 3 类任务，但距人类能执行的数百种日常操作任务相去甚远

3.2 论文二: UMI-FT — 在野外学习顺应性操作

Choi, H.*, Hou, Y.*, Pan, C., Hong, S., Patel, A., Xu, X., Cutkosky, M.R., Song, S. “In-the-Wild Compliant Manipulation with UMI-FT.” ICRA 2026. arXiv:2601.09988. https://umi-ft.github.io/

3.2.1 问题定义与研究动机

UMI-FT 提出一个反向的问题: 如果 RL 的高探索成本在实际中难以承受，能否通过增加数据采集的力觉模态来弥补？ 现有手持数据采集装置 (包括最初的 UMI [Chi et al., 2024]) 只记录位姿和视觉——而人类操作的核心特征之一就是力觉反馈 (haptic feedback) 引导的精确力调制。UMI-FT 在每个手指上安装一枚硬币大小的六轴 F/T 传感器 CoinFT，使手持装置能够像人类演示者一样”感受”外部接触力和内部抓取力。

3.2.2 硬件核心: CoinFT 传感器

CoinFT [Choi et al., 2025] 是一种电容式六轴力/力矩传感器:

属性 值 与 ATI Gamma 对比
尺寸 20 mm \(\varnothing\) \(\times\) 3 mm ~75 mm \(\times\) 33 mm
重量 2 g ~250 g
BOM 成本 ~$10 ~$5000+
采样率 360 Hz 最高 7000+ Hz
传感范围 \(F_n\): 25 N, \(F_s\): 20 N, \(\tau\): 500 mNm 可定制
响应特性 非线性 (需 MLP 标定) 近似线性
机械鲁棒性 压缩耐受，拉伸脆弱 各向敏感

关键设计决策: CoinFT 安装在指尖附近以缩短力臂，确保抓取主要产生压缩力 (\(\uparrow\) 耐受)，减少拉伸力矩 (\(\downarrow\) 脆弱的拉伸方向)。原始 UMI 的 fin-ray 手指设计经大幅修改以容纳传感器结构。介质层由椭圆形截面 pillar 构成 (外层长轴 \(\approx\) 4 mm, 短轴 \(\approx\) 0.5 mm)，替代原始设计的圆形 pillar (直径 0.2 mm)，以增强鲁棒性并增大传感范围。

3.2.3 三层控制架构

UMI-FT 的软件架构包含三个频率分层的并行控制回路 (Fig. 3)，从低到高:

回路 1 (~10 Hz) — 学习型视-触-力策略: Adaptive Compliance Policy (ACP) [Hou et al., 2025] 的增强版。输入: 最近 32 帧 CoinFT wrench + 最近 2 帧 RGB/深度/位姿。多模态融合: RGB \(\to\) CLIP ViT-B/32, 深度 (裁剪至 0.5 m, 三通道复制) \(\to\) 相同 ViT, F/T \(\to\) 因果卷积网络, 三者的 token 经 Transformer 自注意力融合, 再与本体感觉拼接 \(\to\) 条件扩散策略。输出 22 维向量:

回路 2 (500 Hz) — 腕部导纳控制器: 6D 任务空间导纳控制，将机器人手臂变为虚拟弹簧-质量-阻尼系统。关键设计: TCP 设为两指尖中心 (而非腕部法兰)，以优化表面擦除等任务中的对齐行为。

回路 3 (30 Hz) — 抓取力控制器: Eq. (2.5) 的速度分解导纳控制。抓取力 \(f_G\) 测量为两指在抓取轴方向力读数的均值。

频率分层的设计理由: 导纳回路 (500 Hz) 处理高频物理交互 (力的高频分量必须在硬件层面响应以保证稳定); 策略回路 (~10 Hz) 负责低频的认知推理 (下一步做什么——受限于视觉编码吞吐量); 抓取力回路 (30 Hz) 位于中间——夹爪硬件本身无法支持更高频率。

3.2.4 实验评估 (三个任务)

任务一: Whiteboard Wiping (擦白板) (275 demos, Table I):

方法 Overall 成功率 关键失败模式
Ours (ACP) 92% —
DP w/ F (有力觉，无顺应) 28% 擦拭不完整，力过大
DP (baseline) 16% 抓取失败 + 力过大
DP w/ Contact Mic 0% 全部因力过大触发安全停止

发现 A.1 — 抓取力调制提高了对未见物体形状的鲁棒性: 当擦子宽度减少 7 mm (训练中未见的形状)，ACP 仍保持 5/5 成功，而所有不含力觉信息的 baseline 在该子任务上全部失败。

发现 A.2 — 接触麦克风的音频信号可以检测接触事件的离散存在性，但不足以用于连续力调制: DP w/ CM 的 0% 成功率 (全部因力过大被安全停止) 清晰证明了这一点。接触麦克风只能报告”有接触”或”无接触”，而无法反馈”接触力是 5 N 还是 25 N”。

任务二: Skewering Zucchini (串西葫芦) (200 demos, Table II):

场景 ACP DP w/ F DP
Normal 5/5 5/5 2/5
Yellow Zucchini (未见颜色) 5/5 4/5 2/5
Thicker 2 cm (更大穿刺力) 3/5 3/5 2/5
Fork (未见工具) 3/5 2/5 0/5
Total 80% 70% 30%

发现 B.1 — 抓取力调控对保持抓取至关重要: DP baseline 的主要失败模式是西葫芦在穿刺过程中滑脱 (线性或旋转滑动)。一旦引入力觉信息 (DP w/ F)，滑脱大幅减少。

发现 B.2 — 顺应控制的增益在此任务上相对较小 (ACP 80% vs DP w/ F 70%): 论文推测原因是西葫芦本身是顺应的 (zucchini compliance)，其自然柔顺性部分替代了机器人级柔顺的功能。

发现 B.3 — 在野数据实现环境泛化 (Table III): ACP + in-the-wild 数据 (630 demos, 15 个场景): 20/20 成功 vs ACP + in-lab 数据: 4/20。场景多样性是泛化的关键推动力。

任务三: Lightbulb Insertion (灯泡插入) (200 demos, Table IV):

场景 ACP DP w/ F DP w/ CM DP
Normal 10/10 4/10 3/10 0/10
Table Height 5/5 5/5 1/5 0/5
Stiffer Socket (20 N) 4/5 3/5 0/5 0/5
Total 95% 60% 20% 0%

发现 C.1 — 顺应控制对触觉搜索 (haptic search) 至关重要: 灯泡插入的核心困难在于视觉遮挡——灯泡本体遮挡了卡榫 (bayonet pin) 与槽口 (slot) 的对齐状态。ACP 策略展现的行为是: (1) 将灯泡推入灯座，保持适度接触力 (~5-10 N); (2) 保持接触的同时旋转灯泡; (3) 当卡榫对齐槽口时，顺应控制自动允许灯泡”掉入”正确位置。这种行为不是被显式编程的——它从人类演示者的顺应性行为模式中涌现 (emerge)。

发现 C.2 — 顺应控制提供了对未建模环境变化的天然鲁棒性: 即使在训练中只见过 15 N 弹簧的灯座，ACP 在面对 20 N 弹簧时仍取得 4/5 成功率——因为导纳控制的下层回路会根据反馈力自动调节插入力，无需策略网络”理解”弹簧刚度的变化。

3.2.5 两篇论文的对比与互补

维度 Recipe (Lin et al.) UMI-FT (Choi et al.)
学习范式 Sim-to-Real RL (PPO, tabula rasa) 模仿学习 + 顺应控制 (ACP + Diffusion Policy)
力觉传感 无显式力传感器 (仿真中通过接触物理隐式学习力) 显式六轴 F/T 传感 (CoinFT, 指端级)
数据来源 零真实数据 (100% 仿真) 真实人类遥操作演示
灵巧手 多指人形手 (6 主动 + 5 欠驱动 DoF) 平行夹爪 + 指端 F/T 传感器
核心挑战 双手协调 + 视觉 sim-to-real gap 接触力调制 + 在野泛化
任务类型 大运动为主 (抓取-搬运-交接) 精细力控为主 (擦除-穿刺-插入)
可扩展性 仿真无限数据，理论上可无限扩展 受限于真实数据采集成本

二者的互补性指向一个令人兴奋的未来方向: Recipe 的仿真训练 + UMI-FT 的顺应执行。策略在仿真中学习任务级行为 (通过 contact sticker 奖励)，然后在真实机器人上用导纳控制作为低层执行器——同时利用少量的真实力觉演示进行 fine-tune。

四、算法与代码实现

4.1 导纳控制器 (PyTorch 完整实现)

"""
admittance_controller.py
===
6D 任务空间导纳控制器与抓取力控制器。
参考: Hogan (1985) — 阻抗控制理论
      Choi et al. (2026) — Sec. IV-A Eq.1-2, UMI-FT 三层架构
      Villani & De Schutter (2016) — Springer Handbook of Robotics Ch.7

架构:
  外环 (导纳): F_ext -> 虚拟弹簧-质量-阻尼 -> Δx
  内环 (位置): 高带宽位置控制器跟踪 x_d + Δx
  抓取力环: f_G 误差 -> 速度分解导纳 -> 夹爪速度指令

控制周期:
  腕部导纳: 500 Hz (UR5e), dt = 0.002 s
  抓取力:   30 Hz (WSG50), dt ≈ 0.033 s
"""

import torch
import torch.nn as nn
from typing import Tuple, Optional


class AdmittanceController(nn.Module):
    """6D 任务空间导纳控制器

    数学: M_d * Δẍ + D_d * Δẋ + K_d * Δx = F_ext
    因果: F_ext (测量) -> Δx (输出) -> 位置控制器跟踪

    Args:
        M_d: 期望惯性矩阵 (6, 6), 正定对称
        D_d: 期望阻尼矩阵 (6, 6), 正定对称
        K_d: 期望刚度矩阵 (6, 6), 正定对称
        dt: 控制周期 (s)
        sensor_positions: 两个传感器在 TCP 坐标系中的位置, shape (2, 3)
        sensor_rotations: 两个传感器在 TCP 坐标系中的旋转, shape (2, 3, 3)
    """

    def __init__(
        self,
        M_d: torch.Tensor,            # (6, 6)
        D_d: torch.Tensor,            # (6, 6)
        K_d: torch.Tensor,            # (6, 6)
        dt: float = 1.0 / 500.0,
        sensor_positions: Optional[torch.Tensor] = None,  # (2, 3)
        sensor_rotations: Optional[torch.Tensor] = None,  # (2, 3, 3)
    ):
        super().__init__()
        self.dt = dt

        # 注册导纳参数为 buffer (随模型自动迁移设备)
        self.register_buffer("M_d", M_d)
        self.register_buffer("D_d", D_d)
        self.register_buffer("K_d", K_d)
        self.register_buffer("M_d_inv", torch.inverse(M_d))

        # 默认: 两指对称放置在 TCP 两侧 0.05 m
        if sensor_positions is None:
            sensor_positions = torch.tensor([
                [0.0,  0.05, 0.0],   # 右手指
                [0.0, -0.05, 0.0],   # 左手指
            ])
        if sensor_rotations is None:
            sensor_rotations = torch.stack([torch.eye(3), torch.eye(3)])

        self.register_buffer("sensor_p", sensor_positions)
        self.register_buffer("sensor_R", sensor_rotations)

        # 内部状态: 柔顺修正的位姿和速度 (运行时维护)
        self.x_c: torch.Tensor = torch.zeros(6)     # 柔顺位姿
        self.v_c: torch.Tensor = torch.zeros(6)     # 柔顺速度
        self.x_g: torch.Tensor = torch.zeros(1)     # 夹爪宽度

    def reset(self, x_init: Optional[torch.Tensor] = None,
              x_g_init: Optional[torch.Tensor] = None):
        """重置内部状态 (每个 episode 开始时调用)"""
        self.x_c = x_init if x_init is not None else torch.zeros(6)
        self.v_c = torch.zeros(6)
        if x_g_init is not None:
            self.x_g = x_g_init

    # ================================================================
    # 伴随变换: 力旋量坐标变换 (Sec 2.3)
    # ================================================================

    @staticmethod
    def skew_symmetric(v: torch.Tensor) -> torch.Tensor:
        """构建 3D 向量的反对称矩阵 [v]×  (叉乘矩阵)

        [v]× = [[0, -vz, vy], [vz, 0, -vx], [-vy, vx, 0]]
        性质: [v]× w = v × w
        """
        zeros = torch.zeros_like(v[..., 0])
        return torch.stack([
            torch.stack([zeros, -v[..., 2], v[..., 1]], dim=-1),
            torch.stack([v[..., 2], zeros, -v[..., 0]], dim=-1),
            torch.stack([-v[..., 1], v[..., 0], zeros], dim=-1),
        ], dim=-2)

    def adjoint_transform(self, R: torch.Tensor,
                          p: torch.Tensor) -> torch.Tensor:
        """构建伴随变换矩阵 Ad_g

        Ad_g = [[R, 0], [skew(p)·R, R]]

        力旋量变换规则 (使用 Ad^T):
          W_T = Ad_g^T · W_S
        """
        top = torch.cat([
            R,
            torch.zeros(*R.shape[:-2], 3, 3, device=R.device, dtype=R.dtype)
        ], dim=-1)
        bottom = torch.cat([
            self.skew_symmetric(p) @ R,
            R
        ], dim=-1)
        Ad = torch.cat([top, bottom], dim=-2)  # (6, 6)
        return Ad.transpose(-1, -2)             # Ad^T

    def transform_wrench_to_tcp(
        self,
        wrench_s1: torch.Tensor,  # (B, 6) 传感器 1 的 [Fx,Fy,Fz,τx,τy,τz]
        wrench_s2: torch.Tensor,  # (B, 6) 传感器 2 的力旋量
    ) -> torch.Tensor:
        """将两个指端力旋量变换并汇总至 TCP (Eq. from Sec IV-A)

        W_wrist = Ad_{S1}^T * W_{S1} + Ad_{S2}^T * W_{S2}
        """
        Ad1_T = self.adjoint_transform(self.sensor_R[0], self.sensor_p[0])  # (6, 6)
        Ad2_T = self.adjoint_transform(self.sensor_R[1], self.sensor_p[1])  # (6, 6)

        w1 = wrench_s1.unsqueeze(-1)   # (B, 6, 1)
        w2 = wrench_s2.unsqueeze(-1)   # (B, 6, 1)

        w_tcp = (Ad1_T @ w1 + Ad2_T @ w2).squeeze(-1)  # (B, 6)
        return w_tcp

    # ================================================================
    # 导纳控制步进
    # ================================================================

    def admittance_step(
        self,
        x_d: torch.Tensor,        # (B, 6) 虚拟目标位姿 (来自 ACP 策略)
        F_ext: torch.Tensor,      # (B, 6) 外部力旋量 (含已变换的 wrench)
        stiffness_scale: float = 1.0,  # 刚度缩放因子 (ACP 输出)
    ) -> torch.Tensor:
        """单步导纳控制前向传播 (Eq. 2.2)

        使用半隐式 Euler 积分:
          a_c[t] = M^{-1}[F_ext[t] - D*v_c[t-1] - s*K*(x_c[t-1] - x_d[t])]
          v_c[t] = v_c[t-1] + a_c[t] * dt
          x_c[t] = x_c[t-1] + v_c[t] * dt

        返回: x_c (B, 6) — 发送给位置控制器的柔顺位姿指令
        """
        # 确保批次维度
        if F_ext.dim() == 1:
            F_ext = F_ext.unsqueeze(0)
        if x_d.dim() == 1:
            x_d = x_d.unsqueeze(0)
        batch_size = F_ext.shape[0]

        # 扩展内部状态到批次维度
        if self.x_c.shape[0] != batch_size:
            self.x_c = self.x_c.expand(batch_size, -1).clone()
            self.v_c = self.v_c.expand(batch_size, -1).clone()

        # 计算柔顺加速度
        position_error = self.x_c - x_d                            # (B, 6)
        spring_force = stiffness_scale * (self.K_d @ position_error.unsqueeze(-1)).squeeze(-1)  # (B, 6)
        damping_force = (self.D_d @ self.v_c.unsqueeze(-1)).squeeze(-1)                       # (B, 6)
        a_c = (self.M_d_inv @ (F_ext - damping_force - spring_force).unsqueeze(-1)).squeeze(-1)  # (B, 6)

        # Euler 积分
        self.v_c = self.v_c + a_c * self.dt
        self.x_c = self.x_c + self.v_c * self.dt

        return self.x_c.clone()

    # ================================================================
    # 抓取力控制 (Eq. 2.4)
    # ================================================================

    def compute_grasp_force(
        self,
        wrench_s1: torch.Tensor,  # (B, 6)
        wrench_s2: torch.Tensor,  # (B, 6)
        grasp_axis: int = 1,      # 抓取轴 (0=x, 1=y, 2=z)
    ) -> torch.Tensor:
        """计算当前抓取力 (两指在抓取轴方向读数的均值)

        f_G = (F_axis_left + F_axis_right) / 2
        """
        f_g1 = wrench_s1[..., grasp_axis]
        f_g2 = wrench_s2[..., grasp_axis]
        return (f_g1 + f_g2) / 2.0

    def grasp_force_control_step(
        self,
        x_g_star: torch.Tensor,    # (B, 1) 目标夹爪宽度 (来自 ACP)
        f_g_star: torch.Tensor,    # (B, 1) 目标抓取力 (来自 ACP)
        f_g: torch.Tensor,         # (B, 1) 当前抓取力 (来自 CoinFT)
        K_p: float = 0.5,          # 位置增益 [UNSPECIFIED]
        K_f: float = 2.0,          # 力增益 [UNSPECIFIED]
    ) -> torch.Tensor:
        """抓取力控制步进 (Eq. 2.4)

        v_G = K_p * (x_G* - x_G) + K_f * (f_G* - f_G)

        返回: v_g (B, 1) — 夹爪速度指令
        """
        if x_g_star.dim() == 0:
            x_g_star = x_g_star.unsqueeze(0)
        if f_g_star.dim() == 0:
            f_g_star = f_g_star.unsqueeze(0)
        if f_g.dim() == 0:
            f_g = f_g.unsqueeze(0)

        position_error = x_g_star - self.x_g
        force_error = f_g_star - f_g
        v_g = K_p * position_error + K_f * force_error

        self.x_g = self.x_g + v_g * self.dt
        return v_g

    def set_admittance_params(
        self,
        M_d: Optional[torch.Tensor] = None,
        D_d: Optional[torch.Tensor] = None,
        K_d: Optional[torch.Tensor] = None,
    ):
        """运行时调整导纳参数 (用于自适应柔顺控制)"""
        if M_d is not None:
            self.M_d.copy_(M_d)
            self.M_d_inv.copy_(torch.inverse(M_d))
        if D_d is not None:
            self.D_d.copy_(D_d)
        if K_d is not None:
            self.K_d.copy_(K_d)

    def get_state(self) -> dict:
        """返回内部状态 (用于日志记录和调试)"""
        return {
            "x_c": self.x_c.clone(),
            "v_c": self.v_c.clone(),
            "x_g": self.x_g.clone(),
        }


# ================================================================
# Sanity Check
# ================================================================
if __name__ == "__main__":
    print("=" * 60)
    print("Admittance Controller Sanity Check")
    print("=" * 60)

    # 初始化: 仅使用平移 3D (简化版, UMI-FT Sec V-A)
    M_d = torch.eye(6) * 1.0
    D_d = torch.eye(6) * 50.0
    K_d = torch.eye(6) * 200.0

    ctrl = AdmittanceController(M_d, D_d, K_d, dt=0.002)
    ctrl.reset()

    # 测试 1: 模拟两个传感器的力旋量 -> TCP 变换
    print("\n[Test 1] Wrench coordinate transformation")
    w_s1 = torch.tensor([5.0, -2.0, 10.0, 0.1, 0.2, 0.05])
    w_s2 = torch.tensor([5.0,  2.0, 10.0, -0.1, 0.2, -0.05])
    w_tcp = ctrl.transform_wrench_to_tcp(w_s1, w_s2)
    print(f"  Sensor 1 wrench: {w_s1.tolist()}")
    print(f"  Sensor 2 wrench: {w_s2.tolist()}")
    print(f"  TCP wrench:      {[f'{v:.2f}' for v in w_tcp.tolist()]}")
    print(f"  Expected: Fx≈10, Fy≈0 (symmetric cancel), Fz≈20 (additive)")
    # 验证: 不对 y 方向的力来源于对称抵消
    assert abs(w_tcp[1].item()) < 1.0, f"Fy should be near 0, got {w_tcp[1]}"

    # 测试 2: 导纳控制步进 — 恒定外力下的稳态响应
    print("\n[Test 2] Admittance step response under constant force")
    ctrl.reset()
    x_d = torch.zeros(6)
    F_ext = torch.tensor([0.0, 0.0, 10.0, 0.0, 0.0, 0.0])  # 10 N 沿 z 轴

    # 运行 1000 步 (2 秒), 观察稳态
    for step in range(1000):
        x_c = ctrl.admittance_step(x_d, F_ext)
    print(f"  After 1000 steps (2 sec) with F_ext=10 N:")
    print(f"  Steady-state position: {[f'{v:.4f}' for v in x_c.tolist()]}")
    # 理论稳态: x_c = F_ext / K (K=200) -> 10/200 = 0.05 m 在 z 方向
    expected_dz = 10.0 / 200.0
    print(f"  Expected Δz = F/K = {expected_dz:.4f} m")
    assert abs(x_c[2].item() - expected_dz) < 0.001, \
        f"Steady-state error: got {x_c[2]:.4f}, expected {expected_dz:.4f}"

    # 测试 3: 抓取力控制
    print("\n[Test 3] Grasp force control")
    f_g = ctrl.compute_grasp_force(w_s1, w_s2)
    print(f"  Grasp force f_G = {f_g.item():.2f} N")
    # 期望: (Fy_left + Fy_right)/2 = (-2.0 + 2.0)/2 = 0.0
    assert abs(f_g.item()) < 0.01, f"Expected f_G ≈ 0 (symmetry), got {f_g.item()}"

    v_g = ctrl.grasp_force_control_step(
        torch.tensor([0.05]),   # 目标宽度 5 cm
        torch.tensor([5.0]),    # 目标抓取力 5 N
        f_g.unsqueeze(0),       # 当前抓取力 ≈ 0
    )
    print(f"  Gripper velocity command: {v_g.item():.4f} m/s")
    # 期望: v_g = 0.5*(0.05 - 0) + 2.0*(5.0 - 0.0) = 0.025 + 10.0 = 10.025
    print(f"  Expected: K_p*Δx + K_f*Δf = {0.5*0.05 + 2.0*5.0:.4f} m/s")

    print("\n" + "=" * 60)
    print("All sanity checks passed!")
    print("=" * 60)

4.2 接触奖励与 Sim-to-Real 域随机化

"""
sim_to_real_policy.py
===
基于 Lin et al. (CoRL 2025) Recipe 的 Sim-to-Real 灵巧操作策略组件。

成分:
  ContactReward       — Eq.1-2: 接触标记奖励
  DomainRandomizer    — Appendix C, Table IV: 域随机化参数管理
  DexterousPolicy     — Appendix D: 通才策略 (深度 + 位置 + 本体系)
"""

import torch
import torch.nn as nn
import torch.nn.functional as F
from typing import Tuple, Optional


# ================================================================
# 接触奖励 (Lin et al. Eq.1-2)
# ================================================================

class ContactReward(nn.Module):
    """基于接触标记 (contact markers) 的奖励函数

    数学 (Eq.1):
      r_contact = Σ_i [1/(1 + α·d(X_L, F_L^i)) + 1/(1 + β·d(X_R, F_R^i))]

      d(A, x) = min_j ||A_j - x||_2  (Eq.2: 点到点集的最小欧氏距离)

    性质:
      - 平滑: 从 0 连续增大至 1 (指尖接近接触标记)
      - 可解释: 标记位置直接决定涌现的接触行为
      - 归一化: 每个标记贡献 [0, 1]，总奖励 ∈ [0, 2N_markers]
    """

    def __init__(self, alpha: float = 10.0, beta: float = 10.0):
        """初始化接触奖励函数

        Args:
            alpha: 左手接触奖励的陡峭度 [UNSPECIFIED exact value]
            beta:  右手接触奖励的陡峭度 [UNSPECIFIED exact value]
        """
        super().__init__()
        self.alpha = alpha
        self.beta = beta

    def forward(
        self,
        left_markers: torch.Tensor,    # (B, N_L, 3) 左手接触标记
        right_markers: torch.Tensor,   # (B, N_R, 3) 右手接触标记
        left_fingertips: torch.Tensor, # (B, M_L, 3) 左手指尖位置
        right_fingertips: torch.Tensor,# (B, M_R, 3) 右手指尖位置
    ) -> Tuple[torch.Tensor, dict]:
        """计算接触奖励

        返回:
          reward: (B,) 接触奖励
          info:   dict 包含左右手分别的奖励 (用于日志)
        """
        # 左侧: 每个标记到最近指尖的距离
        # d_left[b,i,j] = ||left_markers[b,i] - left_fingertips[b,j]||
        d_left = torch.cdist(left_markers, left_fingertips)       # (B, N_L, M_L)
        d_left_min = d_left.min(dim=-1).values                     # (B, N_L)
        r_left = (1.0 / (1.0 + self.alpha * d_left_min)).sum(dim=-1)  # (B,)

        # 右侧: 同理
        d_right = torch.cdist(right_markers, right_fingertips)    # (B, N_R, M_R)
        d_right_min = d_right.min(dim=-1).values                   # (B, N_R)
        r_right = (1.0 / (1.0 + self.beta * d_right_min)).sum(dim=-1)  # (B,)

        reward = r_left + r_right  # (B,)
        info = {
            "r_contact_left": r_left.mean().item(),
            "r_contact_right": r_right.mean().item(),
        }
        return reward, info

    def compute_full_reward(
        self,
        left_markers: torch.Tensor,
        right_markers: torch.Tensor,
        left_fingertips: torch.Tensor,
        right_fingertips: torch.Tensor,
        object_pos: torch.Tensor,    # (B, 3) 当前物体质心
        goal_pos: torch.Tensor,      # (B, 3) 目标物体位置
        w_contact: float = 1.0,
        w_object: float = 1.0,
    ) -> Tuple[torch.Tensor, dict]:
        """完整任务奖励 = w_contact * r_contact + w_object * r_object

        r_object = -||p_object - p_goal||_2
        """
        r_contact, info = self.forward(
            left_markers, right_markers, left_fingertips, right_fingertips
        )
        r_object = -torch.norm(object_pos - goal_pos, dim=-1)  # (B,)
        reward = w_contact * r_contact + w_object * r_object
        info["r_object"] = r_object.mean().item()
        info["r_total"] = reward.mean().item()
        return reward, info


# ================================================================
# 域随机化管理 (Lin et al. Appendix C, Table IV)
# ================================================================

class DomainRandomizer:
    """Sim-to-Real 域随机化参数管理器

    在每个 episode 开始时采样，应用于所有并行仿真环境。

    Table IV 完整参数列表:
      Physical:  mass [0.03,0.1], friction [0.5,1.5], scale [0.95,1.05],
                 hand_friction [0.5,1.5], PD P×[0.8,1.1], PD D×[0.7,1.2]
      Random Force: scale=2.0, prob=0.2, decay=0.99/0.1s
      Observation Noise: obj_pos ±0.02 m, joint N(0,0.4) rad,
                         action N(0,0.1)
      Perception: depth cam pos ±0.005 m, rot ±5°, fov ±5°
      Latency: frame_lag_prob=0.1, action_lag_prob=0.1
    """

    def __init__(self, seed: int = 42):
        self.rng = torch.Generator()
        self.rng.manual_seed(seed)

    def reset_episode(self, batch_size: int = 1) -> dict:
        """每个 episode 开始时采样所有随机化参数

        Returns:
          参数字典, 每项为 (B,) 或 (B, dim) 的张量
        """
        B = batch_size
        return {
            # ---- 物理参数 ----
            "object_mass": torch.rand(B, generator=self.rng) * 0.07 + 0.03,
            "object_friction": torch.rand(B, generator=self.rng) * 1.0 + 0.5,
            "object_scale": 1.0 + (torch.rand(B, generator=self.rng) * 0.1 - 0.05),
            "hand_friction": torch.rand(B, generator=self.rng) * 1.0 + 0.5,
            "pd_p_gain_scale": torch.rand(B, generator=self.rng) * 0.3 + 0.8,
            "pd_d_gain_scale": torch.rand(B, generator=self.rng) * 0.5 + 0.7,
            # ---- 观测噪声 ----
            "obj_pos_noise": torch.randn(B, 3, generator=self.rng) * 0.02,
            "joint_noise": torch.randn(B, 26, generator=self.rng) * 0.4,
            "action_noise_std": 0.1,
            # ---- 外力扰动 ----
            "random_force_prob": 0.2,
            "random_force_scale": 2.0,
            # ---- 相机噪声 ----
            "cam_pos_noise": torch.randn(B, 3, generator=self.rng) * 0.005,
            "cam_rot_noise": torch.randn(B, 3, generator=self.rng) * 5.0,
            "cam_fov_noise": torch.randn(B, 1, generator=self.rng) * 5.0,
            # ---- 延迟 ----
            "frame_lag_prob": 0.1,
            "action_lag_prob": 0.1,
        }


# ================================================================
# 网络组件: 混合物体表示的灵巧操作策略 (Lin et al. Appendix D)
# ================================================================

def replace_bn_with_gn(module: nn.Module, num_groups: int = 8):
    """递归替换 BatchNorm 为 GroupNorm

    原因: BatchNorm 累积的 running mean/var 在 sim-to-real 时因
          分布偏移导致性能退化; GroupNorm 对 batch size 和
          域偏移不敏感 [Wu & He, ECCV 2018].
    参考: Lin et al. Appendix D; Diffusion Policy [Chi et al., 2023]
    """
    for name, child in module.named_children():
        if isinstance(child, nn.BatchNorm2d):
            setattr(module, name, nn.GroupNorm(
                min(num_groups, child.num_features), child.num_features
            ))
        else:
            replace_bn_with_gn(child, num_groups)


class ProprioceptionEncoder(nn.Module):
    """本体感觉 + 物体位置编码器

    架构: 3 层 MLP [512, 512, 512] + ELU -> 64D
    参考: Lin et al. Appendix D

    输入: 关节角度 (14D 双臂) + 手指关节 (12D) + 指尖位置 (24D)
         + 物体 3D CoM (3D) = 53D [UNSPECIFIED: exact dim]
    """

    def __init__(self, input_dim: int = 53, output_dim: int = 64):
        super().__init__()
        self.net = nn.Sequential(
            nn.Linear(input_dim, 512), nn.ELU(),
            nn.Linear(512, 512), nn.ELU(),
            nn.Linear(512, 512), nn.ELU(),
            nn.Linear(512, output_dim),
        )

    def forward(self, x: torch.Tensor) -> torch.Tensor:
        return self.net(x)


class DepthEncoder(nn.Module):
    """深度图编码器: ResNet-18 with GroupNorm

    输入: (B, 1, H, W) 分割深度图
    深度值裁剪: [0.3, 1.5] m (对应 Table IV 域随机化范围)
    """

    def __init__(self, output_dim: int = 512):
        super().__init__()
        import torchvision.models as models
        self.backbone = models.resnet18(weights=None)
        # 单通道深度输入 (替代原始 3 通道 RGB)
        self.backbone.conv1 = nn.Conv2d(
            1, 64, kernel_size=7, stride=2, padding=3, bias=False
        )
        # BatchNorm -> GroupNorm
        replace_bn_with_gn(self.backbone, num_groups=8)
        in_features = self.backbone.fc.in_features
        self.backbone.fc = nn.Linear(in_features, output_dim)

    def forward(self, depth: torch.Tensor) -> torch.Tensor:
        depth = torch.clamp(depth, 0.3, 1.5)
        return self.backbone(depth)


class DexterousPolicy(nn.Module):
    """灵巧操作通才策略 (蒸馏阶段)

    融合: 本体系 (53D) + 深度图 -> 拼接 -> 3 层 MLP -> 动作

    输出: 26D = 双臂关节 (7+7) + 双手手指 (6+6)

    注意: 这是简化版, 论文中的完整实现使用 Diffusion Policy
          作为动作生成器 (100 denoising steps, square cosine schedule).
          Diffusion Policy 的引入减少了蒸馏阶段的 multi-modal 行为问题.
    """

    def __init__(self, proprio_dim: int = 53, depth_out_dim: int = 512,
                 action_dim: int = 26):
        super().__init__()
        self.proprio_encoder = ProprioceptionEncoder(
            input_dim=proprio_dim, output_dim=64
        )
        self.depth_encoder = DepthEncoder(output_dim=depth_out_dim)

        fusion_dim = 64 + depth_out_dim
        self.fusion = nn.Sequential(
            nn.Linear(fusion_dim, 256), nn.ELU(),
            nn.Linear(256, 256), nn.ELU(),
            nn.Linear(256, action_dim),
            nn.Tanh(),  # 动作归一化到 [-1, 1]
        )

        # 动作缩放 (将 [-1, 1] 映射到关节限位)
        # [UNSPECIFIED: 具体的关节限位值]
        self.register_buffer("action_scale", torch.ones(action_dim) * 0.5)

    def forward(self, proprio: torch.Tensor,
                depth: torch.Tensor) -> torch.Tensor:
        f_proprio = self.proprio_encoder(proprio)    # (B, 64)
        f_depth = self.depth_encoder(depth)           # (B, depth_out_dim)
        fused = torch.cat([f_proprio, f_depth], dim=-1)  # (B, fusion_dim)
        action = self.fusion(fused) * self.action_scale
        return action


# ================================================================
# Sanity Checks
# ================================================================
if __name__ == "__main__":
    print("=" * 60)
    print("Sim-to-Real Policy Components: Sanity Checks")
    print("=" * 60)

    # Test ContactReward
    print("\n[Test] ContactReward")
    reward_fn = ContactReward(alpha=10.0, beta=10.0)

    lm = torch.tensor([[[0.0, 0.0, -0.05]]])    # (1, 1, 3) 左侧标记
    rm = torch.tensor([[[0.0, 0.0,  0.05]]])    # (1, 1, 3) 右侧标记
    lt = torch.tensor([[[0.0, 0.0, -0.05]]])    # (1, 1, 3) 指尖正好在标记上
    rt = torch.tensor([[[0.0, 0.0,  0.05]]])

    r, _ = reward_fn(lm, rm, lt, rt)
    print(f"  Perfect contact reward: {r.item():.4f} (expected ~2.0)")

    lt_far = torch.tensor([[[0.1, 0.1, -0.1]]])
    rt_far = torch.tensor([[[-0.1, -0.1, 0.1]]])
    r_far, _ = reward_fn(lm, rm, lt_far, rt_far)
    print(f"  Distant contact reward:  {r_far.item():.4f} (expected << 2.0)")
    assert r_far.item() < r.item(), "Far reward should be less than near reward"

    # Test DomainRandomizer
    print("\n[Test] DomainRandomizer")
    dr = DomainRandomizer(seed=42)
    params = dr.reset_episode(batch_size=3)
    for key in ["object_mass", "object_friction", "pd_p_gain_scale"]:
        print(f"  {key}: {params[key].tolist()}")
    assert 0.03 <= params["object_mass"].min() <= params["object_mass"].max() <= 0.1

    # Test DexterousPolicy forward pass
    print("\n[Test] DexterousPolicy forward pass")
    policy = DexterousPolicy()
    dummy_proprio = torch.randn(2, 53)
    dummy_depth = torch.randn(2, 1, 120, 160)   # [UNSPECIFIED: exact resolution]
    action = policy(dummy_proprio, dummy_depth)
    print(f"  Input: proprio (2,53), depth (2,1,120,160)")
    print(f"  Output: action shape = {action.shape} (expected [2, 26])")
    assert action.shape == (2, 26), f"Wrong output shape: {action.shape}"

    print("\n" + "=" * 60)
    print("All sanity checks passed!")
    print("=" * 60)

五、核心 Takeaway 与延伸阅读

5.1 五个核心洞察

1. Sim-to-Real 是一个系统工程问题，而非单一算法问题。 Lin et al. 使用的基础 RL 算法 (PPO + asymmetric actor-critic) 是成熟的。真正的突破在于系统工程的四个组件 (autotune + 奖励设计 + 分治蒸馏 + 混合表示)。Autotune 消融实验的极端对比 (8/10 vs 0/10) 是最佳证据: 在机器人学中，环境建模的精确度直接决定了学习的上限。

2. 力觉是模仿学习的”缺失维度”。 UMI-FT 的实验证据系统性地证明了: 纯位置/视觉的策略在接触密集型任务中系统性失败 (lightbulb insertion 0%, whiteboard wiping 16%)。力觉模态不仅提供额外的感知信息，更根本性地改变了控制范式——从”预测精确位置”变为”预测期望的力-运动关系 (impedance)”，后者对模型误差和观测噪声具有天然的鲁棒性。

3. 混合表示是连接 sim 和 real 的桥梁。 Lin et al. 的混合物体表示 (3D CoM + 深度图) 与 UMI-FT 的多模态融合 (RGB + 深度 + F/T) 在方法论上形成了深层呼应: 低维特征泛化好但信息不足，高维特征信息丰富但 gap 大。折中方案是在仿真中用高维特征学习、在真实环境中有选择地降维或增强。

4. 频率分层架构是力控的工程共识。 UMI-FT 的三层架构 (500 Hz 导纳 / 30 Hz 抓取力 / 10 Hz 策略) 体现了接触物理的基本约束: 力的高频分量必须在硬件层面 (导纳控制器) 实时响应以保证稳定性; 认知推理 (策略网络) 可以运行在较低的频率。这种分层不是学术偏好，而是接触物理的必然要求。

5. 多样性是泛化的第一性原理。 Recipe 的域随机化表 (15+ 参数维度) 和 UMI-FT 的 in-the-wild 实验 (15 个场景, 20/20 vs 4/20) 从仿真和真实两侧提供了同一哲学的证据: 泛化不是某种神秘的”涌现属性”，而是训练过程中系统性地”见过”多样性的直接产物。

5.2 开放问题与研究方向

  1. 动态 sim-to-real gap: 双臂交接任务 52.5% 的低成功率暴露了 naive domain randomization 在动态接触任务上的不足。系统辨识 (system identification) + 在线自适应 (online adaptation) 可能是突破口。
  2. 灵巧手的硬件瓶颈: 现有机器人手 (6 主动 DoF) 与人手 (20+ DoF) 存在数量级差距。更强的灵巧手将直接提升 RL 策略的上限——但也将指数级增加探索空间和 sim-to-real gap。
  3. Recipe + UMI-FT 融合: 在仿真中用 Recipe 训练策略，在真实机器人上用 UMI-FT 的导纳控制作为低层执行器，同时用少量真实力觉演示 fine-tune——可能是下一代灵巧操作系统的重要方向。
  4. 力觉的 sim-to-real: UMI-FT 的力觉数据来自真实采集。能否在仿真中建模逼真的力觉信号，实现力觉层面的 sim-to-real 迁移？这将解锁力觉 RL 的大规模仿真训练。

5.3 推荐延伸阅读

六、练习与思考

练习 16.1 (阻抗控制的瞬态分析) 考虑一个 3D 平移导纳控制器 (\(M_d = \text{diag}(1, 1, 1)\), \(D_d = \text{diag}(10, 10, 50)\), \(K_d = \text{diag}(100, 100, 200)\))，阶跃外力 \(\mathbf{F}_{\text{ext}} = (0, 0, 10)\) N。设 \(\Delta t = 0.002\) s, \(\mathbf{x}_d = \mathbf{0}\)。 (a) 用微分方程求解稳态 (符号解): \(\mathbf{x}_{ss} = ?\) (b) z 轴的时间常数 \(\tau_z\) 和固有频率 \(\omega_n^z\) 分别是多少？ (c) 若执行 whiteboard wiping 任务，沿接触法向 (z) 和沿擦拭方向 (x) 的刚度应如何设置？给出数值和物理理由。

练习 16.2 (Sim-to-Real Gap 诊断) 某策略在仿真中成功率 85%，真实机器人上 35%。 (a) 按动力学、感知、本体三个维度列举至少 4 种可能的 gap 来源。 (b) 设计一个诊断实验来区分”视觉感知 gap”和”动力学 gap”各自对总 gap 的贡献。实验需要在真实机器人上进行，但不能修改仿真环境。 (c) 如果发现主要 gap 来自动力学，列出至少 3 种缓解策略并按预期效果排序。

练习 16.3 (奖励设计) 针对以下任务，按 Lin et al. 的 “接触目标 + 物体目标” 范式设计奖励函数: (a) 用筷子夹起一块豆腐放入碗中 (豆腐易碎，接触力过大即破坏)。 (b) 单手将笔从桌面旋转至书写姿态 (in-hand reorientation)。 对每个任务: 列出接触状态和物体状态序列，给出至少一个可能的 reward hacking 情形。

练习 16.4 (编程: 导纳控制器) 基于 4.1 节的 AdmittanceController: (a) 在 UR5e 上执行 lightbulb insertion 时，TCP 应设在何处？修改 sensor_positions 并重新计算伴随变换矩阵，验证变换后的 wrench 是否合理。 (b) 如果在手指安装时 CoinFT 传感器存在 5 度的旋转偏差 (绕 x 轴)，对 \(f_G\) 的计算引入多大相对误差？写出修正公式。 (c) 设计并实现一个简单的 safety monitor: 当任意方向的力超过 25 N 或力矩超过 0.5 Nm 时，控制器应输出零速度并发出警告。

练习 16.5 (设计思维) UMI-FT 当前需有线连接 (USB 电缆)。Choi et al. 在 Sec VI 提出了 Bluetooth 无线化的未来方向。 (a) Bluetooth 传输引入的延迟 (~10-30 ms) 对三层控制架构的哪一层影响最大？量化分析。 (b) 如果无法消除延迟，设计一种 Smith predictor 式的补偿策略，并给出其数学形式。 (c) 从产品设计的角度，提出至少两种提高 CoinFT 拉伸耐受性的工程方案，评估其成本和对传感性能的影响。

练习 16.6 (综合设计) 设计一个融合 Recipe 和 UMI-FT 的系统:

请回答: (a) 系统架构框图 (标注信息流和控制频率); (b) 仿真训练中策略的输出应是什么 (参考 UMI-FT 的 22D 输出); (c) fine-tune 阶段哪些参数冻结、哪些继续学习，给出理由。

章节完成时间: 2026-06-19 字数: 约 12,500 字 LaTeX 公式: 40+ 行 代码: PyTorch, 3 个完整模块 (AdmittanceController + ContactReward + DexterousPolicy), 全部含 sanity checks 标注: 所有 [UNSPECIFIED] 参数均已在代码注释中标注默认值与来源

← 第15章 导航案例研究
↑ 目录
第17章 长程规划与语言 →