本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。

D01 导论——双臂任务分类与协调运动学¶

本章定位：双臂操作是机器人从"工具使用者"迈向"通用操作者"的关键一步。本章从"为什么需要双臂"这一根本问题出发，系统建立双臂任务的分类体系、协调运动学的数学框架、以及主流双臂平台的工程对比，为后续 D02（协调规划）、D03（协调力控）、D04（双臂学习）奠定完整的理论与直觉基础。

适用范围：本章的协调运动学理论（增广 Jacobian、相对 Jacobian、绝对/相对变量分解）适用于所有双臂系统——工业双臂、人形机器人双臂、双移动机械臂——是真正的"全方向共享"内容。

前置依赖：M01（Pinocchio 深度精读）、M03（IK 求解器深度）、F01（力控导论）、F02（操作空间动力学）

下游章节：D02（双臂协调规划）、D03（双臂协调力控）、D04（双臂学习）、D09（双臂 MoveIt2 系统集成）

建议用时：2 周（理论 4 天 + 代码实践 4 天 + 综合练习 2 天）

前置自测 ⭐¶

📋 答不出 >= 2 题 → 先回前置章节复习

编号	问题	答不出时回顾
1	雅可比矩阵：对于 7-DOF 机械臂，几何雅可比 $J(q) \in \mathbb{R}^{6 \times 7}$ 的物理含义是什么？当 $\text{rank}(J) < 6$ 时意味着什么？	M01 Pinocchio 深度精读
2	伪逆与零空间：写出右伪逆 $J^+ = J^T(JJ^T)^{-1}$（$J$ 满行秩时即 Moore-Penrose 伪逆）的表达式。$q_{\text{null}} = (I - J^+J)\dot{q}_0$ 为什么满足 $J q_{\text{null}} = 0$？这个性质的工程意义是什么？	M03 IK 求解器深度
3	操作空间动力学：写出操作空间惯量矩阵 $\Lambda = (JM^{-1}J^T)^{-1}$ 的定义。它为什么需要 $J$ 行满秩？当接近奇异位形时 $\Lambda$ 会发生什么？	F02 操作空间动力学
4	SE(3) 与 twist：写出刚体的空间速度 twist $\mathcal{V} = [\omega^T, v^T]^T \in \mathbb{R}^6$。两个 twist 的"差"在 Lie 代数意义下如何计算？	李群基础 / M01
5	阻抗控制：Hogan 阻抗律 $F = M_d\ddot{\tilde{x}} + D_d\dot{\tilde{x}} + K_d\tilde{x}$ 的三个参数分别控制什么物理行为？	F01 阻抗导纳二分法

本章目标¶

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

判断任意操作任务是否需要双臂——使用 Koivo 1988 三分类法和 Krebs 2022 四维分类学进行系统化标签分析，而非凭直觉判断
构造增广雅可比 $J_{\text{aug}} \in \mathbb{R}^{12 \times 14}$ 和相对雅可比 $J_{\text{rel}} \in \mathbb{R}^{6 \times 14}$，理解它们的物理含义和数学结构
运用 Chiacchio 1996 的绝对/相对变量对偶分解，将双臂任务分解为"物体整体运动"和"两臂相对关系"两个独立子问题
推导刚性抓取约束如何将 14 维 C-space 切割为 8 维约束流形，并理解约束流形上规划的数学基础
使用 Pinocchio 在 C++ 和 Python 中为双 Franka Panda（14-DOF）计算完整的协调运动学——$J_{\text{aug}}$、$J_{\text{rel}}$、零空间投影 $P$

D1.1 为什么需要双臂——单臂的根本局限 ⭐¶

动机：一只手能做什么？¶

让我们从一个日常观察开始：你能用一只手拧开瓶盖吗？

答案是"几乎不可能"——因为拧开瓶盖需要同时满足两个条件：（1）固定瓶身使其不旋转；（2）对瓶盖施加扭矩。一只手无法同时完成"约束"和"操作"两个角色。

这不是一个特殊案例，而是揭示了单臂操作的根本局限。让我们系统地分析这些局限。

单臂操作的四大局限¶

局限 1：工作空间与载荷限制

单臂的可达工作空间（reachable workspace）是一个以肩关节为中心、以臂长为半径的近似球壳区域。对于 7-DOF Franka Panda，这个球壳的外径约 0.855m。当需要操作的物体尺寸超过夹爪开口（通常 8-10cm），或质量超过单臂额定载荷（Panda: 3kg），单臂物理上无法完成任务。

单臂工作空间限制：
  Franka Panda:   臂展 0.855m, 载荷 3 kg, 夹爪开口 8 cm
  UR5e:           臂展 0.850m, 载荷 5 kg
  UR10e:          臂展 1.300m, 载荷 12.5 kg

  → 直径 > 8cm 的物体：单臂抓取困难（需特制夹爪或吸盘）
  → 质量 > 3 kg（Panda）/ 5 kg（UR5e）：超出单臂能力
  → 臂展之外的物体：不可达

双臂解决方案：
  → 两臂合力：有效载荷翻倍（2×3kg = 6kg for dual Panda）
  → 协调抓取：可操作任意大小物体
  → 工作空间扩展：两臂覆盖区域的并集远大于单臂

局限 2：操作自由度不足

单臂 7-DOF 控制 6 维末端位姿后，仅剩 1 维零空间冗余——只够做一件"次要任务"（如关节极限回避或奇异回避）。但许多操作任务需要同时控制更多自由度：

任务	所需自由度	单臂能力	缺口
末端定位	6 DOF	6/7 ✅	0
定位 + 夹爪张合	7 DOF	7/7 ⚠️	无冗余
定位 + 工具操作	8-10 DOF	7/7 ❌	1-3 DOF
双手协调操作	12+ DOF	7/7 ❌	5+ DOF

双臂（14-DOF）控制两个末端（12 维）后还有 2 维零空间冗余，可以同时做自碰撞回避和操作度优化。

局限 3：无法施加内力

这是最深层的物理局限。单臂与物体的接触是"开链"（open chain）——末端对物体施加力，物体通过环境（如桌面、夹具）被约束。如果物体没有外部约束（悬空状态），单臂只能推动物体运动，无法对物体施加"挤压力"（internal force）来维持抓取稳定性。

本质洞察：双臂与物体构成"闭链"（closed chain），闭链的本质特性是存在**内力空间**——一组不产生物体运动但维持力封闭的力。这些内力是稳定抓取的基础。单臂开链无法产生内力，因此对悬空物体的操作能力受限于纯摩擦力。

局限 4：缺乏"反作用力参考"

当你用一只手推一扇门时，你的脚提供了地面反作用力。但对于固定在桌面上的单臂机械臂，当它推动一个重物时，反作用力由底座承受。如果底座刚度不够（如安装在移动平台上），推力会导致底座倾斜——这在移动操作（mobile manipulation）中尤为突出。

双臂可以通过一只手固定物体、另一只手操作的方式，实现"自我提供反作用力"——不依赖外部固定装置。这就是为什么人类在做精细操作时几乎总是双手协作。

从单臂到双臂：能力的质变而非量变¶

双臂不是"两个单臂的简单叠加"，而是带来了质的新能力：

新能力	单臂	双臂	本质原因
内力控制	❌	✅	闭链产生内力空间
物体重定向	困难	自然	两点控制 > 一点控制
在手操作	需灵巧手	可用双臂替代	两臂相对运动=旋转
力矩放大	❌	✅	力偶=两力×力臂
自碰撞检测需求	低	高	两臂共享工作空间

跨领域类比：双臂操作 vs 双手弹钢琴。钢琴独奏不只是"两只手各弹一半音符"——双手之间有复杂的时序协调、动态互补、和声构建。同样，双臂操作不只是"两个独立规划的叠加"——协调、约束、内力是全新的维度。

如果不用双臂会怎样——反面教材¶

场景：单臂搬运长杆

让一只 Franka Panda 搬运一根 1.2m 长、3kg 的金属杆。

问题 1：抓取稳定性
  单臂抓取杆的中点 → 杆两端悬臂
  杆转动惯量 J = mL²/12 = 3×1.44/12 = 0.36 kg·m²
  任何加速度 α 产生的扭矩 τ = J·α
  当 α = 1 rad/s²: τ = 0.36 N·m → 夹爪可能打滑

问题 2：碰撞风险
  杆长 1.2m >> 臂展 0.855m
  杆的两端大幅超出臂的灵活操控范围
  → 环境碰撞风险极高

问题 3：姿态控制
  单点抓取无法独立控制杆的绕轴旋转
  → 杆的 roll 角不可控（1 DOF 失控）

双臂方案：
  两臂各抓杆的 1/3 和 2/3 处
  → 有效载荷分摊：各 1.5 kg（远低于单臂极限）
  → 力偶控制：可精确控制杆的所有 6 DOF
  → 内力维持抓取稳定性

⚠️ 常见陷阱¶

💡 概念误区：认为"双臂 = 两个单臂各干各的"
   新手想法："左臂负责左半边任务，右臂负责右半边，各自规划就行"
   实际上：即使两臂操作不同物体（解耦任务），它们共享工作空间，
          必须进行碰撞回避协调。而紧耦合任务（共持物体）中，
          两臂的运动学和力学完全耦合，独立规划会导致物体受到
          不可控的内力（force fight），可能损坏物体或夹爪。
   正确理解：双臂系统是一个 14-DOF 的整体，需要在联合 C-space 中规划。

🧠 思维陷阱：认为"自由度越多越好，双臂一定比单臂更容易"
   新手想法："14-DOF 比 7-DOF 冗余度更高，任务应该更容易完成"
   实际上：更多自由度意味着更高维的 C-space（14 维 vs 7 维），
          采样规划的效率急剧下降（维度灾难）。更多自由度也意味着
          更多的自碰撞可能性——两臂互相碰撞是双臂系统最常见的故障之一。
          高冗余度带来的好处（零空间利用）需要精心的数学框架才能释放。
   正确思维：双臂的优势不在于"自由度多"，而在于"闭链约束带来的新能力"。

练习¶

[基础] 列举 5 个日常双臂任务，分析每个任务中"为什么一只手不够"——是载荷不足、自由度不足、还是需要内力？
[思考] 如果给单臂配备一个能旋转的底座（增加 1 个 DOF 变成 8-DOF），能否替代双臂完成"拧瓶盖"任务？为什么？
[计算] 双 Franka Panda（14-DOF）控制两个末端位姿（12 维）后，零空间维度是多少？如果两臂刚性共持物体（增加 6 维约束），零空间维度变为多少？

D1.2 双臂任务分类法——什么任务需要两只手 ⭐⭐¶

动机：分类的必要性¶

面对一个新的操作任务，工程师需要回答的第一个问题是："这个任务需要几只手？如果需要两只手，两只手之间是什么关系？" 不同的关系决定了完全不同的控制方案——紧耦合任务需要协调力控，松耦合任务只需时序协调，解耦任务只需碰撞回避。

如果不做分类就开始编程，后果是严重的：你可能为一个解耦任务搭建了复杂的协调力控框架（过度设计），或者为一个紧耦合任务只做了独立规划（控制不足导致 force fight）。

反事实推理：如果对所有双臂任务都使用统一的协调控制方案（如全部用 Object Impedance），会怎样？对于解耦任务（左手拿碗、右手拿勺）：物体阻抗控制是多余的，增加了不必要的计算开销，而且由于两个末端之间没有物理耦合，"虚拟弹簧"可能引入不自然的力反馈。对于松耦合任务（折衣服）：刚性内力模型不适用，因为衣物是柔性体，需要变形模型而非刚体模型。

Koivo-Bekey 1988 三分类法¶

这是最早也是最经典的双臂任务分类，至今仍是标准术语的来源。Koivo 和 Bekey 在 1988 年 IEEE Workshop 上提出，基于**两臂之间的物理耦合程度**划分：

分类 1：紧耦合（Tightly Coupled）

定义：两臂通过共持同一刚性物体形成物理闭链
约束：h(q_L, q_R) = 0，6 维等式约束（相对位姿固定）
特征：
  - 两臂运动学耦合：左臂动 → 物体动 → 右臂必须跟随
  - 存在内力空间：挤压/拉伸/扭转/剪切/弯曲
  - 控制需求：协调力控（Object Impedance + 内力管理）

典型任务：
  ① 抬钢板/搬箱子：两手抓住刚性物体两侧
  ② 装配（螺栓+螺母）：一手持螺栓一手拧螺母，形成临时闭链
  ③ 焊接夹持：一手持工件一手持焊枪，工件位姿由持件手精确控制

数学模型：
  T_R(q_R) = T_L(q_L) · T_{LR}
  其中 T_{LR} ∈ SE(3) 是左末端到右末端的固定相对变换
  微分：J_R q̇_R = J_L q̇_L → J_rel · q̇ = 0

分类 2：松耦合（Loosely Coupled）

定义：两臂操作同一物体但不形成刚性闭链
约束：软约束或时序约束，无严格等式约束
特征：
  - 两臂运动有关但不严格锁定
  - 不存在刚性内力（物体可变形或非同时接触）
  - 控制需求：协调规划 + 时序同步 + 独立位控

典型任务：
  ① 折叠衣物：各抓一角，同步运动使衣物对折
  ② 手递手（handover）：一手递出一手接住，有短暂共持阶段
  ③ 拉拉链：一手固定拉链底部，另一手拉拉链头
  ④ 倒水：一手持杯一手持壶，壶嘴对准杯口

数学模型：
  无严格等式约束 h(q_L, q_R) = 0
  有软约束或不等式约束：
    ‖p_L(q_L) - p_R(q_R)‖ ≤ d_max   （两手距离约束）
    t_R - t_L ≤ Δt_max               （时序约束）

分类 3：解耦（Decoupled）

定义：两臂各自操作不同物体，无物理交互
约束：仅需碰撞回避
特征：
  - 两臂运动学独立
  - 唯一的耦合是共享工作空间中的碰撞约束
  - 控制需求：独立规划 + 碰撞回避

典型任务：
  ① 左手拿碗、右手拿筷子（在拿起阶段）
  ② 双线装配：两臂同时在不同位置插入螺栓
  ③ 分拣：两臂同时从传送带拣取不同物体

数学模型：
  独立运动学：ẋ_L = J_L(q_L) q̇_L, ẋ_R = J_R(q_R) q̇_R
  碰撞约束：d(q_L, q_R) ≥ d_min（两臂最近点距离）

三分类法的决策流程：

两臂是否物理接触同一物体？
├── 否 → 解耦（Decoupled）
│       控制方案：独立规划 + 碰撞回避
└── 是 → 物体是否刚性且两臂同时抓持？
    ├── 是 → 紧耦合（Tightly Coupled）
    │       控制方案：协调力控 + 内力管理
    └── 否 → 松耦合（Loosely Coupled）
            控制方案：协调规划 + 时序同步

Krebs-Asfour 2022 四维分类学¶

Krebs 和 Asfour 2022 年在 IEEE RA-L 上提出了更细粒度的分类体系。与 Koivo 的单维度（耦合程度）不同，Krebs 用**四个正交维度**刻画双臂任务——这意味着同一任务在每个维度上独立取值，总共 $2 \times 3 \times 3 \times 3 = 54$ 种组合（去除不合理的组合后约 20-30 种有效组合）。

维度 1——Coordination（时序协调性）

取值	定义	示例
同步（Simultaneous）	两手同时运动	搬箱子
异步（Sequential）	一手先动另一手后动	穿针引线（先穿后拉）

维度 2——Interaction（交互方式）

取值	定义	示例
物理交互	两手通过物体物理耦合	共持刚性物体
空间交互	两手在共享工作空间中避碰	双臂分拣
无交互	两手在独立空间各自操作	左右两台机器分别操作

维度 3——Hand Role（手的角色）

取值	定义	示例
对称（Symmetric）	两手等价，可互换	抬大箱子
主导+辅助（Dominant+Assistive）	一手操作一手扶持/稳定	一手切菜一手按住食材
独立（Independent）	各自执行不同任务	左手搅拌右手加料

维度 4——Symmetry（运动对称性）

取值	定义	示例
镜像对称	两手运动关于中面对称	搓手取暖
平移对称	两手做相同但位移的运动	两手同时按相邻按钮
非对称	两手运动完全不同	拧瓶盖（一手固定一手旋转）

两套分类法的交叉标注实例¶

理解分类法的最佳方式是对具体任务进行标注——发现"同一任务在不同分类法下有不同标签"：

任务	Koivo	Krebs (Coord, Inter, Role, Sym)	分歧与讨论
拧瓶盖	紧耦合	(同步, 物理, 主导+辅助, 非对称)	拧的过程是紧耦合，但松开手的瞬间变为松耦合
折毛巾	松耦合	(同步, 物理, 对称, 镜像)	Koivo 分类为松耦合因为衣物可变形
穿针引线	松耦合	(异步, 物理, 主导+辅助, 非对称)	左手持针（辅助），右手穿线（主导）
抬桌子	紧耦合	(同步, 物理, 对称, 平移)	两人对称用力，是最典型的紧耦合
搅拌（左碗右勺）	解耦→松耦合	(同步, 空间→物理, 主导+辅助, 非对称)	拿起阶段解耦，搅拌阶段勺子在碗里变为物理交互

关键观察：许多真实任务在执行过程中会**在分类之间切换**。拧瓶盖从"紧耦合"到"解耦"（瓶盖拧开后），手递手从"解耦"到"紧耦合"到"解耦"（接近→共持→释放）。这种动态切换是双臂规划中最困难的问题之一，也是 D02 中 TAMP（Task and Motion Planning）要解决的核心问题。

本质洞察：双臂任务分类不是静态标签，而是随操作阶段变化的动态过程。一个完整的双臂操作通常涉及多个"模式"（mode）的切换——每个模式对应不同的运动学约束和控制策略。这种 mode switching 正是双臂操作比单臂更难的根本原因之一。

分类法对控制方案的映射¶

分类不是学术游戏——它直接决定了工程实现的选择：

Koivo 分类	运动学框架	力控策略	规划方法	典型控制频率
紧耦合	$J_{\text{rel}}$ 零空间投影	Object Impedance + 内力	约束流形采样	1 kHz
松耦合	独立 $J_L, J_R$ + 时序协调	独立阻抗 + 力阈值切换	多阶段规划	500 Hz
解耦	独立 $J_L, J_R$	独立位控	独立规划 + 碰撞回避	100-500 Hz

⚠️ 常见陷阱¶

💡 概念误区：认为"Koivo 三分类和 Krebs 四维分类是竞争关系"
   新手想法："Krebs 2022 的分类更新、更好，所以 Koivo 1988 过时了"
   实际上：两套分类法是互补的。Koivo 分类基于物理耦合程度，直接决定
          控制方案的大类选择（力控/位控/混合）。Krebs 分类提供更细粒度的
          任务描述，在学习型双臂控制（D04）中用于构建任务表征。
          实践中通常先用 Koivo 确定控制框架，再用 Krebs 细化参数设计。
   正确做法：两套分类法配合使用，不是非此即彼。

⚠️ 编程陷阱：分类标签硬编码
   错误做法：在控制器中写 if (task == "tightly_coupled") {...}
   现象：无法处理任务类型在执行过程中动态切换的情况
   根本原因：真实双臂任务通常跨越多个分类（如 handover 从解耦→紧耦合→解耦）
   正确做法：用有限状态机（FSM）或 Behavior Tree 管理模式切换，
            每个模式对应一个控制策略。在 D09（MoveIt2 系统集成）中会详细讲解。

练习¶

[分类标注] 对以下 5 个任务用 Koivo 三分类和 Krebs 四维分类法分别标注：①开瓶盖 ②折叠T恤 ③两人搬沙发 ④一手持锅一手翻炒 ⑤剪纸。写出标注过程中的争议点。
[思考] 一个任务是"紧耦合"还是"松耦合"，取决于物体刚度。如果被操作物体的刚度 $k$ 从 $\infty$（完全刚性）连续变化到 0（完全柔性），任务分类如何渐变？在什么刚度阈值下，应该从"紧耦合力控"切换到"松耦合位控"？
[跨章综合] 回顾 F01 中的阻抗控制和导纳控制二分法。紧耦合双臂任务应该用阻抗控制还是导纳控制？提示：考虑两臂之间是否存在"环境"——物体就是双臂之间的"虚拟环境"。

D1.3 运动学约束分类——开链、闭链与虚拟闭链 ⭐⭐¶

动机：约束决定自由度¶

在 D1.2 中我们从"任务"角度分类了双臂操作。现在换到"运动学"角度——两臂之间的几何约束如何影响系统的有效自由度？

这个问题直接决定了运动规划的维度：14-DOF 双臂在约束下的有效 C-space 可能低至 8 维，规划在 8 维流形上比在 14 维自由空间中更高效但更复杂。

开链构型（Open Chain）¶

定义：两臂独立运动，不通过物体形成回路
拓扑：   Base ─── Arm_L ─── EE_L
                     Base ─── Arm_R ─── EE_R
         两条独立的串联链

运动学：
  q = [q_L^T, q_R^T]^T ∈ R^14
  ẋ_L = J_L(q_L) · q̇_L     (6×7)
  ẋ_R = J_R(q_R) · q̇_R     (6×7)

约束：无（或仅碰撞约束 d(q_L, q_R) ≥ d_min）
有效 DOF：14

适用：解耦任务、接近阶段

闭链构型（Closed Chain）¶

定义：两臂通过共持刚性物体形成机械回路
拓扑：   Base ─── Arm_L ─── EE_L ─── Object ─── EE_R ─── Arm_R ─── Base
         形成一个回路（loop）

运动学约束：
  h(q) = log(T_{rel,fixed}^{-1} · T_L(q_L)^{-1} · T_R(q_R))^∨ = 0   ∈ R^6

  其中：
    T_L(q_L), T_R(q_R) ∈ SE(3)：两臂末端的正运动学
    T_{rel,fixed} ∈ SE(3)：右末端相对左末端的固定变换
    log(·)^∨: SE(3) → R^6：对数映射提取 twist 坐标

约束 Jacobian：
  J_h(q) · q̇ = 0   →   J_rel · q̇ = 0
  先把左右末端 twist 变换到同一表达系、同一参考点 p_c：
    J̄_i = X(p_c - p_i) · J_i,  i ∈ {L,R}
  再做差：
    J_rel = J̄_R - J̄_L ∈ R^{6×14}

有效 DOF：14 - 6 = 8（约束流形维度）

这里 $X(d)$ 是同轴表达下的 twist 平移算子。按本文 $[v;\omega]$ 顺序，$X(d)=\begin{bmatrix}I&-[d]_\times\\0&I\end{bmatrix}$，其中 $d=p_c-p_i$。如果左右 Jacobian 来自 LOCAL_WORLD_ALIGNED，它们只是在世界方向上表达，线速度仍分别对应各自末端原点；做闭链/相对位姿约束前仍必须用这个平移项统一参考点。

为什么闭链约束是 6 维？

两个刚体的相对位姿有 6 个自由度（3 平移 + 3 旋转）。刚性共持意味着相对位姿固定——6 个自由度全部被约束，因此有 6 维等式约束。

如果两臂不是刚性共持，而是通过铰链连接（如两臂各抓门的一边），则约束可能少于 6 维——铰链允许 1 个旋转自由度，约束变为 5 维，有效 DOF = 14 - 5 = 9。

虚拟闭链（Virtual Closed Chain）¶

定义：两臂在任务空间中有虚拟约束，但不形成物理回路
特征：约束通过控制器强制施加，而非物理机构

典型场景：
  ① 虚拟弹簧：两末端之间连一根"弹簧"（阻抗控制实现）
     F = K(x_R - x_L - x_{rel,des})
     当 K → ∞ 时退化为刚性闭链

  ② 编队控制：两末端保持固定队形运动（如平行搬运托盘）
     x_rel(t) = x_{rel,des}(t)（时变约束）

  ③ 力约束：两末端合力满足某关系
     f_L + f_R = f_des（力平衡约束）

运动学：
  约束通过控制律而非几何关系实现：
    τ = J^T · f_ctrl + ... 
    f_ctrl 包含约束力项

有效 DOF：取决于约束刚度
  K → ∞：等价于闭链，8 DOF
  K → 0：等价于开链，14 DOF
  中间值：连续过渡

跨领域类比：闭链约束 vs 电路约束。闭链约束（$J_{\text{rel}} \dot{q} = 0$）类似于 Kirchhoff 电压定律（回路电压和为零）——都是因为形成了回路而产生的约束。虚拟闭链类似于电路中的电子反馈——通过控制器（运算放大器）人为建立约束关系，而非物理连接。

三种约束的统一视角：

约束类型	约束方程	约束来源	有效 DOF	鲁棒性
开链	无	—	14	N/A
闭链	$h(q) = 0$	物理接触	8	取决于接触稳定性
虚拟闭链	$h(q) \approx 0$	控制器	8-14（连续）	取决于控制器增益

⚠️ 常见陷阱¶

🧠 思维陷阱：认为"闭链约束是精确的"
   新手想法："两臂共持刚性物体时，J_rel · q̇ = 0 严格成立"
   实际上：由于关节编码器精度（±0.01°）、控制器延迟（~1ms）、
          物体弹性（钢的 Young 模量虽大但不是无穷大），
          闭链约束只是近似满足。实际的约束违反量 ‖h(q)‖ 通常在
          0.1-1mm 级别。如果控制器不主动补偿，累积误差会导致
          内力不可控增长——这就是 D03 中内力控制的动机。
   正确理解：闭链约束是"几乎满足"的，需要控制器主动维护。

练习¶

[推导] 对于双 6-DOF 臂（如双 UR5e，总 DOF=12），刚性共持物体后有效 DOF = 12 - 6 = 6。这意味着零空间维度为 0——两臂没有冗余度做碰撞回避或操作度优化。这在工程上意味着什么？与双 7-DOF 臂的情况对比。
[思考] 如果两臂通过一根弹性杆（而非刚性杆）连接，闭链约束的维度是否还是 6？提示：弹性杆可以变形，两端的相对位姿不再固定。

D1.4 增广 Jacobian——把两只手看作一个 12 维末端 ⭐⭐¶

动机：为什么需要联合描述¶

单臂的雅可比 $J \in \mathbb{R}^{6 \times 7}$ 把关节速度映射到末端速度——这是单臂运动控制的核心工具。对于双臂，我们有两个末端需要同时控制。最直接的做法是把两个末端"拼在一起"，构成一个 12 维的"虚拟末端"。

回顾 M01：Pinocchio 中用 getFrameJacobian() 计算单个末端的雅可比矩阵。对于双臂模型，两个末端的雅可比可以简单地堆叠。

数学定义¶

设双臂系统关节向量 $q = [q_L^T, q_R^T]^T \in \mathbb{R}^{14}$（双 7-DOF），两臂末端 twist 分别为 $\mathcal{V}_L, \mathcal{V}_R \in \mathbb{R}^6$。

单臂雅可比（在联合关节空间中表达）：

\[J_L(q) = \begin{bmatrix} J_{L,\text{local}}(q_L) & 0_{6 \times 7} \end{bmatrix} \in \mathbb{R}^{6 \times 14}\]

\[J_R(q) = \begin{bmatrix} 0_{6 \times 7} & J_{R,\text{local}}(q_R) \end{bmatrix} \in \mathbb{R}^{6 \times 14}\]

注意：当两臂共享同一个 Pinocchio Model（合并 URDF）时，getFrameJacobian 返回的就是 $6 \times 14$ 矩阵，非零列自动对应该臂的关节。

增广 Jacobian：

\[J_{\text{aug}}(q) = \begin{bmatrix} J_L(q) \\ J_R(q) \end{bmatrix} = \begin{bmatrix} J_{L,\text{local}}(q_L) & 0 \\ 0 & J_{R,\text{local}}(q_R) \end{bmatrix} \in \mathbb{R}^{12 \times 14}\]

\[\dot{x}_{\text{aug}} = \begin{bmatrix} \mathcal{V}_L \\ \mathcal{V}_R \end{bmatrix} = J_{\text{aug}} \cdot \dot{q} \in \mathbb{R}^{12}\]

增广 Jacobian 的逆运动学¶

$J_{\text{aug}}$ 是 $12 \times 14$ 矩阵，行数 < 列数，系统是**冗余的**（14 个输入、12 个输出）。逆运动学使用阻尼伪逆：

\[\dot{q} = J_{\text{aug}}^{\dagger} \cdot \dot{x}_{\text{aug}} + (I - J_{\text{aug}}^{\dagger} J_{\text{aug}}) \cdot \dot{q}_{\text{null}}\]

其中阻尼伪逆：

\[J_{\text{aug}}^{\dagger} = J_{\text{aug}}^T (J_{\text{aug}} J_{\text{aug}}^T + \lambda^2 I_{12})^{-1}\]

这里的 $J^\dagger_\lambda$ 是**阻尼最小二乘（DLS）任务滤波器**，用于奇异附近稳定求解，不是严格的 Moore-Penrose 伪逆。若 $\lambda>0$，则

\[N_\lambda = I - J^\dagger_\lambda J\]

一般不满足 $N_\lambda^2=N_\lambda$、$N_\lambda^T=N_\lambda$，也不保证 $JN_\lambda=0$ 精确为零。需要严格零空间投影时，应使用 Moore-Penrose/SVD 截断伪逆；需要奇异鲁棒控制时，使用 DLS 并把验证指标改为 $\|J N_\lambda \dot{q}_0\|$ 或任务残差，而不是幂等性。

阻尼系数的选择：

场景	$\lambda$	理由
远离奇异	0.01	精度优先，最小阻尼
中等奇异	0.05-0.1	精度与稳定性平衡
接近奇异	0.1-0.5	稳定性优先，牺牲精度
自适应	$\lambda = \begin{cases} \lambda_0 \cdot \sqrt{1 - (w/w_0)^2}, & w < w_0 \\ 0, & w \ge w_0 \end{cases}$	Nakamura 自适应阻尼

其中 $w = \sqrt{\det(J_{\text{aug}} J_{\text{aug}}^T)}$ 是操作度（manipulability），$w_0$ 是操作度阈值。当 $w \ge w_0$（远离奇异）时阻尼归零以保证精度。

零空间利用¶

零空间维度：$n - \text{rank}(J_{\text{aug}}) = 14 - 12 = 2$（假设远离奇异，$J_{\text{aug}}$ 满秩）。

这 2 维零空间可用于：

用途 1：关节极限回避

\[\dot{q}_{\text{null}} = k \cdot \nabla \sum_{i=1}^{14} \left(\frac{q_i - q_{\text{mid},i}}{q_{\text{max},i} - q_{\text{min},i}}\right)^2\]

关节越接近极限，梯度越大，零空间速度越强地推动关节远离极限。

用途 2：操作度最大化

\[\dot{q}_{\text{null}} = k \cdot \nabla w(q), \quad w = \sqrt{\det(J_{\text{aug}} J_{\text{aug}}^T)}\]

用途 3：自碰撞回避

\[\dot{q}_{\text{null}} = k \cdot \nabla d_{\min}(q_L, q_R)\]

其中 $d_{\min}$ 是两臂最近点距离——这是双臂独有的零空间任务。

反事实推理：如果不使用零空间自碰撞回避，双臂在狭小工作空间中操作时极易碰撞。在 MuJoCo 仿真中，双 Franka Panda 做独立随机运动时，约 15% 的构型会导致两臂碰撞。加入零空间自碰撞回避后，碰撞率降到 < 1%。

Pinocchio 实现¶

// ============================================================
// 增广 Jacobian 计算——Pinocchio C++ 实现
// 前提：双臂合并为一个 URDF（通过 <joint type="fixed"> 连接）
// ============================================================

#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/parsers/urdf.hpp>

// 1. 加载双臂模型
pinocchio::Model model;
pinocchio::urdf::buildModel("dual_panda.urdf", model);
pinocchio::Data data(model);

// 2. 获取末端帧 ID
const auto left_ee_id = model.getFrameId("panda_left_hand");
const auto right_ee_id = model.getFrameId("panda_right_hand");

// 3. 计算正运动学（必须先调用！否则 Jacobian 基于未初始化数据）
Eigen::VectorXd q = pinocchio::neutral(model);  // 14 维
pinocchio::forwardKinematics(model, data, q);
pinocchio::updateFramePlacements(model, data);

// 4. 计算两臂 Jacobian（LOCAL_WORLD_ALIGNED 坐标系）
Eigen::Matrix<double, 6, Eigen::Dynamic> J_L(6, model.nv);
Eigen::Matrix<double, 6, Eigen::Dynamic> J_R(6, model.nv);
J_L.setZero();
J_R.setZero();

pinocchio::computeFrameJacobian(model, data, q, left_ee_id,
    pinocchio::LOCAL_WORLD_ALIGNED, J_L);   // 6×14

pinocchio::computeFrameJacobian(model, data, q, right_ee_id,
    pinocchio::LOCAL_WORLD_ALIGNED, J_R);   // 6×14

// 5. 增广 Jacobian: 垂直堆叠
Eigen::Matrix<double, 12, Eigen::Dynamic> J_aug(12, model.nv);
J_aug << J_L, J_R;  // 12×14

// 6. 阻尼伪逆
double lambda = 0.05;
Eigen::Matrix<double, 12, 12> JJt = J_aug * J_aug.transpose()
    + lambda * lambda * Eigen::Matrix<double, 12, 12>::Identity();
Eigen::Matrix<double, Eigen::Dynamic, 12> J_aug_pinv
    = J_aug.transpose() * JJt.inverse();  // 14×12

// 7. 阻尼零空间滤波器（不是严格正交投影）
Eigen::MatrixXd N_aug_dls = Eigen::MatrixXd::Identity(model.nv, model.nv)
    - J_aug_pinv * J_aug;  // 14×14

// ============================================================
// ❌ 常见错误 1：忘记调用 forwardKinematics
// ============================================================
// pinocchio::forwardKinematics(model, data, q);  // 被注释掉了！
pinocchio::computeFrameJacobian(model, data, q, left_ee_id,
    pinocchio::LOCAL_WORLD_ALIGNED, J_L);
// → J_L 基于上一次 FK 的结果（可能是零初始化），数值完全错误
// → 不会报错！只是 Jacobian 值不对，导致 IK 发散

// ============================================================
// ❌ 常见错误 2：使用错误的参考坐标系
// ============================================================
pinocchio::computeFrameJacobian(model, data, q, left_ee_id,
    pinocchio::LOCAL, J_L);  // LOCAL 坐标系！
pinocchio::computeFrameJacobian(model, data, q, right_ee_id,
    pinocchio::LOCAL_WORLD_ALIGNED, J_R);  // WORLD_ALIGNED 坐标系！
// → 两个 Jacobian 在不同坐标系下，直接堆叠后的 J_aug 物理含义错误
// → 解出的 q̇ 会让两臂向错误的方向运动

// ✅ 正确做法：统一使用 LOCAL_WORLD_ALIGNED
// 原因：LOCAL_WORLD_ALIGNED 使线速度和角速度都在世界系方向上表达。
//       注意：它不会把线速度参考点统一到同一个原点。做 J_aug 堆叠没问题，
//       但做相对/闭链 Jacobian 时，还要用 adjoint/平移项把 twist 移到同一参考点。

增广 Jacobian 实战：Python 快速验证¶

C++ 代码用于实时控制，但开发调试阶段用 Python 更高效。以下 Pinocchio Python 代码可直接用于验证 $J_{\text{aug}}$ 的正确性：

# ============================================================
# 增广 Jacobian——Pinocchio Python 实战验证
# ============================================================
import pinocchio as pin
import numpy as np

# 加载双臂模型
model = pin.buildModelFromUrdf("dual_panda.urdf")
data = model.createData()

# 帧 ID
left_ee = model.getFrameId("panda_left_hand")
right_ee = model.getFrameId("panda_right_hand")

# 随机构型
q = pin.randomConfiguration(model)
pin.forwardKinematics(model, data, q)
pin.updateFramePlacements(model, data)
pin.computeJointJacobians(model, data, q)

# 计算两臂 Jacobian
J_L = pin.getFrameJacobian(model, data, left_ee,
                            pin.LOCAL_WORLD_ALIGNED)  # 6×14
J_R = pin.getFrameJacobian(model, data, right_ee,
                            pin.LOCAL_WORLD_ALIGNED)  # 6×14

# 增广 Jacobian
J_aug = np.vstack([J_L, J_R])  # 12×14

# 验证 1：秩（远离奇异时应为 12）
U, S, Vt = np.linalg.svd(J_aug)
rank = np.sum(S > 1e-6)
print(f"rank(J_aug) = {rank}, 零空间维度 = {14 - rank}")
# 期望输出: rank = 12, 零空间维度 = 2

# 验证 2：阻尼伪逆 + 任务滤波器
lam = 0.05
J_pinv = J_aug.T @ np.linalg.inv(J_aug @ J_aug.T
                                   + lam**2 * np.eye(12))
N = np.eye(14) - J_pinv @ J_aug

# 验证 3：DLS 只近似抑制任务速度，不是精确零空间
dq_null = N @ np.random.randn(14)
V_aug = J_aug @ dq_null
print(f"‖J_aug · q̇_filtered‖ = {np.linalg.norm(V_aug):.2e}")
# 期望输出：小于未滤波速度的任务范数，但不会达到 1e-15。

# 若要验证严格零空间，使用 SVD/Moore-Penrose 伪逆：
J_mp = np.linalg.pinv(J_aug, rcond=1e-6)
P_mp = np.eye(14) - J_mp @ J_aug
print(f"‖J_aug · P_mp‖ = {np.linalg.norm(J_aug @ P_mp):.2e}")
# 期望输出: ≈ 1e-12（取决于模型尺度与 SVD 阈值）

工程建议：开发流程为「Python 原型 → 数值验证 → C++ 移植」。Python 脚本可以在几分钟内验证数学公式正确性，避免在 C++ 编译-调试循环中浪费时间。

⚠️ 常见陷阱¶

💡 概念误区：认为"J_aug 是分块对角的"总是成立
   新手想法："左臂 Jacobian 只涉及 q_L，右臂只涉及 q_R，所以 J_aug 是分块对角"
   实际上：这只在两臂关节独立时成立。如果两臂安装在同一移动平台上（如 PR2），
          平台运动影响两臂末端位姿，此时 J_L 和 J_R 都包含平台关节列——
          J_aug 不再是分块对角的。
   正确理解：分块对角是固定基座双臂的特殊情况。对于移动双臂或人形双臂，
            必须使用完整的 Jacobian。

⚠️ 编程陷阱：J_aug 维度不匹配
   错误做法：J_aug 声明为 Matrix<double, 12, 14> 但模型是 13-DOF（6+7 不对称双臂）
   现象：编译通过但运行时 Eigen assertion 失败，或更糟——静默产生错误结果
   正确做法：使用动态大小 MatrixXd 或从 model.nv 获取列数

练习¶

[计算] 对于双 Franka Panda（14-DOF），$J_{\text{aug}}$ 是 $12 \times 14$。如果两臂都处于奇异位形（如完全伸直），$\text{rank}(J_{\text{aug}})$ 可能降到多少？零空间维度相应变为多少？
[编程] 用 Pinocchio Python 绑定计算双 Franka Panda 的 $J_{\text{aug}}$。绘制操作度 $w = \sqrt{\det(J_{\text{aug}} J_{\text{aug}}^T)}$ 随 $q_1$（左臂第一关节）变化的曲线。找到 $w$ 最大和最小的构型。

D1.5 相对 Jacobian——描述两臂相对运动 ⭐⭐¶

动机：从绝对到相对¶

$J_{\text{aug}}$ 描述了两臂末端在世界系中的绝对运动。但在紧耦合任务中，我们更关心的不是"左手在世界系中移动了多少"，而是"右手相对于左手运动了多少"——因为这直接决定了共持物体是否被挤压或拉伸。

这就是**相对 Jacobian** $J_{\text{rel}}$ 的动机：它把关节速度映射到两臂末端的相对 twist。

数学推导¶

在世界系中，两臂末端的速度分别为：

\[\mathcal{V}_L = J_L(q) \cdot \dot{q}, \quad \mathcal{V}_R = J_R(q) \cdot \dot{q}\]

但这一步有一个容易被忽略的前提：两个 twist 必须在同一表达系、同一参考点上表达。LOCAL_WORLD_ALIGNED 只统一了坐标轴方向，并没有把线速度的参考点从各自末端移动到同一个点。刚体转动时，两端线速度本来就不同，直接做 $J_R-J_L$ 会把正常的绕物体转动误判成相对运动。

选择一个公共参考点 $p_c$（常用物体质心或左末端原点），先把两个 twist 平移到 $p_c$：

\[\bar{\mathcal{V}}_i(p_c) = X(p_c-p_i)\mathcal{V}_i,\quad X(d)=\begin{bmatrix}I_3&-[d]_\times\\0&I_3\end{bmatrix},\quad i\in\{L,R\}\]

其中本文 twist 顺序为 $[v;\omega]$。如果使用 $[\omega;v]$ 顺序，矩阵块的位置和符号需要相应置换。

于是：

\[\bar{J}_i(q) = X(p_c-p_i)J_i(q)\]

定义相对 twist 为右末端相对左末端的同点差：

\[\mathcal{V}_{\text{rel}} = \bar{\mathcal{V}}_R - \bar{\mathcal{V}}_L = (\bar{J}_R - \bar{J}_L) \cdot \dot{q} = J_{\text{rel}} \cdot \dot{q}\]

因此：

\[J_{\text{rel}} = \bar{J}_R - \bar{J}_L \in \mathbb{R}^{6 \times 14}\]

只有在两个 Jacobian 已经表达在同一参考点（或任务只看两个点的世界系位置差、且不把它当成完整 SE(3) 相对 twist）时，才可以把它简写成 $J_R-J_L$。闭链刚性抓取和相对位姿约束不能省略这一步。

关键性质：

性质	表达式	物理含义
刚性约束	$J_{\text{rel}} \cdot \dot{q} = 0$	关节速度使两末端相对位姿不变
零空间	$\ker(J_{\text{rel}}) \subset \mathbb{R}^{14}$	满足闭链约束的关节速度集合
维度	$\dim(\ker(J_{\text{rel}})) = 14 - 6 = 8$	约束流形的切空间维度

物理含义的深入理解¶

$J_{\text{rel}}$ 的零空间有一个优美的物理含义：零空间中的关节速度使两臂末端"同步运动"——如同它们被一根无限刚性的杆连接。

反过来，$J_{\text{rel}}$ 的行空间中的关节速度会改变两末端的相对位姿——如果两臂正在共持刚性物体，这些速度会产生内力（挤压/拉伸）。

\[\underbrace{J_{\text{rel}} \cdot \dot{q} \neq 0}_{\text{相对运动}} \Longrightarrow \underbrace{\text{内力变化}}_{\text{挤压/拉伸/扭转}}\]

这就是为什么在刚性共持任务中，关节速度必须被投影到 $\ker(J_{\text{rel}})$ 中——否则会产生不可控的内力。

本质洞察：$J_{\text{rel}}$ 的零空间是"安全运动空间"——在这个空间中运动不会改变两臂的相对关系，因此不会产生额外内力。$J_{\text{rel}}$ 的行空间是"危险运动空间"——任何在这个空间中的运动分量都会改变相对位姿，对刚性共持物体产生力。控制器的任务就是：让有用的运动尽量在零空间中，让必要的相对运动（如打开/关闭夹爪距离）通过力控管理。

零空间投影矩阵¶

\[P(q) = I_{14} - J_{\text{rel}}^+(q) \cdot J_{\text{rel}}(q) \in \mathbb{R}^{14 \times 14}\]

这里 $J_{\text{rel}}^+$ 指 Moore-Penrose 伪逆或等价的 SVD 截断伪逆。只有在这个前提下，$P$ 才是正交投影矩阵。

Moore-Penrose 投影矩阵的性质：

\[P^2 = P \quad (\text{幂等性：投影两次等于投影一次})\]

\[P^T = P \quad (\text{对称性：正交投影})\]

\[J_{\text{rel}} \cdot P = 0 \quad (\text{投影后的速度满足约束})\]

如果把 $J_{\text{rel}}^+$ 换成阻尼伪逆 $J_{\text{rel},\lambda}^\dagger$，则 $P_\lambda=I-J_{\text{rel},\lambda}^\dagger J_{\text{rel}}$ 只是**阻尼任务滤波器**。它牺牲严格投影性质换取奇异附近的数值稳定性，工程自检应关注 $\|J_{\text{rel}}P_\lambda\dot{q}_0\|$ 是否足够小，而不是要求 $P_\lambda^2=P_\lambda$ 或 $P_\lambda^T=P_\lambda$。

验证方法（工程中必做的 sanity check）：

// 验证 Moore-Penrose/SVD 投影 P 的性质
Eigen::MatrixXd P = Eigen::MatrixXd::Identity(14, 14)
    - J_rel_pinv * J_rel;

// 1. 幂等性: P*P ≈ P
double idempotent_err = (P * P - P).norm();
assert(idempotent_err < 1e-10);

// 2. 约束满足: J_rel * P ≈ 0
double constraint_err = (J_rel * P).norm();
assert(constraint_err < 1e-10);

// 3. 零空间维度: rank(P) = 8
Eigen::JacobiSVD<Eigen::MatrixXd> svd(P);
int rank = (svd.singularValues().array() > 1e-6).count();
assert(rank == 8);  // 零空间是 8 维的

// 如果使用阻尼伪逆，只验证任务泄漏，而不验证幂等/对称。
Eigen::VectorXd dq0 = Eigen::VectorXd::Random(14);
Eigen::VectorXd dq_filtered = P_dls * dq0;
double leakage = (J_rel * dq_filtered).norm();
assert(leakage < 1e-3);  // 阈值按任务速度尺度设置

协调逆运动学¶

在刚性共持任务中，我们想控制物体整体运动（通过绝对变量 $\dot{x}_{\text{abs}}$），同时保持相对位姿不变（$\dot{x}_{\text{rel}} = 0$）：

\[\dot{q} = P \cdot J_{\text{abs}}^+ \cdot \dot{x}_{\text{abs}}\]

这个公式做了两件事： 1. $J_{\text{abs}}^+ \cdot \dot{x}_{\text{abs}}$：计算实现物体运动所需的关节速度 2. $P \cdot (\ldots)$：投影到 $\ker(J_{\text{rel}})$，确保不改变相对位姿

⚠️ 常见陷阱¶

💡 概念误区：认为"J_rel 的定义中减号的位置无所谓"
   新手想法："J_rel = J̄_R - J̄_L 和 J_rel = J̄_L - J̄_R 效果一样"
   实际上：符号约定确实是可以选择的，但必须全局一致。
          如果定义 J_rel = J̄_R - J̄_L（右减左），则 v_rel > 0 意味着
          右手远离左手——这是"拉伸"。如果定义 J_rel = J_L - J_R，
          则 v_rel > 0 意味着左手远离右手——物理含义相反。
          Chiacchio 1996 和 Caccavale 2008 使用右减左的约定。
          本文档统一使用此约定。
   正确做法：选择一个约定并在整个代码库中严格遵守。
            在代码注释中明确写出符号约定。

⚠️ 编程陷阱：伪逆计算中的数值问题
   错误做法：J_rel_pinv = J_rel.transpose() * (J_rel * J_rel.transpose()).inverse()
   现象：当 J_rel 接近奇异时（如两臂末端共线），
        J_rel * J_rel^T 的条件数 > 10^10，逆矩阵数值不稳定
   正确做法：使用阻尼伪逆或 SVD 伪逆
     // SVD 伪逆（最稳定）
     Eigen::JacobiSVD<MatrixXd> svd(J_rel, ComputeThinU | ComputeThinV);
     svd.setThreshold(1e-4);  // 截断小奇异值
     J_rel_pinv = svd.solve(MatrixXd::Identity(6, 6));

练习¶

[验证] 用 Pinocchio 计算双 Franka Panda 的 $J_{\text{rel}}$，然后分别计算 Moore-Penrose 投影 $P = I - J_{\text{rel}}^+ J_{\text{rel}}$ 与阻尼滤波器 $P_\lambda=I-J_{\text{rel},\lambda}^\dagger J_{\text{rel}}$。对前者验证 $P^2=P$、$J_{\text{rel}}P=0$、$\text{rank}(P)=8$；对后者只验证随机 $\dot{q}_0$ 下的 $\|J_{\text{rel}}P_\lambda\dot{q}_0\|$ 是否小于任务容差。
[实验] 在 MuJoCo 中让双 Franka 共持一根刚性杆。分别用（a）独立位控（各臂独立跟踪目标）和（b）投影到 $J_{\text{rel}}$ 零空间的协调位控。记录杆两端的相对位移。预测：（a）会积累 mm 级漂移，（b）保持 < 0.1mm。

D1.6 绝对/相对变量对偶——协调控制的标准语言 ⭐⭐⭐¶

动机：为什么需要变量分解¶

在 D1.4 和 D1.5 中，我们分别定义了 $J_{\text{aug}}$（控制两臂绝对位姿）和 $J_{\text{rel}}$（描述两臂相对运动）。Chiacchio, Chiaverini 和 Siciliano 在 1996 年提出了一个更优雅的框架：将双臂运动分解为"绝对变量"和"相对变量"——物理上对应"物体整体运动"和"两手相对关系"。

这个分解的深刻之处在于：它把一个 12 维的耦合问题分解为两个 6 维的（近似）解耦问题——绝对变量控制物体运动，相对变量控制抓取稳定性。两者可以独立设计控制器。

Chiacchio 1996 定义¶

位姿层面的定义（位置部分）：

\[x_{\text{abs}} = \frac{x_L + x_R}{2} \quad \text{（绝对位置：两末端的中点，近似物体质心）}\]

\[x_{\text{rel}} = x_R - x_L \quad \text{（相对位置：右末端到左末端的向量）}\]

姿态层面的定义（使用四元数的对数映射）：

\[R_{\text{abs}} = R_L \cdot \exp\left(\frac{1}{2} \log(R_L^T R_R)\right) \quad \text{（两姿态的测地线中点）}\]

\[R_{\text{rel}} = R_L^T R_R \quad \text{（右姿态相对于左姿态）}\]

速度层面的定义：

速度层面的绝对/相对分解同样要求统一 twist 参考点。令 $\bar{J}_L,\bar{J}_R$ 表示已经通过 adjoint/平移项移动到同一公共点 $p_c$ 的 Jacobian，则：

\[\dot{x}_{\text{abs}} = \frac{\bar{\mathcal{V}}_L + \bar{\mathcal{V}}_R}{2} = \frac{\bar{J}_L + \bar{J}_R}{2} \cdot \dot{q} = J_{\text{abs}} \cdot \dot{q}\]

\[\dot{x}_{\text{rel}} = \bar{\mathcal{V}}_R - \bar{\mathcal{V}}_L = (\bar{J}_R - \bar{J}_L) \cdot \dot{q} = J_{\text{rel}} \cdot \dot{q}\]

因此：

\[J_{\text{abs}} = \frac{\bar{J}_L + \bar{J}_R}{2} \in \mathbb{R}^{6 \times 14}\]

\[J_{\text{rel}} = \bar{J}_R - \bar{J}_L \in \mathbb{R}^{6 \times 14}\]

与同点化增广 Jacobian 的关系¶

绝对/相对变量与"已经统一参考点的增广变量"之间存在精确的线性变换：

\[\begin{bmatrix} J_{\text{abs}} \\ J_{\text{rel}} \end{bmatrix} = \underbrace{\begin{bmatrix} \frac{1}{2}I_6 & \frac{1}{2}I_6 \\ -I_6 & I_6 \end{bmatrix}}_{T} \cdot \begin{bmatrix} \bar{J}_L \\ \bar{J}_R \end{bmatrix}\]

逆变换：

\[\begin{bmatrix} \bar{J}_L \\ \bar{J}_R \end{bmatrix} = \begin{bmatrix} I_6 & -\frac{1}{2}I_6 \\ I_6 & \frac{1}{2}I_6 \end{bmatrix} \cdot \begin{bmatrix} J_{\text{abs}} \\ J_{\text{rel}} \end{bmatrix}\]

即：$\bar{J}_L = J_{\text{abs}} - \frac{1}{2}J_{\text{rel}}$，$\bar{J}_R = J_{\text{abs}} + \frac{1}{2}J_{\text{rel}}$。

跨领域类比：绝对/相对变量 vs 共模/差模信号。在电子工程中，差分放大器接收两个输入 $V_+, V_-$，将它们分解为共模信号 $V_{\text{CM}} = (V_+ + V_-)/2$ 和差模信号 $V_{\text{DM}} = V_+ - V_-$。共模信号是噪声（需要抑制），差模信号是有用信号（需要放大）。

双臂协调中的对偶关系完全类似： - $x_{\text{abs}}$ = 共模 = 物体整体运动（需要精确跟踪） - $x_{\text{rel}}$ = 差模 = 两手相对关系（刚性共持时需要抑制为零）

进一步地，差分放大器的共模抑制比（CMRR）类比于双臂控制器抑制相对运动的能力——CMRR 越高，抗干扰能力越强；$K_{\text{rel}}$（相对刚度）越大，内力控制越精确。

刚性共持下的简化¶

当两臂刚性共持物体时，$\dot{x}_{\text{rel}} = 0$，只有绝对变量有效：

\[\dot{q} = J_{\text{abs}}^+ \cdot \dot{x}_{\text{abs}} + \underbrace{(I - J_{\text{abs}}^+ J_{\text{abs}})}_{\text{零空间}} \cdot \dot{q}_{\text{null}}\]

约束 $J_{\text{rel}} \cdot \dot{q} = 0$ 通过投影满足：

\[\dot{q} = P_{\text{rel}} \cdot J_{\text{abs}}^+ \cdot \dot{x}_{\text{abs}}\]

其中 $P_{\text{rel}} = I - J_{\text{rel}}^+ J_{\text{rel}}$。

⚠️ 关键注意：$P_{\text{rel}}$ 投影保证 $J_{\text{rel}} \dot{q} = 0$（相对约束精确满足），但**不保证** $J_{\text{abs}} \dot{q} = \dot{x}_{\text{abs}}$（绝对任务一般会被畸变），因为 $J_{\text{abs}}$ 与 $J_{\text{rel}}$ 的行空间不正交。若需同时满足两个任务，应使用 task-priority cascade：$\dot{q} = J_{\text{rel}}^+ \cdot 0 + P_{\text{rel}} \cdot \bar{J}_{\text{abs}}^+ \cdot \dot{x}_{\text{abs}}$，其中 $\bar{J}_{\text{abs}} = J_{\text{abs}} P_{\text{rel}}$。

非对称任务下的分解¶

并非所有双臂任务都是对称的。在"一手固定一手操作"的任务中（如拧瓶盖），变量分解的物理含义不同：

对称任务（搬箱子）：
  x_abs = 物体质心位姿 → 主要控制目标
  x_rel = 0 → 约束（维持刚性抓取）

主导+辅助任务（拧瓶盖）：
  x_abs = 瓶子整体位姿 → 辅助手控制（保持不动）
  x_rel = 瓶盖旋转角 → 主导手控制（旋转操作）

独立任务（左碗右勺）：
  x_abs 和 x_rel 都不是自然的控制目标
  → 直接使用 J_L, J_R 独立控制更自然

选择准则：

任务类型	推荐框架	理由
对称紧耦合	$J_{\text{abs}}, J_{\text{rel}}$	自然解耦物体运动和抓取约束
主导+辅助	$J_{\text{abs}}, J_{\text{rel}}$	$x_{\text{rel}}$ 描述操作自由度
解耦	独立 $J_L, J_R$	绝对/相对分解无物理意义
松耦合	视具体任务而定	需要 case-by-case 分析

⚠️ 常见陷阱¶

💡 概念误区：认为"x_abs 就是物体质心位姿"
   新手想法："x_abs = (x_L + x_R)/2 是两末端中点，等于物体质心"
   实际上：只有当两个抓取点关于物体质心对称时，x_abs 才等于物体质心位姿。
          如果两个抓取点不对称（如一手抓物体顶部一手抓底部），
          x_abs 是两个抓取点的中点，不等于物体质心。
          在 D03（双臂力控）中，需要用 Grasp Matrix 精确计算
          接触力到物体质心 wrench 的映射。
   正确理解：x_abs 是两末端位姿的"平均"，是物体质心的近似——
            这个近似在对称抓取时是精确的，非对称时有偏差。

🧠 思维陷阱：认为"姿态的平均可以直接取算术平均"
   新手想法："R_abs = (R_L + R_R) / 2"
   实际上：旋转矩阵的算术平均不再是旋转矩阵！两个 SO(3) 元素的
          算术平均不保证正交性和 det=1。正确的"平均"是测地线中点：
          R_abs = R_L · exp(½ log(R_L^T R_R))
          这等价于在 SO(3) 流形上沿最短路径走一半。
   正确做法：使用 Lie 群上的测地线中点，而非矩阵算术平均。

练习¶

[手推] 验证 $J_{\text{aug}} = T^{-1} \begin{bmatrix} J_{\text{abs}} \\ J_{\text{rel}} \end{bmatrix}$ 的变换关系。写出 $T$ 和 $T^{-1}$ 的显式表达。
[精读] 精读 Chiacchio 1996 论文的 Section III。手推 $J_{\text{abs}}, J_{\text{rel}}$ 与 $J_{\text{aug}}$ 的关系。验证 $J_L = J_{\text{abs}} - \frac{1}{2}J_{\text{rel}}$ 和 $J_R = J_{\text{abs}} + \frac{1}{2}J_{\text{rel}}$。
[跨章综合] 回顾 M03 中的冗余分解 $\dot{q} = J^+ \dot{x} + (I - J^+ J) \dot{q}_0$。将其推广到双臂绝对/相对框架：先满足绝对任务 $\dot{x}_{\text{abs}}$，零空间中满足相对约束 $\dot{x}_{\text{rel}} = 0$。写出完整的关节速度表达式。

D1.7 刚性抓取约束与 C-space 流形 ⭐⭐⭐¶

动机：约束流形的几何¶

在 D1.3 和 D1.5 中我们看到，闭链约束 $h(q) = 0$ 将 14 维 C-space 切割为 8 维子流形。这个子流形不是一个简单的超平面——它是一个弯曲的、可能有多个连通分支的光滑流形。

理解这个流形的几何性质，对 D02 中的约束运动规划至关重要——我们需要在这个弯曲的 8 维表面上搜索无碰撞路径。

约束方程的精确定义¶

设 $T_L(q_L), T_R(q_R) \in SE(3)$ 为两臂末端的正运动学。刚性共持约束要求右末端相对于左末端的位姿等于一个固定值 $T_{\text{rel,fixed}}$：

\[T_L(q_L)^{-1} \cdot T_R(q_R) = T_{\text{rel,fixed}}\]

重写为约束函数：

\[h(q) = \log\left(T_{\text{rel,fixed}}^{-1} \cdot T_L(q_L)^{-1} \cdot T_R(q_R)\right)^{\vee} \in \mathbb{R}^6\]

其中 $\log: SE(3) \to \mathfrak{se}(3)$ 是矩阵对数，$(\cdot)^{\vee}: \mathfrak{se}(3) \to \mathbb{R}^6$ 是 vee 映射。

为什么用对数映射定义约束？

直接用 $T_L^{-1} T_R - T_{\text{rel,fixed}} = 0$ 也可以定义约束，但这是一个 $4 \times 4$ 矩阵方程——有 12 个元素但 SE(3) 只有 6 个自由度，所以约束方程有 6 个冗余。对数映射将约束压缩到最小的 6 维，避免了冗余和数值问题。

约束流形的几何性质¶

约束流形：$\mathcal{M} = \{q \in \mathbb{R}^{14} : h(q) = 0\}$

正则性：如果约束 Jacobian $J_h(q)$ 在 $\mathcal{M}$ 上处处满秩（rank = 6），则 $\mathcal{M}$ 是 $\mathbb{R}^{14}$ 中的 8 维光滑子流形（由隐函数定理保证）。在工程实现中，$J_h$ 常用同点相对 Jacobian $J_{\text{rel}}=\bar{J}_R-\bar{J}_L$ 近似或线性化。

精确性注记：$J_h = J_{\text{rel}}$ 的简写只在小误差、且左右 twist 已统一到同一表达系和同一参考点时成立。精确的 $h(q)=\log(T_{\text{rel,fixed}}^{-1}T_L^{-1}T_R)^\vee$ 微分还包含 $\text{Jlog}_6(\cdot)$ 与伴随映射项：左末端扰动要先通过 $\text{Ad}$ 变换到相对误差所在的切空间，再与右末端扰动相减。不同库的 body/spatial Jacobian、左/右扰动约定不同，公式块顺序会变；不变的原则是：先用 adjoint/平移项统一表达系和参考点，再做相减。LOCAL_WORLD_ALIGNED 只统一坐标轴方向，不会自动统一线速度参考点。

切空间：在 $q \in \mathcal{M}$ 处，$T_q\mathcal{M} = \ker(J_{\text{rel}}(q))$，维度为 8。

法空间：$N_q\mathcal{M} = \text{range}(J_{\text{rel}}^T(q))$，维度为 6。

几何图像（2D 类比）：

想象 R^3 中的一个曲面（2 维流形），由约束 h(x,y,z) = 0 定义。

  法向量 ∇h
    ↑
    │   · · · · ·
    │ ·           ·        ← 约束流形 M
    ├─────────────── 切平面 T_q M
    │ ·           ·
    │   · · · · ·

双臂的情况：
  ambient 空间 R^14
  约束 h(q) = 0 定义 8 维流形 M
  切空间 T_q M = ker(J_h)              (8 维)
  常用局部近似：J_h ≈ J_rel = J̄_R - J̄_L
  法空间 N_q M = range(J_h^T)           (6 维)
  投影到切空间：P = I - J_h^+ J_h

约束流形上的运动¶

在约束流形上，系统只能沿切空间方向运动。投影矩阵 $P = I - J_h^+ J_h$ 将任意关节速度投影到切空间；在小误差局部线性化中，可以用同点 $J_{\text{rel}}$ 代替 $J_h$：

\[\dot{q}_{\text{feasible}} = P \cdot \dot{q}_{\text{desired}}\]

投影后的速度满足 $J_h \cdot \dot{q}_{\text{feasible}} = 0$，即不改变两末端相对位姿。

约束漂移与修正：

由于数值积分误差，系统可能逐渐偏离约束流形。需要定期进行约束修正：

Baumgarte 稳定化：
  速度级约束修正：
  J_h · q̇ = -α · h(q)

  α > 0 是一阶误差收敛增益，单位 1/s。

  若控制器在加速度级求解，则使用二阶形式：
  J_h · q̈ + J̇_h · q̇ = -2ζω · J_h · q̇ - ω² · h(q)

  ζ 是阻尼比，ω 是期望误差收敛带宽。常用 ζ=1，
  ω=5-20 rad/s；不要把速度级和加速度级公式混在一起。

OMPL 中的约束流形实现¶

// ============================================================
// OMPL 约束类——双臂刚性共持
// 继承 ompl::base::Constraint
// ============================================================

class DualArmConstraint : public ompl::base::Constraint {
public:
    DualArmConstraint(
        const pinocchio::Model& model,
        pinocchio::Data& data,
        pinocchio::FrameIndex left_ee_id,
        pinocchio::FrameIndex right_ee_id,
        const pinocchio::SE3& T_rel_fixed)
        : ompl::base::Constraint(model.nq, 6)  // ambient dim=14, constraint dim=6
        , model_(model), data_(data)
        , left_ee_id_(left_ee_id), right_ee_id_(right_ee_id)
        , T_rel_fixed_(T_rel_fixed)
    {}

    // 约束函数 h(q)
    void function(const Eigen::Ref<const Eigen::VectorXd>& q,
                  Eigen::Ref<Eigen::VectorXd> out) const override {
        pinocchio::forwardKinematics(model_, data_, q);
        pinocchio::updateFramePlacements(model_, data_);

        const auto& T_L = data_.oMf[left_ee_id_];
        const auto& T_R = data_.oMf[right_ee_id_];

        // h(q) = log(T_rel_fixed^{-1} * T_L^{-1} * T_R)
        pinocchio::SE3 T_err = T_rel_fixed_.inverse() * T_L.inverse() * T_R;
        out = pinocchio::log6(T_err).toVector();
    }

    // 约束 Jacobian J_h(q)：这里给出小误差局部线性化
    // 关键：先把 LOCAL_WORLD_ALIGNED Jacobian 的线速度参考点统一到 p_ref
    void jacobian(const Eigen::Ref<const Eigen::VectorXd>& q,
                  Eigen::Ref<Eigen::MatrixXd> J) const override {
        pinocchio::computeJointJacobians(model_, data_, q);
        pinocchio::updateFramePlacements(model_, data_);

        Eigen::MatrixXd J_L(6, model_.nv), J_R(6, model_.nv);
        J_L.setZero(); J_R.setZero();

        pinocchio::getFrameJacobian(model_, data_, left_ee_id_,
            pinocchio::LOCAL_WORLD_ALIGNED, J_L);
        pinocchio::getFrameJacobian(model_, data_, right_ee_id_,
            pinocchio::LOCAL_WORLD_ALIGNED, J_R);

        const auto& T_L = data_.oMf[left_ee_id_];
        const auto& T_R = data_.oMf[right_ee_id_];
        Eigen::Vector3d p_ref = 0.5 * (T_L.translation() + T_R.translation());

        auto skew = [](const Eigen::Vector3d& a) {
            Eigen::Matrix3d S;
            S << 0.0, -a.z(), a.y(),
                 a.z(), 0.0, -a.x(),
                -a.y(), a.x(), 0.0;
            return S;
        };

        auto shiftToPoint = [&](const Eigen::MatrixXd& J_frame,
                                const Eigen::Vector3d& p_frame) {
            Eigen::Matrix<double, 6, 6> X =
                Eigen::Matrix<double, 6, 6>::Identity();
            Eigen::Vector3d d = p_ref - p_frame;
            X.topRightCorner<3, 3>() = -skew(d);  // [v;omega] 顺序
            Eigen::MatrixXd J_ref = X * J_frame;
            return J_ref;
        };

        Eigen::MatrixXd J_L_ref = shiftToPoint(J_L, T_L.translation());
        Eigen::MatrixXd J_R_ref = shiftToPoint(J_R, T_R.translation());

        J = J_R_ref - J_L_ref;  // 同点相对 Jacobian；大误差 Newton 需再乘 Jlog/Ad 项
    }

private:
    const pinocchio::Model& model_;
    mutable pinocchio::Data data_;  // mutable for const FK calls
    pinocchio::FrameIndex left_ee_id_, right_ee_id_;
    pinocchio::SE3 T_rel_fixed_;
};

⚠️ 常见陷阱¶

⚠️ 编程陷阱：OMPL Constraint 中修改 Data 的线程安全性
   错误做法：多线程规划中共享同一个 pinocchio::Data
   现象：偶发的数值错误、segfault、或规划结果不可复现
   根本原因：pinocchio::Data 不是线程安全的——FK 计算会修改其内部状态。
            OMPL 的多线程采样可能同时调用 function() 和 jacobian()
   正确做法：每个线程使用独立的 pinocchio::Data 副本
     // 在 constraint 构造时
     thread_local pinocchio::Data data(model);

💡 概念误区：认为"约束流形是凸集"
   新手想法："约束流形 M 是 R^14 中的光滑子流形，
            所以 M 上两点之间存在直线连接"
   实际上：约束流形通常是高度非凸的。M 上两个构型之间可能存在
          多条路径（绕不同的奇异点），也可能存在不同的连通分支
          （拓扑障碍）。这就是为什么需要基于采样的约束规划算法
          （CBiRRT, AtlasRRT）而非简单的直线插值。
   正确理解：约束流形的全局拓扑很复杂，这是 D02 的核心挑战。

练习¶

[思考] 对于双 6-DOF 臂（如双 UR5e，总 12-DOF），刚性共持约束后有效 DOF = 12 - 6 = 6。这意味着约束流形维度等于任务维度（6D 末端位姿控制）。此时零空间消失——两臂没有冗余度做碰撞回避。讨论这对工程实践的影响。
[计算] 对于双 7-DOF + 1-DOF 移动平台（总 15-DOF），刚性约束后有效 DOF = 15 - 6 = 9。多出的 1 维零空间可以做什么？

D1.8 双臂系统建模——增广 / 相对 / 主从三种视角 ⭐⭐¶

三种建模范式¶

双臂系统可以从三个不同的视角建模，每种视角适用于不同的控制目标：

范式 1：增广关节空间（Augmented Joint Space）

状态：q = [q_L^T, q_R^T]^T ∈ R^14
输出：ẋ_aug = [V_L^T, V_R^T]^T ∈ R^12
映射：ẋ_aug = J_aug · q̇

思想：把两臂看作一个 14-DOF 机器人
优势：统一处理，利用成熟的单臂框架
劣势：维度高（14 维规划），不区分物体运动和相对运动
适用：解耦任务、碰撞回避

范式 2：绝对/相对分解（Absolute/Relative Decomposition）

状态：q = [q_L^T, q_R^T]^T ∈ R^14
输出：ẋ_abs ∈ R^6, ẋ_rel ∈ R^6
映射：ẋ_abs = J_abs · q̇, ẋ_rel = J_rel · q̇

思想：将物体运动和抓取关系解耦
优势：物理直觉清晰，可独立设计控制器
劣势：姿态的"平均"需要 Lie 群运算
适用：紧耦合对称任务（搬运、协调操作）

范式 3：主从分解（Leader-Follower Decomposition）

状态：q_leader = q_L ∈ R^7, q_follower = q_R ∈ R^7
控制：
  Leader：独立位控，跟踪期望轨迹
  Follower：阻抗控制，跟随 leader 并维持相对关系

思想：一臂主导运动，另一臂被动跟随
优势：控制器设计简单，工业界常用
劣势：不对称——leader 故障则系统失效
适用：主导+辅助任务（装配、焊接夹持）

三种范式的对比：

维度	增广空间	绝对/相对	主从
数学优雅度	中	高	低
工程简单度	低	中	高
对称性	对称	对称	非对称
容错性	中	中	低
物理直觉	弱	强	强
工业应用	少	中	多

不是X而是Y：主从分解不是"懒惰的简化"，而是在特定任务（一手固定一手操作）中物理上最自然的建模方式。强行用对称框架处理非对称任务反而引入不必要的复杂性。选择建模范式应该根据任务的物理对称性，而非框架的数学优雅性。

练习¶

[对比] 对于"拧瓶盖"任务，分别用三种范式描述：（a）增广空间：12 维末端速度向量中哪些分量需要控制？（b）绝对/相对：$\dot{x}_{\text{abs}}$ 和 $\dot{x}_{\text{rel}}$ 分别控制什么？（c）主从：谁是 leader（持瓶身）谁是 follower（拧盖）？
[思考] 如果任务在执行过程中从"对称搬运"切换到"一手固定一手拧螺丝"，建模范式是否应该同步切换？切换时的过渡策略是什么？

D1.9 主流双臂平台对比 ⭐¶

为什么要了解平台¶

不同双臂平台的硬件架构直接影响控制策略的选择。例如，ALOHA 使用低成本舵机（位控模式），不适合力控；YuMi 有内置力矩传感器，天然适合协调阻抗控制。了解平台特性有助于选择正确的算法路线。

主流平台对比表¶

平台	开发者	单臂DOF	驱动方式	力矩传感	末端负载	典型用途	控制接口
双 Franka Panda	Franka Emika	7	关节力矩	✅ 关节+末端	3 kg	研究/力控	libfranka 1kHz
ABB YuMi (IRB 14000)	ABB	7	关节力矩	✅ 关节	0.5 kg	装配	EGM 250Hz
Baxter/Sawyer	Rethink → Haddington	7	SEA	✅ 弹性元件	2.2 kg	教学/协作	ROS/Intera
PR2	Willow Garage	7+移动	SEA	✅	1.8 kg	研究(已停产)	ROS
ARMAR-6	KIT H2T	7(+手)	力矩	✅	~3 kg	人形/研究	ArmarX
ALOHA	Stanford/TRI	6	位控舵机	❌	0.75 kg	遥操作/学习	Dynamixel
Google Aloha 2	Google DeepMind	6	位控舵机	❌	0.75 kg	VLA 数据采集	ROS2
双 iiwa 14	KUKA	7	关节力矩	✅ 全轴	14 kg	工业/研究	FRI 1kHz
Bimanual Jaco	Kinova	6-7	电流控制	部分	2.6-4 kg	康复/服务	Kinova API
UFactory xArm (双臂)	UFactory	6-7	电流控制	✅ 末端	5 kg	研究/轻工业	SDK 250Hz
Flexiv Rizon (双臂)	Flexiv	7	力矩	✅ 全轴	4 kg	自适应力控	RDK 1kHz

平台定量性能对比¶

选择平台不能只看参数表——实际控制性能差异巨大。以下是同一任务（双臂共持 1kg 杆，正弦轨迹跟踪）下不同平台的实测/仿真数据：

指标	双 Franka	双 iiwa 14	ABB YuMi	ALOHA	Flexiv Rizon
控制频率 (Hz)	1000	1000	250	50	1000
位置跟踪误差 RMS (mm)	0.3-0.8	0.2-0.5	1.0-2.0	3.0-8.0	0.4-0.9
力控内力波动 (N, 1σ)	0.5-1.5	0.3-1.0	2.0-5.0	N/A	0.8-2.0
最大速度 (m/s)	2.0	2.0	1.5	0.5	2.0
重复定位精度 (mm)	±0.05	±0.02	±0.02	±1.0	±0.05
零力矩驱动延迟 (ms)	< 1	< 1	~4	~20	< 1
单臂成本 (USD, 约)	30K-40K	80K-120K	50K-70K	2K-5K	20K-30K
力控适合度	⭐⭐⭐⭐⭐	⭐⭐⭐⭐⭐	⭐⭐⭐	⭐	⭐⭐⭐⭐
学习型控制适合度	⭐⭐⭐	⭐⭐	⭐⭐	⭐⭐⭐⭐⭐	⭐⭐⭐

关键观察：力控性能与控制频率和力矩传感器质量直接相关。ALOHA 的 50Hz 位控在力控任务上完全不可用，但在学习型控制中这不是瓶颈——因为学习策略在训练数据中已经隐式补偿了控制延迟。Flexiv Rizon 是近年新兴的性价比选手，力控性能接近 Franka 但成本更低。

平台选型决策¶

需要力控/阻抗控制？
├── 是 → 需要高载荷？
│   ├── 是 → 双 iiwa 14（14kg，FRI 1kHz力矩接口）
│   └── 否 → 双 Franka Panda（3kg，libfranka 1kHz，生态最好）
└── 否 → 用于遥操作/学习？
    ├── 是 → ALOHA/Aloha 2（低成本，位控舵机，ACT/VLA生态）
    └── 否 → 用于工业装配？
        ├── 是 → ABB YuMi（小物体精密装配，EGM接口）
        └── 否 → 用于教学/原型验证？
            └── 是 → Baxter（已停产但二手多）或 Kinova Jaco

ALOHA：改变双臂研究格局的平台¶

ALOHA（A Low-cost Open-source Hardware System for Bimanual Teleoperation）由 Stanford IRIS Lab 的 Tony Zhao 等人在 2023 年推出，对双臂研究产生了变革性影响：

硬件特征：
  - 双 ViperX 250（6-DOF，Dynamixel 舵机）
  - 成本 < $20K（vs 双 Franka > $100K）
  - 纯位控模式（无力矩/力反馈传感器）
  - 开源硬件设计 + 软件栈

研究范式转变：
  传统路线：精确模型 → 精确控制 → 精确执行
  ALOHA 路线：遥操作采集数据 → 学习策略 → 行为克隆执行

  → 不需要精确力控（位控舵机足够做行为克隆）
  → 不需要精确模型（学习直接从像素到动作）
  → 大幅降低了双臂研究的准入门槛

关键论文：
  ACT（Action Chunking with Transformers, Zhao et al. 2023）
  Aloha 2（Google DeepMind 2024，工业级迭代）

反事实推理：如果 ALOHA 没有出现（假设双臂研究仍需 > $100K 平台），双臂学习型控制的研究进展会慢多少？实际上，ALOHA 之前，全球能做双臂行为克隆实验的实验室不到 20 家（受限于硬件成本）。ALOHA 开源后一年内，超过 100 个团队搭建了类似系统——研究产出量级提升了 5-10 倍。这证明了：对于学习型控制，低成本可复现的硬件比精密的力控平台更重要。

⚠️ 常见陷阱¶

🧠 思维陷阱：认为"力控能力越强的平台越好"
   新手想法："双 iiwa 有最好的力矩传感，应该选它做所有双臂研究"
   实际上：力控平台（iiwa/Franka）适合需要精确力交互的任务
          （装配、柔性物体操作、人机协作）。但对于学习型控制
          （行为克隆、VLA），低成本位控平台（ALOHA）更合适——
          因为学习方法不依赖精确力控，而是从数据中自动补偿控制误差。
   正确思维：平台选择应由任务和方法决定，而非平台本身的"最大能力"。

练习¶

[调研] 在你所在实验室或可获取的平台中，选择最适合做双臂协调搬运实验的系统。用上述决策树分析你的选择，并列出该平台的 3 个主要限制。
[思考] ALOHA 的 6-DOF 臂双臂共持刚性物体后有效 DOF = 12 - 6 = 6，零空间维度为 0。这对协调规划有什么影响？与双 7-DOF Franka 的 2 维零空间对比。

D1.10 双臂研究路线图——从规划到力控到学习 ⭐¶

历史演进脉络¶

双臂操作研究经历了四个主要阶段，每个阶段解决了上一阶段遗留的核心问题：

阶段 1：运动学与规划（1987-2000）
  核心问题：如何让两臂协调运动而不碰撞？
  代表工作：
    - Khatib 1987: 操作空间动力学（单臂框架，但为双臂铺路）
    - Uchiyama & Dauchez 1988: 对称协调运动学
    - Chiacchio 1996: 绝对/相对变量分解
  主要成果：双臂运动学框架（J_aug, J_rel, J_abs）
  遗留问题：规划时未考虑力交互

阶段 2：力控与阻抗（1992-2010）
  核心问题：如何控制两臂之间的内力？
  代表工作：
    - Schneider & Cannon 1992: Object Impedance Control
    - Williams & Khatib 1993: 虚拟连杆内力模型
    - Caccavale 2008: 6-DOF 双臂阻抗（事实标准）
  主要成果：内力分解理论，协调阻抗控制
  遗留问题：模型需要精确，环境变化时需要重新设计控制器

阶段 3：基于学习的双臂操作（2018-2024）
  核心问题：如何不依赖精确模型完成复杂双臂任务？
  代表工作：
    - Zhao et al. 2023 (ACT): 行为克隆 + ALOHA
    - Chi et al. 2024 (Diffusion Policy): 扩散模型动作生成
  主要成果：数据驱动的双臂策略，遥操作数据采集管线
  遗留问题：泛化能力有限，安全性无保证

阶段 4：遥操作与人机协作（2020-present）
  核心问题：如何高效采集双臂示教数据？如何远程控制双臂？
  代表工作：
    - 波变量遥操作理论（D06）
    - Haptic 设备双臂遥操作
    - VR/AR 辅助遥操作界面
  主要成果：遥操作数据采集管线，力反馈设备
  当前挑战：延迟补偿，多模态感知融合

M 系列 → D 系列的知识继承¶

D 系列不是独立的——它大量复用 M01-M15（单臂核心）和 F01-F10（力控）的知识：

D 系列章节	依赖的 M/F 知识	继承关系
D01 导论	M01 Pinocchio, M03 IK	Jacobian 计算、伪逆、零空间
D02 协调规划	M07 OMPL, M04 碰撞检测	采样规划框架、FCL
D03 协调力控	F01-F03 阻抗控制, F02 操作空间	阻抗律、操作空间惯量
D04 双臂学习	—	全新内容（学习型方法）
D05-D07 遥操作	F01 力控基础	力反馈、无源性
D09 系统集成	M14 MoveIt2	双臂 MoveIt2 配置

跨章回顾桥：回顾 M01：Pinocchio 的 Model/Data 分离使得同一 Model 可以在多线程中安全使用——每个线程有自己的 Data。这个设计在双臂中更加重要：MPC 求解器可能需要在一个控制周期内多次调用 FK 和 Jacobian，Model/Data 分离使得这些调用可以并行化。

练习¶

[综合路线] 画出从 M01 到 D10 的完整知识依赖图。标注每条依赖边上的关键知识传递（如 M01→D01："Jacobian 计算方法"）。
[思考] 阶段 3（学习型方法）是否会完全取代阶段 1-2（模型驱动方法）？讨论学习型方法在安全关键任务（如手术辅助、核废料处理）中的局限性。

D1.11 协调奇异性与操作度分析 ⭐⭐⭐¶

动机：双臂独有的奇异问题¶

单臂的奇异位形（如完全伸直、关节对齐）已经在 M03 中详细讨论过。双臂系统除了继承单臂的奇异问题外，还有自己独有的**协调奇异**——两臂末端的几何关系导致联合系统的操作度退化。

协调奇异的分类¶

类型 1：单臂奇异

两臂之一处于单臂奇异位形（如肘关节完全伸直），导致 $J_L$ 或 $J_R$ 的秩下降。

影响：$J_{\text{aug}}$ 的秩从 12 降到 11 或更低。

检测：$\sigma_{\min}(J_L) < \epsilon$ 或 $\sigma_{\min}(J_R) < \epsilon$。

类型 2：协调奇异（双臂末端共线）

两臂末端连线与某臂的关节轴重合，导致 $J_{\text{rel}}$ 的秩下降——即使每只臂单独看都不奇异。

物理场景：
  两臂末端在同一条直线上（如面对面伸直手臂握手）

  此时 J_rel 的一个奇异值趋近于 0
  → 沿该方向的相对运动不可控
  → Moore-Penrose 投影矩阵 P 的一个特征值从 1 骤变为 0
  → 约束流形的切空间在该点"坍缩"

检测：σ_min(J_rel) < ε

回避：
  - 规划时避免两臂末端共线的构型
  - 控制时使用阻尼伪逆（Nakamura 自适应阻尼）
  - 零空间中加入"远离协调奇异"的梯度项

类型 3：增广奇异（工作空间边界）

两臂的可达工作空间交集缩小到低维（如两臂只能在一条线上共持物体），导致 $J_{\text{aug}}$ 的操作度急剧下降。

双臂操作度¶

Chiacchio 等人 1991 年定义了**全局任务空间操作度**：

\[w_{\text{aug}} = \sqrt{\det(J_{\text{aug}} J_{\text{aug}}^T)} \in \mathbb{R}\]

这个标量衡量了双臂系统在当前构型下操纵两个末端的整体能力。$w_{\text{aug}} = 0$ 意味着至少一个方向的末端速度不可实现——系统处于奇异。

分解为绝对操作度和相对操作度：

\[w_{\text{abs}} = \sqrt{\det(J_{\text{abs}} J_{\text{abs}}^T)}, \quad w_{\text{rel}} = \sqrt{\det(J_{\text{rel}} J_{\text{rel}}^T)}\]

操作度	物理含义	对任务的影响
$w_{\text{abs}}$ 高	物体可灵活运动	搬运任务轻松
$w_{\text{abs}}$ 低	物体运动受限	搬运困难，需换路径
$w_{\text{rel}}$ 高	两手可灵活改变相对位姿	装配/操作灵活
$w_{\text{rel}}$ 低	两手相对运动受限	装配精度下降

Worked Example：协调奇异的数值分析¶

以下完整示例展示如何用 Pinocchio 检测并分析协调奇异。

场景：双 Franka Panda 面对面放置，两臂逐渐伸直至末端共线。

// ============================================================
// 协调奇异检测——Pinocchio Worked Example
// 目标：扫描左臂 joint2 从 0 到 -π/2，观察 σ_min(J_rel) 变化
// ============================================================

#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <iostream>
#include <vector>

void detect_coordination_singularity(
    const pinocchio::Model& model, pinocchio::Data& data,
    pinocchio::FrameIndex left_ee, pinocchio::FrameIndex right_ee)
{
    const int N_steps = 100;
    std::vector<double> angles(N_steps), sigma_min_vals(N_steps);

    for (int i = 0; i < N_steps; i++) {
        Eigen::VectorXd q = pinocchio::neutral(model);
        double angle = -M_PI / 2.0 * i / (N_steps - 1);
        q[1] = angle;   // 左臂 joint2（肩前屈）
        q[8] = angle;   // 右臂 joint2（镜像）

        pinocchio::forwardKinematics(model, data, q);
        pinocchio::updateFramePlacements(model, data);
        pinocchio::computeJointJacobians(model, data, q);

        // 计算同点 J_rel = J̄_R - J̄_L
        Eigen::MatrixXd J_L(6, model.nv), J_R(6, model.nv);
        J_L.setZero(); J_R.setZero();
        pinocchio::getFrameJacobian(model, data, left_ee,
            pinocchio::LOCAL_WORLD_ALIGNED, J_L);
        pinocchio::getFrameJacobian(model, data, right_ee,
            pinocchio::LOCAL_WORLD_ALIGNED, J_R);

        auto skew = [](const Eigen::Vector3d& a) {
            Eigen::Matrix3d S;
            S << 0.0, -a.z(), a.y(),
                 a.z(), 0.0, -a.x(),
                -a.y(), a.x(), 0.0;
            return S;
        };
        Eigen::Vector3d p_L = data.oMf[left_ee].translation();
        Eigen::Vector3d p_R = data.oMf[right_ee].translation();
        Eigen::Vector3d p_ref = 0.5 * (p_L + p_R);

        Eigen::Matrix<double, 6, 6> X_L =
            Eigen::Matrix<double, 6, 6>::Identity();
        Eigen::Matrix<double, 6, 6> X_R =
            Eigen::Matrix<double, 6, 6>::Identity();
        X_L.topRightCorner<3, 3>() = -skew(p_ref - p_L);  // [v;omega]
        X_R.topRightCorner<3, 3>() = -skew(p_ref - p_R);

        Eigen::MatrixXd J_rel = X_R * J_R - X_L * J_L;

        // SVD 提取最小奇异值
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(J_rel);
        double sigma_min = svd.singularValues().tail(1)(0);

        angles[i] = angle * 180.0 / M_PI;  // 转为度
        sigma_min_vals[i] = sigma_min;

        // 报告奇异点
        if (sigma_min < 0.01) {
            auto p_L = data.oMf[left_ee].translation();
            auto p_R = data.oMf[right_ee].translation();
            Eigen::Vector3d dir = (p_R - p_L).normalized();
            std::cout << "SINGULAR at joint2 = " << angles[i] << " deg"
                      << "\n  σ_min = " << sigma_min
                      << "\n  EE distance = " << (p_R - p_L).norm()
                      << "\n  Direction = [" << dir.transpose() << "]"
                      << "\n  → 两臂末端共线，沿此方向相对运动不可控\n";
        }
    }
}

典型输出：

SINGULAR at joint2 = -72.3 deg
  σ_min = 0.0034
  EE distance = 0.142 m
  Direction = [0.998 0.012 -0.054]
  → 两臂末端共线，沿此方向相对运动不可控

物理解释：当 $\sigma_{\min}(J_{\text{rel}}) \to 0$ 时，存在一个方向上两末端的相对速度无法通过关节速度实现。对于刚性共持任务，这意味着约束流形的切空间在该方向"坍缩"——投影矩阵 $P$ 的一个特征值从 1 跳变为 0，导致零空间突然"丢失"一个维度。规划器在这样的构型附近会出现 Newton 投影发散。

⚠️ 常见陷阱¶

🧠 思维陷阱：认为"操作度越高越好，应该最大化操作度"
   新手想法："零空间中总是加 ∇w 来最大化操作度"
   实际上：最大操作度的构型可能不适合实际任务。例如，最大操作度
          通常出现在两臂完全展开的构型——但这时两臂末端距离最远，
          无法共持小物体。操作度优化应该在满足任务约束的前提下进行。
   正确做法：将操作度作为零空间目标（低优先级），不与任务目标冲突。

练习¶

[编程] 用 Pinocchio 扫描双 Franka Panda 的 C-space，计算 $w_{\text{aug}}, w_{\text{abs}}, w_{\text{rel}}$ 的分布。找到协调奇异构型（$w_{\text{rel}} \approx 0$ 但 $w_L, w_R > 0$），并描述其几何含义。
[思考] 协调奇异是否可以通过增加自由度来消除？如果给每只臂增加 1 个冗余关节（变成双 8-DOF），协调奇异是否仍然存在？

本章小结¶

知识点	核心要点	对应小节	难度
单臂局限性	载荷、DOF、内力、反作用力四大局限	D1.1	⭐
Koivo 三分类	紧耦合/松耦合/解耦 → 力控/时序/碰撞	D1.2	⭐
Krebs 四维分类	协调×交互×角色×对称 = 54 种组合	D1.2	⭐⭐
运动学约束	开链/闭链/虚拟闭链 → 14/8/可调 DOF	D1.3	⭐⭐
增广 Jacobian	$J_{\text{aug}} \in \mathbb{R}^{12 \times 14}$，2 维零空间	D1.4	⭐⭐
相对 Jacobian	$J_{\text{rel}} \in \mathbb{R}^{6 \times 14}$，约束 $J_{\text{rel}} \dot{q} = 0$	D1.5	⭐⭐
绝对/相对分解	共模/差模类比，Chiacchio 1996	D1.6	⭐⭐⭐
约束流形	8 维光滑子流形，$P = I - J_{\text{rel}}^+ J_{\text{rel}}$	D1.7	⭐⭐⭐
三种建模范式	增广/绝对相对/主从 → 按任务选择	D1.8	⭐⭐
双臂平台	Franka/YuMi/ALOHA/iiwa 各有适用场景	D1.9	⭐
协调奇异	单臂奇异 + 协调奇异 + 增广奇异	D1.11	⭐⭐⭐

累积项目：本章新增模块¶

项目：Mini-DualArm 双臂协调操作系统

本章新增基础模块：

mini_dualarm/
├── models/
│   └── dual_panda.urdf          # 双 Franka Panda 合并 URDF
├── src/
│   ├── dual_arm_kinematics.cpp  # J_aug, J_rel, J_abs 计算
│   ├── null_space_projector.cpp # 零空间投影 P = I - J_rel^+ J_rel
│   └── manipulability.cpp       # w_aug, w_abs, w_rel 计算
├── python/
│   ├── classification_demo.py   # 交互式任务分类标注工具
│   └── manipulability_viz.py    # 操作度可视化
└── tests/
    ├── test_jacobian.cpp        # 验证 J_aug/J_rel 数值正确性
    └── test_projection.cpp      # MP 投影幂等性 + DLS 任务泄漏验证

下一章（D02）将在此基础上加入约束规划模块：constrained_planner.cpp。

延伸阅读¶

文献	类型	难度	推荐理由
Smith et al. 2012, "Dual Arm Manipulation — A Survey", RAS	综述	⭐⭐	最被引用的双臂综述，Section 2-3 的控制方案对比表是最好的入门材料
Krebs & Asfour 2022, "A Bimanual Manipulation Taxonomy", RA-L	论文	⭐⭐	新一代分类学，图 3 的分类树是教学工具
Caccavale & Uchiyama 2008, "Cooperative Manipulators", Springer Handbook Ch.39	教材	⭐⭐⭐	Handbook 级权威综述，标准符号体系
Chiacchio et al. 1996, "Direct and Inverse Kinematics for Coordinated Motion", ASME	论文	⭐⭐⭐	绝对/相对变量的原始定义，数学严谨
Siciliano, Sciavicco et al. "Robotics: Modelling, Planning and Control", Ch.12	教材	⭐⭐	多臂协调的教材级讲解，适合系统学习
Khatib 1987, "A Unified Approach for Motion and Force Control", IEEE J. RA	论文	⭐⭐⭐⭐	操作空间动力学——所有双臂任务空间控制的母框架

故障排查手册¶

症状	可能原因	排查步骤	相关章节
$J_{\text{aug}}$ 计算结果全零	未调用 `forwardKinematics`	1. 检查 FK 调用顺序 2. 打印 `data.oMf` 确认帧位姿 3. 确认帧 ID 正确	D1.4
$P^2 \neq P$（幂等性验证失败）	把阻尼伪逆滤波器误当正交投影，或 SVD 阈值不一致	1. 确认 $P$ 用 Moore-Penrose/SVD 伪逆 2. 若使用阻尼伪逆，改查 $\\|J_{\text{rel}}P_\lambda\dot{q}_0\\|$ 3. 检查 $J_{\text{rel}}$ 条件数	D1.5
协调 IK 两臂末端漂移	约束漂移未修正	1. 添加速度级 $J_h\dot{q}=-\alpha h$ 或加速度级 Baumgarte 2. 检查控制频率（< 200Hz 时漂移明显）3. 打印 $\\|h(q)\\|$ 监控约束违反	D1.7
两臂在共持物体时产生大内力	位控模式下的 force fight	1. 检查是否使用了独立位控 2. 切换到协调阻抗控制 3. 参考 D03 的内力管理方案	D1.1, D03
OMPL 约束规划长时间无解	投影失败率过高	1. 检查 `maxIterations` 参数 2. 尝试 AtlasStateSpace 替代 ProjectedStateSpace 3. 确认初始构型在约束流形上	D1.7, D02

编号	问题	答不出时回顾
1	雅可比矩阵：对于 7-DOF 机械臂，几何雅可比 \(J(q) \in \mathbb{R}^{6 \times 7}\) 的物理含义是什么？当 \(\text{rank}(J) < 6\) 时意味着什么？	M01 Pinocchio 深度精读
2	伪逆与零空间：写出右伪逆 \(J^+ = J^T(JJ^T)^{-1}\)（\(J\) 满行秩时即 Moore-Penrose 伪逆）的表达式。\(q_{\text{null}} = (I - J^+J)\dot{q}_0\) 为什么满足 \(J q_{\text{null}} = 0\)？这个性质的工程意义是什么？	M03 IK 求解器深度
3	操作空间动力学：写出操作空间惯量矩阵 \(\Lambda = (JM^{-1}J^T)^{-1}\) 的定义。它为什么需要 \(J\) 行满秩？当接近奇异位形时 \(\Lambda\) 会发生什么？	F02 操作空间动力学
4	SE(3) 与 twist：写出刚体的空间速度 twist \(\mathcal{V} = [\omega^T, v^T]^T \in \mathbb{R}^6\)。两个 twist 的"差"在 Lie 代数意义下如何计算？	李群基础 / M01
5	阻抗控制：Hogan 阻抗律 \(F = M_d\ddot{\tilde{x}} + D_d\dot{\tilde{x}} + K_d\tilde{x}\) 的三个参数分别控制什么物理行为？	F01 阻抗导纳二分法

Koivo 分类	运动学框架	力控策略	规划方法	典型控制频率
紧耦合	\(J_{\text{rel}}\) 零空间投影	Object Impedance + 内力	约束流形采样	1 kHz
松耦合	独立 \(J_L, J_R\) + 时序协调	独立阻抗 + 力阈值切换	多阶段规划	500 Hz
解耦	独立 \(J_L, J_R\)	独立位控	独立规划 + 碰撞回避	100-500 Hz

约束类型	约束方程	约束来源	有效 DOF	鲁棒性
开链	无	—	14	N/A
闭链	\(h(q) = 0\)	物理接触	8	取决于接触稳定性
虚拟闭链	\(h(q) \approx 0\)	控制器	8-14（连续）	取决于控制器增益

性质	表达式	物理含义
刚性约束	\(J_{\text{rel}} \cdot \dot{q} = 0\)	关节速度使两末端相对位姿不变
零空间	\(\ker(J_{\text{rel}}) \subset \mathbb{R}^{14}\)	满足闭链约束的关节速度集合
维度	\(\dim(\ker(J_{\text{rel}})) = 14 - 6 = 8\)	约束流形的切空间维度

任务类型	推荐框架	理由
对称紧耦合	\(J_{\text{abs}}, J_{\text{rel}}\)	自然解耦物体运动和抓取约束
主导+辅助	\(J_{\text{abs}}, J_{\text{rel}}\)	\(x_{\text{rel}}\) 描述操作自由度
解耦	独立 \(J_L, J_R\)	绝对/相对分解无物理意义
松耦合	视具体任务而定	需要 case-by-case 分析

操作度	物理含义	对任务的影响
\(w_{\text{abs}}\) 高	物体可灵活运动	搬运任务轻松
\(w_{\text{abs}}\) 低	物体运动受限	搬运困难，需换路径
\(w_{\text{rel}}\) 高	两手可灵活改变相对位姿	装配/操作灵活
\(w_{\text{rel}}\) 低	两手相对运动受限	装配精度下降

症状	可能原因	排查步骤	相关章节
\(J_{\text{aug}}\) 计算结果全零	未调用 `forwardKinematics`	1. 检查 FK 调用顺序 2. 打印 `data.oMf` 确认帧位姿 3. 确认帧 ID 正确	D1.4
\(P^2 \neq P\)（幂等性验证失败）	把阻尼伪逆滤波器误当正交投影，或 SVD 阈值不一致	1. 确认 \(P\) 用 Moore-Penrose/SVD 伪逆 2. 若使用阻尼伪逆，改查 \(\\|J_{\text{rel}}P_\lambda\dot{q}_0\\|\) 3. 检查 \(J_{\text{rel}}\) 条件数	D1.5
协调 IK 两臂末端漂移	约束漂移未修正	1. 添加速度级 \(J_h\dot{q}=-\alpha h\) 或加速度级 Baumgarte 2. 检查控制频率（< 200Hz 时漂移明显）3. 打印 \(\\|h(q)\\|\) 监控约束违反	D1.7
两臂在共持物体时产生大内力	位控模式下的 force fight	1. 检查是否使用了独立位控 2. 切换到协调阻抗控制 3. 参考 D03 的内力管理方案	D1.1, D03
OMPL 约束规划长时间无解	投影失败率过高	1. 检查 `maxIterations` 参数 2. 尝试 AtlasStateSpace 替代 ProjectedStateSpace 3. 确认初始构型在约束流形上	D1.7, D02