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

第 7 章　双臂与多臂协同操作——闭链运动学与力分配（多机协作视角）

性质：算法工程教学 | 难度跨度：⭐⭐ ~ ⭐⭐⭐⭐ | 预计精读：18-26 小时

本章定位：⚪ 部分共享——闭链运动学与内力分配的数学对"任何一组末端共同把持一个负载"都通用，无论这些末端来自两条工业臂、一台人形的两只手、还是四台四足背上的机械臂。

本章与众不同的视角：本项目在 05_运动控制/20_机械臂/ 已经有一套完整的双臂子课程（D01 协调运动学、D02 协调规划、D03 协调力控、D04 双臂学习），那套课程站在**单系统操作**的立场，把"双臂"当作一台 14 自由度的机器，深入到 Pinocchio API、OMPL 约束规划、TSR/CBiRRT 的工程细节。本章不重复那套推导，而是站在**多机协作**的立场重新审视同一个问题：把"双臂"看成"\(N=2\) 的多机协同操作"，把"内力"看成"多智能体之间通过共享负载耦合的相互作用"，把"主从 vs 协调"看成"一场关于多机体系结构的辩论"，并一路推广到 \(N\ge 3\) 的多臂、多移动机械臂、乃至多四足搬运。读完本章，你会发现第 5 章（grasp matrix 与协同搬运）、第 2 章（共识）、第 4 章（分布式 MPC）在"双臂"这个最小但最锋利的例子上**全部交汇**。

前置依赖：第 1 章（通信图、Laplacian、三大架构）、第 2 章（共识 \(\dot x=-Lx\)、最大值共识）、第 4 章（分布式 MPC、ADMM）、第 5 章（grasp matrix \(G\)、内力/运动力分解）；机械臂正逆运动学、雅可比与伪逆、阻抗/导纳控制基础（若薄弱，回 05_运动控制/20_机械臂/D01、F01）。

下游章节：第 8 章（多足协同 loco-manipulation——把"臂"换成"腿"，闭链数学完全复用）、第 9 章（人形/四足+臂层次化全身协同）。

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

---

前置自测

📋 答不出 ≥ 2 题 → 标 ① ② 的回第 1、2 章，标 ③ 的回第 5 章，标 ④ ⑤ 的回 05_运动控制/20_机械臂 的 D01/F01，再来读本章。本章会大量复用这些前置，但不重新教它们。

1. （接第 5 章） 写出 grasp matrix \(G\) 的定义：\(N\) 个接触点的接触力 \(f_{\text{contacts}}\in\mathbb{R}^{3N}\) 如何映射到负载 wrench \(F_{\text{load}}\in\mathbb{R}^6\)？\(G\) 的零空间 \(\ker(G)\) 的维度是多少，它在物理上代表什么？ （答不出 → 回第 5 章 §5.1）

2. （接第 2 章） 什么是**最大值共识**（max-consensus）？它和平均共识（average-consensus）的收敛目标有什么区别？为什么"在通信图上传播一个全局极值"所需的轮数正比于图的**直径**？本章在讲"主从架构里从臂如何知道主臂意图"时会再次用到这个直觉。 （答不出 → 回第 2 章）

3. （接第 5 章） 内力（internal force）与运动力（motion force）的分解公式 \(f_{\text{contacts}}=G^{+}F_{\text{load}}+(I-G^{+}G)f_{\text{internal}}\) 中，为什么 \((I-G^{+}G)f_{\text{internal}}\) 这一项"不产生负载运动"？把它代回 \(F_{\text{load}}=Gf_{\text{contacts}}\) 验证一下。 （答不出 → 回第 5 章 §5.1）

4. （接机械臂 D01） 双 7-DOF 臂共持一个刚性物体，约束流形的维度是多少？相对雅可比 \(J_{\text{rel}}\) 的零空间 \(\ker(J_{\text{rel}})\) 在物理上代表什么样的关节运动？ （答不出 → 回 05_运动控制/20_机械臂/D01 §D1.5）

5. （接力控 F01） 阻抗控制（impedance）与导纳控制（admittance）的二分法是什么？在"两臂之间夹着一个物体"的场景里，物体扮演的是"环境"的角色——这时应该用阻抗还是导纳？为什么？ （答不出 → 回 05_运动控制/20_机械臂/F01）

参考答案要点（先自己答，再对照）：

1. \(F_{\text{load}}=G\,f_{\text{contacts}}\)，其中 \(G\in\mathbb{R}^{6\times 3N}\) 的第 \(i\) 列块为 \(g_i=\begin{bmatrix}I_3\\ [r_i]_\times\end{bmatrix}\)，\(r_i\) 是接触点到负载质心的向量。当 \(N\ge 2\) 时 \(G\) 行满秩，\(\dim\ker(G)=3N-6\)——这个零空间就是**内力空间**：一组接触力的组合，合成到负载上的净 wrench 为零，但它们在物体内部"互相挤压/拉扯"。

2. 最大值共识让每个节点收敛到全网某个量的**最大值**（而非平均）；平均共识收敛到初始值的算术平均。最大值在图上的传播是"接力赛"——一个节点的值要传到最远的节点，需要的跳数等于两者间的最短路长度，全网达成一致的轮数就是图的直径。主从架构里"从臂获知主臂当前给负载的指令"本质就是一次信息在图上的传播。

3. 把 \((I-G^{+}G)f_{\text{internal}}\) 代入 \(G\cdot\)：\(G(I-G^{+}G)f_{\text{internal}}=(G-GG^{+}G)f_{\text{internal}}=(G-G)f_{\text{internal}}=0\)，用到了伪逆的性质 \(GG^{+}G=G\)。所以内力项落在 \(\ker(G)\) 中，对负载不产生任何净 wrench。

4. \(14-6=8\) 维约束流形。\(\ker(J_{\text{rel}})\) 是"使两臂末端相对位姿保持不变"的关节速度集合——在这些方向上运动，两臂如同被一根无限刚性的杆连接，同步运动，不挤压物体。\(J_{\text{rel}}\) 的行空间方向上的运动则会改变相对位姿，对刚性物体产生内力。

5. 阻抗控制：输入运动偏差，输出力（机器人表现得像弹簧-阻尼）；导纳控制：输入力，输出运动修正。两臂夹物体时物体是双臂之间的"虚拟环境"——应该用**阻抗**：让每只臂对"相对位姿偏差"表现出可控的刚度，从而把内力调成期望值。导纳需要力传感且在刚性接触下易失稳，不适合内力直接调控。

---

本章目标

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

1. 用多机协作的语言重述双臂操作：把"两臂共持物体"形式化为"\(N=2\) 个智能体通过共享刚性负载耦合"的特例，说明为什么 grasp matrix \(G\)（第 5 章）与相对雅可比 \(J_{\text{rel}}\)（机械臂 D01）是同一个闭链约束的两种投影——前者在**力空间**、后者在**速度空间**，并通过运动学-静力学对偶把它们连起来。
2. 推导并实现相对雅可比与绝对/相对分解：从 Uchiyama-Dauchez 对称表述出发，推导绝对变量（物体整体运动）与相对变量（两臂相对运动）的对偶分解，给出 Jamisola-Roberts 紧凑相对雅可比公式 \(J_{\text{rel}}=\begin{bmatrix}-\Psi\,{}^{R}\!J_L & {}^{R}\!J_R\end{bmatrix}\) 的来历，并理解非对称任务下的扩展相对雅可比。
3. 统一闭链约束的两个面孔：证明 \(\ker(J_{\text{rel}})\)（速度零空间，8 维）与 \(\ker(G)\)（力零空间/内力空间，\(3N-6\) 维）通过虚功原理互为对偶；说明"刚体把持"为什么同时是 6 维速度约束和 6 维 wrench 约束。
4. 设计内力控制律并推广到 N 臂：推导双臂内力 \(f_{\text{int}}=\tfrac12(f_R-f_L)\) 的物理含义，给出 Caccavale 内/外阻抗（object impedance + internal impedance）的双层结构，并把它推广为 \(N\) 臂的内力调节——理解为什么内力不能"开环施加"而必须闭环维护。
5. 在"主从 vs 协调"的架构辩论中做出有依据的选型：用第 1 章的集中/分布/去中心化三分法分析双臂/多臂控制，说明主从（master-slave）何时够用、何时会因"从臂只跟随不协商"而积累内力，对称协调（symmetric/non-master-slave）如何用绝对/相对解耦消除这个问题，以及 leader-follower 一致性如何把主从"软化"成可扩展的分布式方案。
6. 解决多臂（\(N\ge 3\)）负载分配问题：把"\(N\) 只手怎么分担一个负载的力"形式化为 grasp matrix 伪逆的零空间参数化 + QP 优化（最小化接触力范数 / 满足摩擦锥 / 兼顾各 agent 能力差异），并理解分布式版本（每个 agent 只用本地+邻居信息）与第 4 章 ADMM、第 2 章共识的接合点。
7. 厘清本章与单系统双臂章节、与第 5/8/9 章的边界：说清"什么内容在机械臂 D01-D04 学、什么在本章学、什么在第 5 章学"，避免重复也避免遗漏；理解"双臂→多臂→多移动机械臂→多四足搬运"是同一套闭链数学在不同硬件上的实例化。

---

本章知识导航

一句话定位：本章把"协同操作"从"一群机器人各自运动、互不接触"（第 1-4 章的主战场）推进到**它们通过一个共享的刚性负载在物理上锁死在一起**——这是多机耦合最强的一种形式，强到可以写出精确的代数约束（闭链）。双臂是这种耦合的"最小完整实例"：\(N=2\) 已经包含了内力、冗余、协调、主从所有核心矛盾，却小到能在一页纸上把矩阵写全。

本章七条主线及其递进关系：

主线	解决的问题	关键工具	对应小节
视角重置	为什么把双臂当"\(N=2\) 多机协同"而非"一台 14-DOF 机器"？	多机耦合谱、与第 1-5 章的连线	§7.1
协同运动学	两臂共持物体时，关节速度↔物体运动↔相对运动怎么映射？	相对雅可比、绝对/相对对偶分解、对称表述	§7.2
约束的两个面孔	速度约束 \(\ker(J_{\text{rel}})\) 与力约束 \(\ker(G)\) 是什么关系？	运动学-静力学对偶、虚功原理	§7.3
内力控制	怎么把内力调到期望值（既不松脱也不压坏物体）？	内力分解、object/internal 阻抗、\(N\) 臂推广	§7.4
架构辩论	主从？对称协调？谁主导谁跟随？	集中/分布/去中心化三分、leader-follower 一致性	§7.5
对称/非对称	两手等价 vs 一主一辅，数学上差在哪？	对称协调、扩展相对雅可比、任务空间分配	§7.6
多臂分配	\(N\ge 3\) 只手怎么分担力？分布式怎么做？	grasp 伪逆零空间、QP、摩擦锥、ADMM 分布式	§7.7

推荐阅读路径：§7.1 是视角地基（必读，它决定你怎么"看"后面所有内容）→ §7.2 是数学引擎（必读，相对雅可比是全章的核心对象）→ §7.3 是把第 5 章的力视角和 D01 的速度视角缝起来的关键一节（理论方向必读，工程方向可速读结论）→ §7.4 内力控制是"会用"的核心（做协同力控必读、需动手）→ §7.5 主从 vs 协调是体系结构思维的训练（必读，且与第 1 章呼应）→ §7.6 对称/非对称在做真实任务（拧瓶盖、装配）时必读 → §7.7 是推广到 \(N\ge 3\) 的收口（做多臂/多机搬运必读，纯双臂可速读）。§7.2、§7.4、§7.5、§7.7 是四个 ⭐⭐⭐/⭐⭐⭐⭐ 的硬核小节，值得花最多时间。

前置知识桥接

本章紧接第 5 章（协同搬运）与机械臂 D01（协调运动学），这里把最关键的前置点重新激活——你不必翻回去就能跟上：

• 回顾第 5 章：grasp matrix 与内力/运动力分解。第 5 章我们用 \(G\in\mathbb{R}^{6\times 3N}\) 把 \(N\) 个接触点的力映射到负载 wrench：\(F_{\text{load}}=Gf_{\text{contacts}}\)，并把接触力分解为运动力 \(G^{+}F_{\text{load}}\)（驱动负载运动）和内力 \((I-G^{+}G)f_{\text{int}}\)（落在 \(\ker(G)\)，互相挤压）。本章把双臂当成 \(N=2\) 的特例：两只手就是两个"接触点"，\(G\in\mathbb{R}^{6\times 12}\)（每个接触是 6 维 wrench 而非 3 维力，因为夹爪能传力矩），内力空间维度 \(12-6=6\)——恰好对应两臂之间的 6 维相对位姿可被"较劲"。第 5 章讲的是多四足/多臂在**力空间**的分配，本章把它和**速度空间**（相对雅可比）对偶起来，并深入到双臂特有的协调控制。
• 回顾第 2 章：共识作为"分布式裁决"的原子操作。第 2 章证明了共识能让各 agent 只靠邻居通信收敛到一致（平均共识 \(\dot x=-Lx\)、最大值共识）。本章 §7.5 的 leader-follower 内力调节本质是一种**带物理耦合的一致性**：每个 agent 维护"自己认为物体应该怎么动"的估计，通过共享负载的接触力（隐式通信）或显式邻居通信不断刷新，最终全网对"物体运动 + 内力分配"达成一致。第 1 章的通信图、第 2 章的共识在这里换了层皮再次出场——只不过"边"既可以是无线通信链路，也可以是穿过刚性物体的力。
• 回顾第 4 章：分布式 MPC 与 ADMM。第 4 章我们用 ADMM 把一个大的联合优化拆成各 agent 的本地子问题 + 一致性约束。本章 §7.7 的多臂负载分配，集中式版本是一个 QP（在 grasp 零空间里优化内力满足摩擦锥），分布式版本正是用 ADMM/对偶分解把这个 QP 拆给各 agent——你会看到第 4 章的机器在"力分配"这个新载荷上再跑一遍。
• 回顾机械臂 D01：相对雅可比与零空间投影。D01 推导了 \(J_{\text{rel}}=\bar J_R-\bar J_L\in\mathbb{R}^{6\times 14}\)（先用平移算子 \(X(p_c-p_i)\) 把两臂 twist 统一到公共参考点，再做差），并定义零空间投影 \(P=I-J_{\text{rel}}^{+}J_{\text{rel}}\)。本章会从**对称表述**的角度重新推导它（绝对/相对变量分解），让你看到"\(J_{\text{rel}}=\bar J_R-\bar J_L\)"只是 Uchiyama-Dauchez 对称框架的一个特例，并把它推广到非对称任务和 \(N\) 臂。

如果跳过本章会怎样

• 场景一（把双臂当两个独立单臂，物体被"撕裂"）：你有一台人形机器人，想让它双手端着一个托盘走过来。你图省事，左手跑一个独立的笛卡尔阻抗去跟踪"左手目标轨迹"，右手跑另一个独立阻抗跟踪"右手目标轨迹"，两个控制器互不通气。结果托盘上的杯子先是轻微晃动，几秒后突然被两只手一边拉一边推——内力失控增长，要么杯子被挤碎，要么托盘从一只手里滑脱。根本原因：两个独立控制器各自的微小跟踪误差在**相对位姿**上累积，而没有任何机制把这个误差识别为"内力"并主动调零。§7.2-7.4 会告诉你：必须把 14 个关节当成一个整体，用相对雅可比把运动拆成"物体运动（绝对）+ 相对运动（内力）"两个解耦通道，对前者做运动控制、对后者做力控——这正是单臂经验**不能**直接照搬到双臂的地方。
• 场景二（多臂搬运时力分配拍脑袋，某只手率先饱和）：你让四台移动机械臂抬一块大钢板。你想当然地"四只手平均分担重量，每只手 1/4"。但钢板质心偏向一侧、四只手的臂展和电机扭矩各不相同，平均分配让靠近质心、扭矩最小的那只手最先达到力矩极限并饱和——它一饱和，它该承担的力瞬间转嫁给邻居，引发连锁过载，整块板倾斜甚至掉落。§7.7 会告诉你：\(N\ge 3\) 时负载分配有 \(3N-6\) 维（或 \(6N-6\) 维，含力矩）的内力自由度可以优化，应该用 QP 在 grasp 零空间里最小化"最大归一化接触力"或显式编码各 agent 的能力差异，而不是均分；而要让这件事在没有中央节点时也能做，得用第 4 章的 ADMM 把 QP 分布式化。

预计阅读时间

模式	时长	适合
精读（跟着推完相对雅可比的对称分解、\(\ker(J_{\text{rel}})\)/\(\ker(G)\) 对偶、内力控制律、\(N\) 臂 QP 分配，并在 Pinocchio+MuJoCo 里实现双臂协调阻抗 + 三臂力分配）	18-26 小时	第一次系统学协同操作的算法工程师
速读（读懂相对雅可比、绝对/相对分解、主从 vs 协调的取舍、\(N\) 臂分配的 QP 形式，跳过推导与代码细节）	5-7 小时	已有机械臂 D01 背景，查漏补缺、补"多机视角"
速查（相对雅可比公式 + 内力分解 + 主从/协调对比表 + \(N\) 臂 QP 模板 + 与各章关系表 + 故障排查）	40-60 分钟	实现协同力控时回查

---

科研发展脉络

在钻进具体方法前，先把这条研究线的来龙去脉理清——双臂/多臂协同操作有近 40 年历史，知道每个方法"从哪来、解决了前人什么痛点、又留下什么给后人"，比孤立地记名字有用得多。

年份	论文/工作	Venue	核心贡献（从多机协作视角看）
1987	Uchiyama, Dauchez, "A symmetric hybrid position/force control scheme for the coordination of two robots"	ICRA	对称表述奠基：提出绝对/相对变量分解，明确反对"主从"——两臂地位对等，物体运动与内力解耦控制
1988	Khatib, "Object Manipulation with Whole-Body Control" / "Augmented Object"	IJRR / 后续	augmented object 方法：把负载作为虚拟物体插入动力学，统一多接触点的操作空间动力学
1993	Uchiyama, Dauchez, "Symmetric Kinematic Formulation and Non-Master/Slave Coordinated Control"	Advanced Robotics	把对称表述系统化为完整的非主从协调控制框架——本章 §7.5/§7.6 的理论源头
1996	Chiacchio, Chiaverini, Sciavicco, Siciliano, "Task space dynamic analysis of multiarm system configurations"	IJRR	绝对/相对变量的现代形式：右减左的相对位姿约定、增广雅可比，被后续工程普遍采用
2008	Caccavale, Chiacchio, Marino, Villani, "Six-DOF Impedance Control of Dual-Arm Cooperative Manipulators"	IEEE/ASME T-Mech	内/外阻抗双层：object impedance（外，物体级柔顺）+ internal impedance（内，调控内力），本章 §7.4 主线
2015	Jamisola, Roberts, "A more compact expression of relative Jacobian based on individual manipulator Jacobians"	Robotics & Autonomous Systems	紧凑相对雅可比：直接用各臂自身雅可比拼出 \(J_{\text{rel}}\)，无需重建整链，工程友好
2017	Erhart, Hirche, "Internal Force Analysis and Load Distribution for Cooperative Multi-Robot Manipulation"	IEEE T-RO	多机内力的严格分析：指出 grasp 伪逆的内力依赖末端运动学，给出无内力广义逆的参数化——\(N\) 臂分配的理论基础
2019	Almeida, Lima 等, "Asymmetric Dual-Arm Task Execution using an Extended Relative Jacobian"	arXiv/IROS	扩展相对雅可比：非对称任务（一主一辅）下同时控制相对运动 + 一只手的绝对运动，本章 §7.6
2019	Verginis, Dimarogonas, "Cooperative Manipulation via Internal Force Regulation: A Rigidity Theory Perspective"	(T-CST/arXiv)	刚性理论视角：把 grasp 矩阵与刚性矩阵的零空间-值空间关系连起来，分布式内力调节，无需负载惯性
2022	Zhao, Finn 等, "Learning Fine-Grained Bimanual Manipulation with Low-Cost Hardware (ALOHA)"	RSS	学习范式转向：低成本遥操作 + ACT，双臂技能从"建模控制"转向"模仿学习"（本章 §7.8 简述，深入见机械臂 D04）
2024-2025	Mobile ALOHA / π₀ / 双臂操作综述	CoRL/arXiv	移动双臂、VLA 直出双臂动作、leader-follower 动作分解——学习与经典控制的融合

关键实验室脉络：Tohoku/早稻田 Uchiyama（对称表述原创）→ Stanford Khatib（augmented object 操作空间动力学）→ Napoli Siciliano/Caccavale（绝对/相对变量 + 内外阻抗工程化）→ TUM Hirche/Erhart（多机内力严格分析）→ KTH Dimarogonas/Verginis（分布式内力 + 刚性理论）→ Stanford Zhao/Finn（ALOHA 学习范式）。

看这条线，有两条清晰的主线交织：经典控制线**从"主从"走向"对称协调"再走向"分布式内力调节"，每一步都在**去中心化、减少对单一主导者和全局信息的依赖——这与第 1-4 章"集中式→分布式→去中心化"的大主题完全同构；**学习线**从 2022 年 ALOHA 起异军突起，把"怎么协调"从人工设计的控制律变成从演示数据里学。本章主体讲经典控制线（因为它是"可推广、可分析、可移植到多机"的部分），学习线在 §7.8 简述并指向机械臂 D04。

---

本章符号约定

符号	含义	首见
\(N\)	协同操作的臂/接触点数目（双臂 \(N=2\)）	§7.1
\(q\)	联合关节向量 \(q=[q_1^\top,\dots,q_N^\top]^\top\)，双 7-DOF 时 \(q\in\mathbb{R}^{14}\)	§7.2
\(J_i\)	第 \(i\) 臂末端的几何雅可比（在联合关节空间表达，\(\mathbb{R}^{6\times n}\)）	§7.2
\(\mathcal{V}_i=[v_i;\omega_i]\)	第 \(i\) 臂末端 twist（本文 \([\text{线};\text{角}]\) 顺序）	§7.2
\(J_{\text{aug}}\)	增广雅可比，\([J_1;\dots;J_N]\in\mathbb{R}^{6N\times n}\)	§7.2
\(J_{\text{rel}}\)	相对雅可比，\(\mathcal{V}_{\text{rel}}=J_{\text{rel}}\dot q\in\mathbb{R}^6\)（双臂）	§7.2
\(J_{\text{abs}}\)	绝对雅可比，\(\mathcal{V}_{\text{abs}}=J_{\text{abs}}\dot q\in\mathbb{R}^6\)	§7.2
\(X(d)\)	twist 平移算子 \(\begin{bmatrix}I&-[d]_\times\\0&I\end{bmatrix}\)（\([v;\omega]\) 顺序）	§7.2
\(T_i,\,T_{\text{abs}}\)	第 \(i\) 末端位姿、绝对（物体）位姿 \(\in SE(3)\)	§7.2
\(G\)	grasp matrix，\(F_{\text{load}}=G\,h\)，\(\in\mathbb{R}^{6\times 6N}\)（含力矩）或 \(\mathbb{R}^{6\times 3N}\)（仅力）	§7.3
\(h=[h_1;\dots;h_N]\)	各末端施加到物体的 wrench 堆叠	§7.3
\(f_{\text{int}}\)	内力（wrench），落在 \(\ker(G)\)	§7.3
\(W\)	内力子空间的基矩阵（\(\text{Im}(W)=\ker(G)\)）	§7.3
\(K_d,D_d,M_d\)	阻抗刚度/阻尼/惯量矩阵	§7.4
\(L\)	通信图 Laplacian（第 1 章）	§7.5

---

§7.1 视角重置：把双臂当成"\(N=2\) 的多机协同操作" ⭐⭐

这一节解决什么问题：在写任何公式之前，先回答一个"元问题"——同一个"两臂共持物体"的系统，可以有两种看法：(A) 它是**一台 14 自由度的机器**，用一套大雅可比、大动力学描述（这是 05_运动控制/20_机械臂 D01-D04 的立场）；(B) 它是**两个智能体通过共享负载耦合的多机系统**（本章立场）。这一节论证：视角 (B) 不是文字游戏，而是能让你把第 1-5 章学的所有多机工具——共识、任务分配、分布式优化、grasp matrix——直接搬过来，并且天然地推广到 \(N\ge 3\) 和异构多机。

动机：为什么"两个看法"会导出不同的工程

设想一个具体场景：两台 7-DOF 臂端着一块玻璃。现在问你三个问题：

1. 玻璃应该往哪走、走多快？
2. 两只手"夹"玻璃的力应该多大？
3. 如果其中一只手的电机临时过热降功率，系统怎么办？

如果你持视角 (A)——"一台 14-DOF 机器"——你会本能地写一个大的逆运动学/逆动力学，把"玻璃位姿"和"夹持力"塞进同一个任务向量里求解。这没错，机械臂 D01-D03 正是这么做的，而且做得很深。但当我问到第 3 个问题时，视角 (A) 开始别扭：一台机器的"一个电机降功率"在数学上是某个关节力矩约束变了，你得重新组装整个 QP——它没有"某个 agent 能力下降，把它的份额转给邻居"这种**天然的模块边界**。

如果你持视角 (B)——"两个智能体"——三个问题立刻对应到第 1-5 章的三个熟悉框架：

问题	多机协作视角下的对应	在本课程的出处
玻璃往哪走、多快	团队对"共同目标（物体轨迹）"达成一致	第 2 章共识 / 第 4 章分布式 MPC
两手夹持力多大	内力分配 = grasp 零空间里的力优化	第 5 章 grasp matrix + 本章 §7.4/§7.7
一只手降功率怎么办	某 agent 能力下降 → 重新分配负载份额	第 5 章自适应分配 / 本章 §7.7 QP

本质洞察：双臂操作之所以是"多机协作"的最小完整实例，是因为它在 \(N=2\)、自由度最少、还能把所有矩阵写满的前提下，**同时具备**多机系统的全部核心矛盾——共享目标的一致（物体往哪走）、资源的分配（力怎么分）、容错与异构（一只手坏了/能力不同）。它比"两台机器人在走廊里避让"（第 3 章 MAPF）耦合强得多（那里只有碰撞约束这种不等式弱耦合，这里是穿过刚体的等式强耦合），却又比"十台机器人搬一栋楼"简单得多（\(N=2\) 没有可扩展性爆炸）。研究双臂，是在最干净的实验台上研究多机耦合本身。

如果只用视角 (A) 会怎样——反面教材

视角 (A)（单台大机器）不是错的，而是**会让你看不见某些结构**，从而在三类情况下吃亏。

吃亏一：异构与容错被埋没。 当两条臂型号不同（一条 Franka 一条 UR10e，臂展、载荷、控制频率都不一样），或当系统从"两臂"扩展到"两台移动双臂机器人各出一只手"时，视角 (A) 的"一台 14-DOF 机器"假设崩塌——它默认所有自由度共享同一个时钟、同一个求解器、同一份全局状态。视角 (B) 从一开始就假设"每个 agent 有自己的控制器、只能观测局部 + 邻居"，异构和容错是它的出厂设定。

吃亏二：通信约束被忽略。 单台机器的关节之间通过板内总线通信，延迟可忽略、带宽近乎无限——视角 (A) 顺理成章地假设"全局状态随时可得"。但真实的多臂系统里，两条臂可能挂在不同的工控机上，通过以太网/DDS 交换状态，有毫秒级延迟和丢包。视角 (B)（继承第 1-2 章）把通信图 \(G\)、延迟、拓扑变化当成一等公民——这正是为什么本章 §7.5 要认真区分"主从（一个全局主导者）vs 分布式（各自只用邻居信息）"。

吃亏三：与课程其余部分割裂。 如果用视角 (A) 写本章，它会变成机械臂 D01-D04 的一个缩写版，和第 1-5 章（共识、任务分配、分布式 MPC、grasp matrix）没有任何连线——学生学完不知道"这和我前面学的多机有什么关系"。用视角 (B)，本章成为"第 5 章（力空间分配）+ 第 2 章（共识）+ 第 4 章（分布式优化）在协同操作上的汇流"，知识形成网络而非孤岛。

反事实推理：如果把双臂从课程的"多机协作"部分删掉，只在机械臂方向讲，会丢什么？会丢掉"协同操作是协同运动的特例还是另一回事"这个关键认知。第 4 章讲多机**协同运动**（不接触，靠通信协调轨迹），第 5 章讲多机**协同搬运**（接触，但常把每只手简化为一个施力点），本章讲多机**协同操作**（接触，且每只手是有完整运动学的冗余臂，能做在手重定向、内力精调）。三者是"耦合强度递增"的谱：协同运动（通信耦合）→ 协同搬运（力耦合，点接触）→ 协同操作（力+运动耦合，刚体把持）。删掉双臂，这个谱就断了一截。

多机协作视角下的双臂：一张"翻译表"

为了让"视角 (B)"落到实处，下面这张表把双臂操作的每个对象"翻译"成多机协作的标准语言。后面每一节都会兑现表里的某一行。

双臂操作里的对象	多机协作里的对应概念	本章兑现处
左臂、右臂	两个智能体（agent）	全章
共持的刚性物体	把两个 agent 锁在一起的**共享约束载体**（强耦合边）	§7.2-7.3
物体的目标轨迹	团队的共同目标（需达成一致）	§7.2、§7.5
两手相对位姿不变（刚体约束）	等式耦合约束 \(\to\) 速度空间 \(\ker(J_{\text{rel}})\) / 力空间 \(\ker(G)\)	§7.2-7.3
夹持内力	内力 = 共享约束的"对偶变量"，落在 grasp 零空间	§7.3-7.4
主臂主导、从臂跟随	集中式 / leader-follower 架构	§7.5
两臂地位对等、协商运动	分布式 / 去中心化（对称协调）	§7.5-7.6
一只手扶、一只手操作	异构角色分工（任务分配的退化形式）	§7.6
\(N\) 只手抬大物体的力分配	多机资源分配（QP / ADMM 分布式）	§7.7
一只手能力下降	异构 + 容错（份额再分配）	§7.7

跨领域类比（带边界）：双臂协同操作 vs 两个人合抬一张桌子。像的部分：都要对"桌子往哪搬"达成默契（共同目标一致），都会无意识地调节"夹/托"的力让桌子不晃（内力调节），都可能一个人主导一个人跟随（主从）或两人平等协商（对称）。不像的部分：人靠视觉、语言、几十年的身体经验隐式协调，机器人必须把这一切写成显式的雅可比、grasp matrix 和控制律；人对内力的感知是连续自适应的，机器人受限于力传感器精度和控制频率；人有近乎无限的"通信带宽"（一个眼神），机器人受 DDS 延迟约束。不要把类比延伸到："既然人能盲抬，机器人也该能不通信协同"——人的"盲抬"其实重度依赖通过桌子传递的力觉反馈（隐式通信），这恰恰对应 §7.5 要讲的"通过接触力隐式协调"，不是真的零信息。

本章在"耦合强度谱"上的精确位置

把第 1-9 章按"agent 之间耦合的强度"排成一条谱，本章的位置就一目了然：

弱耦合 ←─────────────────────────────────────────────→ 强耦合

通信耦合          碰撞耦合         力耦合(点)        力+运动耦合(刚体)
(共识/任务分配)    (MAPF避让)      (协同搬运,点接触)   (协同操作,刚体把持)
   │                 │                │                   │
 第2,3章           第3章            第5章               【本章 第7章】
 ↓                                                       ↓
"只交换信息,        "共享空间,        "共享负载,          "共享负载+完整臂运动学,
 不接触"            不许撞"          分担力"             还要精调内力与在手运动"

• 比第 3 章（MAPF）强：MAPF 里 agent 间是**不等式弱耦合**（\(d(q_i,q_j)\ge d_{\min}\)，别撞上就行），可以解耦近似。本章是**等式强耦合**（刚体把持 \(h(q)=0\)），不能解耦——一只手动了，另一只手必须精确跟随到亚毫米，否则内力爆炸。
• 比第 5 章（协同搬运）更细：第 5 章常把每个 agent 对负载的作用简化为"一个接触点施一个力/wrench"，关心的是**力怎么分**（grasp matrix 层面）。本章保留每只手是**有完整运动学的冗余臂**这一事实，于是除了"力怎么分"，还要管"两臂运动学怎么协调（相对雅可比）"、"在手重定向怎么做（相对运动）"、"冗余零空间怎么用（自碰撞回避）"——这些在"点接触"抽象下看不见。
• 是第 8、9 章的数学母板：第 8 章把"臂"换成"腿"（多足协同 loco-manipulation，每条腿是接触点+运动学），第 9 章把双臂装到浮动基座上（人形全身协同）。本章的闭链约束、相对雅可比、内力分配在那两章**原样复用**，只是雅可比里多了浮动基座的列、约束里多了接触维持。

⚠️ 常见陷阱

🧠 思维陷阱：认为"多机协作视角只是换个说法，对实现没影响"
   新手想法："不管叫'14-DOF 机器'还是'两个 agent'，最后解的都是同一个 QP，
            视角是哲学问题，工程上无所谓。"
   实际上：视角决定了你给系统画的"模块边界"，而模块边界决定了代码结构、
          容错策略和可扩展性。视角 (A) 自然导出一个单体大求解器（所有自由度
          耦合在一个 KKT 系统里），加一条臂要重写矩阵维度；视角 (B) 自然导出
          "每 agent 一个本地控制器 + 一致性层"，加一条臂只是多一个同构模块。
          当你的项目从"双工业臂"演化到"两台移动双臂机器人协同"（4 只手、
          分布在 2 个计算节点）时，视角 (A) 的代码几乎要重写，视角 (B) 的
          代码增量扩展即可。
   正确理解：视角是架构决策的源头，不是修辞。本章选 (B) 是为了让双臂的
            知识能无缝长进多机系统，而不是停在单系统操作。

💡 概念误区：认为"协同操作 = 协同搬运"
   新手想法："第 5 章已经讲了多机搬物体，本章是不是重复？"
   实际上：协同搬运（第 5 章）关注力分配，常把 agent 简化为施力点，
          适合"四足/移动平台抬大件"这种"末端运动学不重要、力分配重要"的场景；
          协同操作（本章）关注的是"有灵巧末端的臂如何在共持的同时做精细协调"——
          在手重定向、内力精调、相对运动控制，这些需要完整的相对雅可比，
          在点接触抽象下根本无法表达。两章是"力空间"与"力+运动空间"的递进。
   正确理解：搬运是操作的"只关心力"的子集；操作多了一层运动学协调。

练习

1. [概念] 把"两台移动双臂机器人（各 7+7 DOF 臂 + 全向底盘）合作把一个沙发抬过门"这个任务，用本节的"翻译表"逐行翻译成多机协作语言：谁是 agent？共享约束载体是什么？共同目标、内力、主从/对称分别对应什么？这个系统在"耦合强度谱"上落在哪里，和纯双臂相比多了哪些维度？

2. [对比] 列出 3 个"用视角 (A)（单台大机器）更方便"的场景和 3 个"用视角 (B)（多机）更方便"的场景，并说明判据。提示：考虑 agent 数目、是否异构、是否分布式计算、是否需要容错。

3. [跨章综合] 回顾第 3 章的任务分配（谁干哪件任务）。本章 §7.6 会讲"一只手扶、一只手操作"的非对称角色分工。论证：双臂的"角色分配（哪只手当主、哪只手当辅）"是不是第 3 章任务分配问题的一个退化特例（\(N=2\) 个 agent、2 个角色）？它和一般任务分配的关键区别是什么（提示：两个角色之间有强物理耦合，不能独立执行）？

---

§7.2 协同运动学：相对雅可比与绝对/相对分解 ⭐⭐⭐

这一节解决什么问题：两臂共持物体时，14 个关节速度、物体的 6 维运动、两臂的 6 维相对运动，这三者之间是什么映射？我们要建立一组**解耦的坐标**——把"物体整体往哪走"（绝对）和"两手相对位姿变没变"（相对）拆成两个互不干扰的通道，因为前者要做运动控制、后者要做力控（内力）。这套坐标的核心就是 Uchiyama-Dauchez 的**对称表述**与由它导出的**相对雅可比**。机械臂 D01 已经给出了 \(J_{\text{rel}}=\bar J_R-\bar J_L\) 的工程形式；本节从对称表述的**第一性原理**重新推导它，让你看清它为什么长这样、为什么"右减左"、以及怎么推广到非对称任务和 \(N\) 臂。

动机：独立控制两只手为什么不行

最朴素的想法：给左手一个目标位姿 \(T_L^{\text{des}}(t)\)、给右手一个目标位姿 \(T_R^{\text{des}}(t)\)，各跑各的逆运动学。只要这两个目标"事先算好是相容的"（即对应同一个物体位姿），理论上物体就被正确搬运了。

这个想法在**开环、无误差**的理想世界里成立，但一接触现实就崩。问题出在：两个独立控制器各有各的跟踪误差 \(e_L,e_R\)，而真正伤害物体的不是 \(e_L\) 或 \(e_R\) 本身，而是它们的**差**所对应的相对位姿偏差。

独立控制的失效链条：

  左手实际位姿 = T_L^des · exp(e_L)      e_L ~ 0.5mm 级跟踪误差
  右手实际位姿 = T_R^des · exp(e_R)      e_R ~ 0.5mm 级跟踪误差

  两手相对位姿误差 ≈ e_R - e_L           可达 1mm 级
       │
       ▼
  刚性物体被"拉伸/挤压" 1mm
       │
       ▼
  内力 = 刚度 × 形变 = K_object × 1mm
       │  (钢的等效刚度可达 10^7 N/m 级)
       ▼
  内力 ~ 10^4 N！物体被压碎 或 夹爪打滑

本质洞察：独立控制的根本缺陷是它把"绝对运动"和"相对运动"耦合在每只手的位姿误差里——你无法单独惩罚"相对误差"而放过"绝对误差"。而协同操作的物理本质要求**对这两者用完全不同的控制律**：绝对运动是位置任务（物体要精确到达目标），相对运动是力任务（相对位姿允许微小偏移，但偏移对应的内力要被精确调控）。要分开控制，就必须先有一组把它们解耦的坐标——这正是绝对/相对分解的全部动机。

Uchiyama-Dauchez 对称表述：绝对变量与相对变量 ⭐⭐⭐

1987-1993 年，Uchiyama 和 Dauchez 提出了一个影响至今的思想：不要指定一个主、一个从（主从表述会人为打破两臂的对称性，下面 §7.5 详述其代价），而是用两个**对称的**物理量描述双臂状态：

• 绝对位姿（absolute pose） \(T_{\text{abs}}\)：两末端的"中点位姿"——代表被把持物体的整体位姿。
• 相对位姿（relative pose） \(T_{\text{rel}}\)：右末端相对左末端的位姿——代表两手之间的几何关系，刚性把持时它应当恒定。

为什么这组坐标是"对的"？ 因为它和任务的物理结构对齐：

坐标	物理意义	任务类型	期望行为
\(T_{\text{abs}}\)（绝对）	物体往哪走	运动任务	精确跟踪目标轨迹
\(T_{\text{rel}}\)（相对）	两手"较劲"程度	力任务（内力）	保持恒定（刚体）或受控变化（在手操作）

下面给出速度层的精确定义（位姿层用 \(SE(3)\) 的对数映射，思路相同但记号繁琐，速度层足以支撑控制）。设两末端 twist 为 \(\mathcal{V}_L,\mathcal{V}_R\in\mathbb{R}^6\)，都已通过 D01 的平移算子 \(X(p_c-p_i)\) 统一到公共参考点 \(p_c\)（通常取物体质心或两末端中点）——记统一后的 twist 为 \(\bar{\mathcal{V}}_L,\bar{\mathcal{V}}_R\)。

关键前提（D01 已强调，这里重申）：直接拿 LOCAL_WORLD_ALIGNED 雅可比做 \(J_R-J_L\) 是**错的**。LOCAL_WORLD_ALIGNED 只统一了坐标轴方向，没有把线速度参考点移到同一点。刚体绕物体质心转动时，两末端线速度本来就不同，不统一参考点就做差，会把"正常的绕物体转动"误判成"相对运动"，从而误算出根本不存在的内力。统一参考点的算子是 $\(X(d)=\begin{bmatrix}I_3 & -[d]_\times\\ 0 & I_3\end{bmatrix},\quad d=p_c-p_i,\quad \bar{\mathcal{V}}_i=X(p_c-p_i)\,\mathcal{V}_i\)$ （本文 twist 顺序 \([v;\omega]\)；若用 \([\omega;v]\)，块的位置和符号要相应置换。）

绝对 twist（物体整体运动） 取两末端统一 twist 的平均：

\[\boxed{\;\mathcal{V}_{\text{abs}}=\tfrac12\left(\bar{\mathcal{V}}_L+\bar{\mathcal{V}}_R\right)\;}\]

相对 twist（两手相对运动） 取右减左：

\[\boxed{\;\mathcal{V}_{\text{rel}}=\bar{\mathcal{V}}_R-\bar{\mathcal{V}}_L\;}\]

这两个定义看似随意，其实有深刻的对称性：它们是把 \((\bar{\mathcal{V}}_L,\bar{\mathcal{V}}_R)\) 这对量做了一个**正交变换**（求和取半 + 求差），把"两个末端速度"变成"质心速度 + 相对速度"——和力学里把两质点运动分解为"质心运动 + 相对运动"是同一个数学操作（雅可比坐标变换）。

跨领域类比（带边界）：绝对/相对分解 vs 二体问题的质心-相对坐标。在天体力学里，两个相互引力作用的天体，我们不用 \((\vec r_1,\vec r_2)\) 而用"质心位置 \(\vec R=\tfrac{m_1\vec r_1+m_2\vec r_2}{m_1+m_2}\) + 相对位置 \(\vec r=\vec r_2-\vec r_1\)"——因为质心做匀速直线运动、相对坐标满足约化质量的单体方程，两者解耦。双臂的绝对/相对分解动机完全相同：解耦"物体整体运动"和"两手相对运动"。像的部分：都是把"两个个体的坐标"线性变换为"集体坐标+相对坐标"以解耦动力学。不像的部分：二体问题里两个坐标的动力学严格解耦（质心绝不受内力影响）；双臂里这种解耦是**控制器刻意制造的**（通过把关节速度投影到不同子空间），物理上若控制不当，相对运动产生的内力反作用仍会通过物体动力学耦合回绝对运动。不要把类比延伸到："既然质心运动不受内力影响，物体整体运动也不受内力影响"——不对，那是天体的自然律，双臂里要靠控制器主动维持这个解耦。

从对称变量到雅可比 ⭐⭐⭐

把上面两个 twist 写成关于关节速度 \(\dot q\) 的线性映射，就得到绝对雅可比和相对雅可比。

由 \(\bar{\mathcal{V}}_i=\bar J_i\dot q\)（其中 \(\bar J_i=X(p_c-p_i)J_i\) 是统一参考点后的雅可比，\(J_i\in\mathbb{R}^{6\times n}\) 是第 \(i\) 臂在联合关节空间的几何雅可比）：

\[\mathcal{V}_{\text{abs}}=\tfrac12(\bar J_L+\bar J_R)\dot q=J_{\text{abs}}\dot q,\qquad J_{\text{abs}}=\tfrac12(\bar J_L+\bar J_R)\in\mathbb{R}^{6\times n}\]

\[\mathcal{V}_{\text{rel}}=(\bar J_R-\bar J_L)\dot q=J_{\text{rel}}\dot q,\qquad \boxed{J_{\text{rel}}=\bar J_R-\bar J_L\in\mathbb{R}^{6\times n}}\]

这就**重新推出了 D01 给出的 \(J_{\text{rel}}=\bar J_R-\bar J_L\)**——但现在你知道它的来历：它是对称表述里"相对变量"的雅可比，"右减左"的符号是相对位姿定义的直接后果，而不是某个随意约定。同时我们附带得到了它的"对偶搭档"\(J_{\text{abs}}\)，这是 D01 没有强调但内力控制必需的。

把两者堆叠，就是把 \((\mathcal{V}_{\text{abs}},\mathcal{V}_{\text{rel}})\) 当作新的任务坐标：

\[\begin{bmatrix}\mathcal{V}_{\text{abs}}\\ \mathcal{V}_{\text{rel}}\end{bmatrix}=\underbrace{\begin{bmatrix}\tfrac12(\bar J_L+\bar J_R)\\ \bar J_R-\bar J_L\end{bmatrix}}_{J_{\text{coop}}\in\mathbb{R}^{12\times n}}\dot q\]

对双 7-DOF（\(n=14\)），\(J_{\text{coop}}\in\mathbb{R}^{12\times 14}\)，秩 12（远离奇异），零空间维度 \(14-12=2\)——这 2 维是双臂的**冗余**，可用于自碰撞回避、操作度优化、关节限位回避（D01 §D1.4 已详述零空间的三大用途，本章不重复）。

这个分解的威力在哪？ 它把一个 12 维任务拆成两个语义清晰、可分别赋予不同控制律的 6 维通道：

                         ┌─ V_abs (6维) ──→ 运动控制：跟踪物体目标轨迹
   q̇ ──[ J_coop ]──┤
                         └─ V_rel (6维) ──→ 力控/约束：
                                            刚体把持时令 V_rel = 0（保持相对位姿）
                                            在手操作时让 V_rel 跟踪期望相对运动
                                            内力通过相对位姿偏差 × 刚度产生

Jamisola-Roberts 紧凑相对雅可比：工程上怎么"拼" ⭐⭐⭐

上面的推导用了"先统一参考点 \(\bar J_i=X(p_c-p_i)J_i\) 再做差"。Jamisola 与 Roberts（2015）给出了一个**工程上更顺手**的等价表达——直接用各臂在**各自基座**计算的标准雅可比，配上旋转/wrench 变换矩阵拼出 \(J_{\text{rel}}\)，无需把双臂重建成一条长运动链。

核心思想：把"右末端相对左末端的运动"表达出来，需要：(1) 把右臂雅可比变换到左末端参考系；(2) 把左臂雅可比（它描述左末端自身运动，而左末端是"相对运动"的参考基座）取负并做相应变换。结果形如：

\[J_{\text{rel}}=\begin{bmatrix}-\,{}^{R_{\!E}}T_{L_{\!E}}\,\Psi_L\,{}^{L}\!J_L & \quad {}^{R_{\!E}}T_{L_{\!E}}\,{}^{R}\!J_R\end{bmatrix}\]

其中 \({}^{L}\!J_L,{}^{R}\!J_R\) 是两臂在各自坐标系下的标准几何雅可比，\({}^{R_{\!E}}T_{L_{\!E}}\) 是把 twist 从左末端系变换到右末端系的 \(6\times6\) 伴随（adjoint）矩阵，\(\Psi_L\) 是处理"左末端作为相对运动参考基座"时的附加变换。

公式中各变换矩阵的精确形式依赖坐标系约定，细节见 Jamisola-Roberts 2015 原文与 D01。这里要记住的不是矩阵的每一项，而是三个工程结论： 1. 可组合性：\(J_{\text{rel}}\) 可以由两臂"现成的"标准雅可比 + 几个 \(SE(3)\) 变换矩阵拼出来——你不必为双臂系统单独写一套运动学，直接复用单臂的 getFrameJacobian，这对代码复用极其友好。 2. 左臂列带负号：左末端是相对运动的"参考者"，它的运动以相反方式贡献到相对 twist——这是 \(-\bar J_L\) 在紧凑形式里的体现，和我们第一性原理推导一致。 3. 任意一臂可作参考：选左末端还是右末端还是物体质心作参考点，只改变变换矩阵，不改变"相对雅可比 = 一臂雅可比变换后减另一臂雅可比变换"的结构。

# ============================================================
# 相对雅可比 + 绝对雅可比——Pinocchio Python 实战（对称表述版）
# 强调：先用平移算子统一参考点，再做 和/差
# ============================================================
import pinocchio as pin
import numpy as np

def skew(v):
    return np.array([[0, -v[2], v[1]],
                     [v[2], 0, -v[0]],
                     [-v[1], v[0], 0]])

def twist_translate(d):
    """twist 平移算子 X(d)，[v; omega] 顺序：把参考点平移 d。"""
    X = np.eye(6)
    X[0:3, 3:6] = -skew(d)
    return X

model = pin.buildModelFromUrdf("dual_panda.urdf")
data  = model.createData()
L = model.getFrameId("panda_left_hand")
R = model.getFrameId("panda_right_hand")

q = pin.randomConfiguration(model)
pin.forwardKinematics(model, data, q)
pin.updateFramePlacements(model, data)
pin.computeJointJacobians(model, data, q)

# 各臂在 LOCAL_WORLD_ALIGNED 下的雅可比（轴向已对齐世界系，但参考点仍在各自末端）
J_L = pin.getFrameJacobian(model, data, L, pin.LOCAL_WORLD_ALIGNED)  # 6x14
J_R = pin.getFrameJacobian(model, data, R, pin.LOCAL_WORLD_ALIGNED)  # 6x14

p_L = data.oMf[L].translation
p_R = data.oMf[R].translation
p_c = 0.5 * (p_L + p_R)        # 公共参考点：两末端中点（也可取物体质心）

# 统一到公共参考点
Jbar_L = twist_translate(p_c - p_L) @ J_L
Jbar_R = twist_translate(p_c - p_R) @ J_R

# 对称表述的两个雅可比
J_abs = 0.5 * (Jbar_L + Jbar_R)   # 6x14 绝对（物体运动）
J_rel = Jbar_R - Jbar_L           # 6x14 相对（两手相对运动 / 内力通道）

J_coop = np.vstack([J_abs, J_rel])  # 12x14

# 验证：秩与冗余维度
S = np.linalg.svd(J_coop, compute_uv=False)
rank = int(np.sum(S > 1e-6))
print(f"rank(J_coop) = {rank}, 冗余(零空间)维度 = {14 - rank}")  # 期望 12, 2

# 验证：纯绝对运动（V_rel=0）确实不改变相对位姿
#       构造一个只在 J_abs 上有分量、在 J_rel 零空间里的关节速度
J_rel_pinv = np.linalg.pinv(J_rel, rcond=1e-6)
P_rel = np.eye(14) - J_rel_pinv @ J_rel        # 投影到 ker(J_rel)
dq = P_rel @ np.random.randn(14)
print(f"||V_rel|| under ker(J_rel) motion = {np.linalg.norm(J_rel @ dq):.2e}")  # ~1e-12
print(f"||V_abs|| under same motion       = {np.linalg.norm(J_abs @ dq):.3f}")   # 一般非零

工程建议：开发期用上面的 Python（对称表述 + 显式平移算子）做数值真值，确认 \(J_{\text{rel}}\)、\(J_{\text{abs}}\) 正确后，实时控制再切到 Jamisola-Roberts 紧凑形式或 C++ 复用单臂雅可比——后者省掉了显式平移，但容易在坐标系约定上犯错，必须用前者交叉验证。

协调逆运动学：两个解耦通道 ⭐⭐⭐

有了 \(J_{\text{abs}}\) 和 \(J_{\text{rel}}\)，刚体把持任务的协调逆运动学就是"绝对通道跟踪物体、相对通道锁零"：

\[\dot q=J_{\text{coop}}^{+}\begin{bmatrix}\mathcal{V}_{\text{abs}}^{\text{des}}\\ \mathcal{V}_{\text{rel}}^{\text{des}}\end{bmatrix}+\left(I-J_{\text{coop}}^{+}J_{\text{coop}}\right)\dot q_0\]

刚体把持时 \(\mathcal{V}_{\text{rel}}^{\text{des}}=0\)，\(\dot q_0\) 是冗余自任务（自碰撞回避等）。等价地，可以用分层（任务优先级）写法：先在 \(\ker(J_{\text{rel}})\) 里满足相对约束，再在其中实现绝对运动：

\[\dot q=\underbrace{P_{\text{rel}}}_{I-J_{\text{rel}}^{+}J_{\text{rel}}}\,J_{\text{abs}}^{+}\,\mathcal{V}_{\text{abs}}^{\text{des}}+\dots\]

一次式（combined）vs 分层式（hierarchical）：选哪个？ 上面给了两种写法，它们在无奇异、无冲突时结果接近，但在约束冲突时行为不同：

写法	绝对/相对的关系	冲突时行为	适用
一次式 \(J_{\text{coop}}^{+}[\mathcal{V}_{\text{abs}};\mathcal{V}_{\text{rel}}]\)	平权（同一伪逆里加权折中）	绝对与相对误差按伪逆"平摊"，可能都不精确	两通道同等重要、追求整体最小二乘
分层式 \(P_{\text{rel}}J_{\text{abs}}^{+}\mathcal{V}_{\text{abs}}\)	相对优先（先满足相对约束，绝对在其零空间里尽力）	相对约束**严格优先**满足，绝对运动让步	内力安全>物体精度（刚体把持的默认选择）

本质洞察：刚体把持任务里应优先用**分层式（相对约束优先）**——因为"不挤压物体"（相对约束）是硬安全约束，"物体精确到位"（绝对）是可让步的性能目标。一次式把两者平权折中，冲突时可能轻微违反相对约束（产生内力）以换取绝对精度，这在易碎物上不可接受。这与机械臂 WBC 的任务优先级（TSID/分层 QP）是同一思想：安全/约束类任务放高优先级，性能类任务放低优先级，低优先级在高优先级零空间里执行。\(N\) 臂时这进一步泛化为 §7.7 的"硬约束（\(Gh=F\)、摩擦锥）+ 软目标（最小化力）"的分层 QP。

反事实推理：如果在协调逆运动学里**不用** \(J_{\text{abs}}\)（对称中点）而沿用 D01 风格的"独立 \(J_L,J_R\) + 投影"会怎样？其实两者数学上等价（都在 \(\ker(J_{\text{rel}})\) 里做绝对运动），区别在**语义与可控性**：用 \(J_{\text{abs}}\) 时，你的任务变量直接是"物体位姿"，期望轨迹 \(\mathcal{V}_{\text{abs}}^{\text{des}}\) 就是物体该怎么动，物理意义一目了然；用"独立 \(J_L,J_R\)"时，你得为左右手分别指定相容的目标，多一层"把物体轨迹翻译成两手轨迹"的手工步骤，且容易因翻译误差引入内力。对称表述的价值正是把"物体"提升为一等任务变量。

非对称任务：扩展相对雅可比 ⭐⭐⭐

对称分解（绝对=中点、相对=右减左）隐含一个假设：两臂地位对等。但很多真实任务是**非对称**的——一只手"扶"（提供稳定参考），另一只手"操作"（执行精细动作）。典型如：左手按住食材，右手切；左手持工件，右手拧螺丝。这类任务里，我们关心的不是"物体中点怎么动"，而是：

• 操作手相对扶持手的**相对运动**（切/拧的动作）；
• 扶持手（或操作手）自身的**绝对运动**（食材/工件放在哪）。

Almeida、Lima 等（2019）提出**扩展相对雅可比（Extended Relative Jacobian）**：把"相对运动"和"指定的某一只手的绝对运动"组合进一个雅可比，从而能同时控制这两个语义上更贴合非对称任务的量。形式上：

\[\begin{bmatrix}\mathcal{V}_{\text{rel}}\\ \mathcal{V}_{\text{abs}}^{(k)}\end{bmatrix}=J_{\text{ext}}\,\dot q,\qquad J_{\text{ext}}=\begin{bmatrix}J_{\text{rel}}\\ J_{\text{abs}}^{(k)}\end{bmatrix}\]

其中 \(\mathcal{V}_{\text{abs}}^{(k)}\) 不再是两手中点，而是**所选参考手 \(k\)（如扶持手）的绝对 twist（或某个任务定义的代表点）。这相当于把对称表述里"绝对=中点"替换成"绝对=某只指定手"——一个**可调的参考权重：

\[\mathcal{V}_{\text{abs}}^{(\alpha)}=\alpha\,\bar{\mathcal{V}}_L+(1-\alpha)\,\bar{\mathcal{V}}_R\]

\(\alpha=\tfrac12\) 退化为 Uchiyama-Dauchez 对称中点；\(\alpha=1\) 表示"以左手为绝对参考"（左手扶、右手操作）；\(\alpha=0\) 反之。这个 \(\alpha\) 把对称/非对称**统一进一个连续参数**——§7.6 会详细讨论它的选取与含义。

本质洞察：从对称（\(\alpha=\tfrac12\)）到非对称（\(\alpha\in\{0,1\}\)）的连续过渡，本质是"绝对运动这个集体坐标，以谁为代表"的选择。对称表述把集体坐标定义为民主的"平均"，非对称表述把它定义为"指定代表（leader）"——这在 §7.5 会直接对应到"对称协调 vs 主从架构"的体系结构选择。运动学层面的 \(\alpha\) 选择，预示了控制架构层面的主从 vs 对称之争。

与机械臂 D01 的分工（避免重复）

主题	机械臂 D01（单系统视角）	本章 §7.2（多机视角）
增广雅可比 \(J_{\text{aug}}\)	详细推导 + Pinocchio C++/Python 实现	仅作符号，引用 D01
相对雅可比 \(J_{\text{rel}}\)	给出 \(\bar J_R-\bar J_L\) 的工程形式 + 平移算子细节	从对称表述第一性原理重新推导其来历
绝对雅可比 \(J_{\text{abs}}\)	未强调	作为内力控制的对偶搭档引入
对称/非对称统一参数 \(\alpha\)	未涉及	引入，连接 §7.6 架构选择
零空间三大用途	详细（限位/操作度/自碰撞回避）	引用 D01，不重复
Jamisola-Roberts 紧凑形式	提及	给出工程拼装结论

如果你需要 \(J_{\text{aug}}\)、零空间投影 \(P\) 的完整 Pinocchio C++ 实现、阻尼伪逆参数表、奇异处理，请回 D01 §D1.4-D1.5；本章假设你已掌握那些，专注于"对称表述如何把双臂提升为可推广的多机协调坐标"。

⚠️ 常见陷阱

⚠️ 编程陷阱：用 LOCAL_WORLD_ALIGNED 雅可比直接 J_R - J_L 当相对雅可比
   错误做法：J_rel = J_R - J_L  （J_R, J_R 来自 getFrameJacobian(LOCAL_WORLD_ALIGNED)）
   现象：物体绕自身质心整体转动（明明是合法的绝对运动）时，算出的 V_rel ≠ 0，
        控制器误以为有相对运动 → 误施加内力 → 物体被无故挤压或两臂对抗
   根本原因：LOCAL_WORLD_ALIGNED 只对齐了坐标轴方向，没把线速度参考点
            移到同一点。刚体转动时两末端线速度天然不同，不统一参考点就做差，
            会把"绕物体转"误算成"相对平移"
   正确做法：先用平移算子 X(p_c - p_i) 把两臂 twist 统一到公共参考点 p_c，
            再做 J_rel = Xbar_R·J_R - Xbar_L·J_L（见上方 Python twist_translate）
   自检方法：构造一个让物体绕质心纯转动的 q̇，验证 ||J_rel · q̇|| ≈ 0；
            若显著非零，说明参考点没统一对

💡 概念误区：认为"绝对雅可比 J_abs 没用，有 J_rel 投影就够了"
   新手想法："刚体把持只要把运动投影到 ker(J_rel) 保证不产生内力就行，
            不需要单独的 J_abs。"
   实际上：投影到 ker(J_rel) 只保证"不破坏相对位姿"，但你还得在这个零空间里
          实现期望的物体运动——而"物体怎么动"恰恰是 J_abs 描述的。没有 J_abs，
          你能保证不挤压物体，却无法精确控制物体走到哪。更重要的是，内力控制
          （§7.4）需要 J_abs/J_rel 的对偶（力分配），缺了 J_abs 内/外力解耦无从谈起。
   正确理解：J_abs 管"物体往哪走"，J_rel 管"两手较劲多少"，二者缺一不可，
            它们是对称表述的一对孪生坐标。

🧠 思维陷阱：把"相对雅可比"当成"两个雅可比之差"这个表象，忽略参考点统一
   新手想法："相对雅可比顾名思义就是 J_R 减 J_L，很直观。"
   实际上：这个"差"只有在两个 twist 表达在同一参考点、同一坐标系时才有物理意义。
          "相对雅可比"的本质是"相对位姿对关节的微分"，而相对位姿的定义涉及
          SE(3) 的群运算（T_L^{-1} T_R），其微分自然带出伴随变换和参考点统一，
          绝不是朴素的矩阵相减。
   正确思维：从 T_rel = T_L^{-1} T_R 出发，对时间求导得到相对 twist，
            自然涌现参考点统一项。把它当 SE(3) 微分而非线性代数减法。

练习

1. [推导] 从相对位姿 \(T_{\text{rel}}=T_L^{-1}T_R\) 出发，对时间求导（用 \(SE(3)\) 上的左/右平凡化），导出相对 twist \(\mathcal{V}_{\text{rel}}\) 关于 \(\mathcal{V}_L,\mathcal{V}_R\) 的表达式，并说明伴随变换（adjoint）与本节的平移算子 \(X(\cdot)\) 在什么近似下一致。提示：当两末端姿态相同、仅位置不同时，伴随退化为平移算子。

2. [验证] 用 Pinocchio 为双 Franka 计算 \(J_{\text{abs}},J_{\text{rel}}\)。构造三种关节速度并验证语义：(a) 让物体纯平移的 \(\dot q\)（\(\mathcal{V}_{\text{rel}}=0\)，\(\mathcal{V}_{\text{abs}}\) 为纯线速度）；(b) 让物体绕质心纯转动的 \(\dot q\)（\(\mathcal{V}_{\text{rel}}=0\)）；(c) 让两手相对拉伸的 \(\dot q\)（\(\mathcal{V}_{\text{rel}}\neq 0\)，对应内力）。记录每种情况下 \(\|\mathcal{V}_{\text{abs}}\|,\|\mathcal{V}_{\text{rel}}\|\)。

3. [设计] 把非对称参数 \(\alpha\) 从 0 连续扫到 1，对"左手持杯、右手持壶倒水"任务，画出"以谁为绝对参考"如何影响控制语义。\(\alpha\) 取多少最合理？为什么倒水任务里"杯子（左手）"更适合当绝对参考？

4. [跨章综合] 回顾第 4 章分布式 MPC：那里把联合状态 \(x=[x_1,\dots,x_N]\) 在各 agent 间分解。本节把双臂运动分解为"绝对 + 相对"。论证：绝对/相对分解是不是一种特殊的"坐标变换式解耦"，和 ADMM 的"变量分裂 + 一致性约束"在思想上有何异同？（提示：ADMM 分裂的是各 agent 的局部变量，绝对/相对分裂的是任务空间的语义通道。）

---

§7.3 闭链约束的两个面孔：\(\ker(J_{\text{rel}})\) 与 \(\ker(G)\) 的对偶 ⭐⭐⭐

这一节解决什么问题：第 5 章用 grasp matrix \(G\) 在**力空间**描述协同搬运（内力 = \(\ker(G)\)）；§7.2 用相对雅可比 \(J_{\text{rel}}\) 在**速度空间**描述双臂协调（同步运动 = \(\ker(J_{\text{rel}})\)）。这两套语言看起来各说各话，但它们描述的是**同一个物理约束**（刚体把持）的两个投影。这一节用运动学-静力学对偶把它们焊在一起——理解了这个对偶，你就明白"内力"为什么既是力空间的零空间元素、又对应速度空间的某个互补子空间，以及为什么"刚体把持"同时是 6 维速度约束和 6 维 wrench 约束。这是本章最具"统一性"的一节，也是把第 5 章和机械臂 D01 真正缝起来的地方。

动机：两套语言，一个物理

回顾两套描述：

力空间（第 5 章 grasp matrix）：\(N\) 只手对物体施加的 wrench 堆叠为 \(h=[h_1;\dots;h_N]\)，grasp matrix \(G\) 把它映射到物体净 wrench：

\[F_{\text{load}}=G\,h,\qquad G=\begin{bmatrix}G_1 & \cdots & G_N\end{bmatrix},\quad G_i=\begin{bmatrix}I_3 & 0\\ [r_i]_\times & I_3\end{bmatrix}\in\mathbb{R}^{6\times6}\]

（这里每只手传递完整 6 维 wrench——力 + 力矩，因为夹爪刚性把持能传力矩；第 5 章的点接触版本 \(G_i\in\mathbb{R}^{6\times3}\) 只传力，是这里的退化。\(r_i\) 是第 \(i\) 抓取点到物体质心的向量。）内力 = \(\ker(G)\)，维度 \(6N-6\)。

速度空间（§7.2 相对雅可比）：刚体把持要求两手相对位姿不变，即 \(\mathcal{V}_{\text{rel}}=J_{\text{rel}}\dot q=0\)。满足约束的关节速度 = \(\ker(J_{\text{rel}})\)。

问题：这两个零空间——力空间的 \(\ker(G)\) 和速度空间的 \(\ker(J_{\text{rel}})\)——是什么关系？它们都和"内力"有关，但一个在力里、一个在速度里，凭什么说它们是"一个约束的两个面孔"？

运动学-静力学对偶：虚功原理 ⭐⭐⭐

答案藏在**虚功原理（principle of virtual work）**里。这是整个机器人学最深刻的对偶之一，值得逐步建立。

考虑物体侧。物体的运动 twist 是 \(\mathcal{V}_{\text{obj}}\in\mathbb{R}^6\)，物体受到的净 wrench 是 \(F_{\text{load}}\in\mathbb{R}^6\)。各手施加 wrench \(h_i\)，各手末端运动 twist \(\bar{\mathcal{V}}_i\)（已统一到物体质心参考点）。刚体把持下，各手末端和物体刚性连接，所以 \(\bar{\mathcal{V}}_i=\mathcal{V}_{\text{obj}}\)（在物体质心表达时）——更一般地，接触点速度 = 物体速度通过抓取几何的传递：

\[\bar{\mathcal{V}}_{\text{contacts}}=G^\top\mathcal{V}_{\text{obj}}\]

这是关键一步：同一个 \(G\)，转置之后，把物体速度映射到各接触点速度。为什么是 \(G^\top\) 而不是别的？这正是虚功原理的内容。

虚功原理说：约束力做的虚功为零（理想约束）。各手施加 wrench \(h\)、各手末端有虚位移（虚 twist）\(\delta\bar{\mathcal{V}}_{\text{contacts}}\)，物体受净 wrench \(F_{\text{load}}\)、有虚 twist \(\delta\mathcal{V}_{\text{obj}}\)。功率平衡（手对物体做的功 = 物体获得的功）：

\[h^\top\delta\bar{\mathcal{V}}_{\text{contacts}}=F_{\text{load}}^\top\delta\mathcal{V}_{\text{obj}}\]

代入 \(F_{\text{load}}=Gh\)：

\[h^\top\delta\bar{\mathcal{V}}_{\text{contacts}}=(Gh)^\top\delta\mathcal{V}_{\text{obj}}=h^\top G^\top\delta\mathcal{V}_{\text{obj}}\]

对任意 \(h\) 成立，故：

\[\boxed{\;\delta\bar{\mathcal{V}}_{\text{contacts}}=G^\top\delta\mathcal{V}_{\text{obj}}\;}\]

这就证明了：接触点速度由物体速度经 \(G^\top\) 得到。力的映射 \(G\) 和速度的映射 \(G^\top\) 是同一个 \(G\) 的转置——这是运动学（速度）与静力学（力）的对偶。

本质洞察：grasp matrix \(G\) 不只是"力的映射矩阵"。它同时编码了抓取的**运动学**和**静力学**：\(G\)（前向）把接触力合成为物体 wrench；\(G^\top\)（转置）把物体速度分配为接触点速度。一个矩阵，两个方向，分别管力和速度——这就是"对偶"的精确含义。第 5 章只用了 \(G\) 的力方向，本章揭示它的速度方向，从而和相对雅可比接上。

两个零空间的物理对应 ⭐⭐⭐

现在可以精确回答"\(\ker(G)\) 和 \(\ker(J_{\text{rel}})\) 是什么关系"了。

\(\ker(G)\)（力的零空间，维度 \(6N-6\)）：满足 \(Gh=0\) 的 wrench 组合 \(h\)。物理上：一组接触 wrench，合成到物体的净 wrench 为零——它们**互相抵消，不推动物体，只在物体内部"较劲"**。这就是内力（internal wrench）。

\(\text{Im}(G^\top)\)（速度的值空间，维度 6）：所有"由某个物体运动产生的接触点速度模式"。物理上：这些是**协调一致的运动**——各手速度恰好对应同一个刚体运动，不破坏抓取。

\(\ker(G^\top)\)（\(G^\top\) 的零空间，维度 \(6N-6\)）：满足 \(G^\top\mathcal{V}_{\text{obj}}\) 永不产生的接触速度模式——即**不对应任何刚体运动的接触点速度**，它们会破坏刚性约束（让接触点之间相对移动）。

把这些对应到双臂（\(N=2\)）：

子空间	维度（\(N=2\)）	物理含义	在速度还是力
\(\ker(G)\)	\(12-6=6\)	内力（两手互相挤压/拉扯，不动物体）	力
\(\text{Im}(G^\top)\)	\(6\)	协调运动（两手速度对应同一刚体运动）	速度
\(\ker(G^\top)=\)"破坏约束的接触速度"	\(12-6=6\)	相对运动（破坏刚性把持的两手相对速度）	速度

关键对应：speed 里"破坏约束的相对运动方向"（\(\ker(G^\top)\)，6 维）与 force 里"内力方向"（\(\ker(G)\)，6 维）——它们是同构的（同维数，且通过 \(G\) 的 SVD 自然配对）。这正是为什么内力总是和相对运动配对：

\[\underbrace{\text{相对运动}}_{\ker(G^\top),\ 6\text{维}}\xleftrightarrow[\text{对偶}]{}\underbrace{\text{内力}}_{\ker(G),\ 6\text{维}}\]

本质洞察：内力和相对运动是同一枚硬币的两面，由抓取几何 \(G\) 的零空间结构锁定。物理直觉：如果你能让两手产生某个"破坏刚性约束的相对运动"（拉伸），那么阻止这个运动（刚体不让你拉伸）就会产生对应方向的"内力"。能拉伸的方向 = 能产生内力的方向。这就是为什么 §7.4 控制内力时，控制的变量是"相对位姿偏差"——因为相对位姿偏差正是内力的"位移共轭"。

把 \(J_{\text{rel}}\) 接进来：关节空间的完整图景 ⭐⭐⭐

上面是"物体侧"的对偶（接触点 ↔ 物体）。还差一步：把它连到**关节空间**（这才是我们真正能控制的）。关节速度 \(\dot q\) 通过各臂雅可比产生末端速度，末端速度要满足"对应某个刚体运动"才不破坏抓取：

关节空间 ──[各臂雅可比 J_i]──→ 末端速度 ──[抓取约束 G^T]──→ 物体速度
   q̇                            V_contacts                  V_obj
   │                                                          
   │                          相对雅可比 J_rel 直接给出
   └──────────────────────────────────────────→ V_rel = J_rel · q̇
                                                  (= 末端速度中"破坏约束"的分量)

精确地说：末端速度 \(\bar{\mathcal{V}}_{\text{contacts}}=J_{\text{aug}}\dot q\)（增广雅可比，D01）。把它分解到"协调运动子空间 \(\text{Im}(G^\top)\)"和"破坏约束子空间 \(\ker(G^\top)\)"：

• 落在 \(\text{Im}(G^\top)\) 的分量 → 对应物体运动 \(\mathcal{V}_{\text{obj}}\)（绝对运动，§7.2 的 \(\mathcal{V}_{\text{abs}}\) 是它的一个 6 维参数化）；
• 落在 \(\ker(G^\top)\) 的分量 → 相对运动，正是 \(\mathcal{V}_{\text{rel}}=J_{\text{rel}}\dot q\)。

于是：

\[\boxed{\;J_{\text{rel}}\dot q=0\;\Longleftrightarrow\;\bar{\mathcal{V}}_{\text{contacts}}\in\text{Im}(G^\top)\;\Longleftrightarrow\;\text{末端速度对应某个纯刚体运动，不产生相对运动}\;}\]

这就是 \(\ker(J_{\text{rel}})\) 与 \(\ker(G)\) 对偶的最终形式：\(\ker(J_{\text{rel}})\)（关节速度满足刚体约束）映射到末端的 \(\text{Im}(G^\top)\)（协调运动），而它的"互补"——会产生 \(\mathcal{V}_{\text{rel}}\neq0\) 的关节速度——映射到 \(\ker(G^\top)\)（破坏约束的相对运动），后者与 \(\ker(G)\)（内力）对偶。

一句话串起全章前三节：

物体运动（绝对，\(\mathcal{V}_{\text{abs}}\)，\(\text{Im}(G^\top)\)）和内力（\(\ker(G)\)）是两个互补的、解耦的控制通道；相对运动（\(\mathcal{V}_{\text{rel}}\)，\(\ker(G^\top)\)）是内力的速度共轭，控制内力就是控制相对位姿偏差。

一张图看懂全部对偶

          速度空间 (运动学)                          力空间 (静力学)
        ─────────────────                         ─────────────────
                                  G^T │ G
   关节速度 q̇                          │
        │ J_aug                        │
        ▼                              │
   末端速度 V_contacts ────────────────┼──────────── 接触 wrench h
        │                              │                  │
   ┌────┴─────┐                        │            ┌─────┴──────┐
   │          │                        │            │            │
Im(G^T)    ker(G^T)                    │         Im(G^T·?)     ker(G)
"协调运动"  "相对运动"                  │         "运动力域"    "内力"
   │          │  ◄────── 对偶配对 ──────┼──────────────────►  │
   ▼          ▼                        │                       ▼
 V_abs      V_rel = J_rel·q̇            │                    f_int
(物体运动)  (内力的速度共轭)            │                  (落在 ker G)
   │                                                           │
   └────────► 运动控制                          力控 ◄─────────┘
              (§7.4 外环)                        (§7.4 内环)

跨领域类比（带边界）：\(J_{\text{rel}}\) 与 \(G\) 的对偶 vs 电路里的基尔霍夫电流/电压定律。像的部分：KCL（节点电流和为零）约束"流量"（类比速度/运动），KVL（回路电压和为零）约束"势差"（类比力/内力），二者描述同一个电路拓扑的两个对偶方面；闭链的速度约束 \(J_{\text{rel}}\dot q=0\) 和力约束 \(Gh=F_{\text{load}}\) 也描述同一个机械回路的两个对偶方面，由同一个几何（\(G\)/抓取拓扑）决定。不像的部分：电路对偶是线性恒定的；机械臂的 \(G\) 和 \(J_{\text{rel}}\) 随构型变化（\(r_i\)、雅可比都依赖 \(q\)），对偶关系在每个瞬时重新建立。不要把类比延伸到："电路里电流电压完全独立可测，所以内力和运动也完全独立可控"——不对，机械系统里相对运动一旦发生就立即通过物体刚度产生内力，二者在动力学上紧密耦合，"解耦"是控制器在每个控制周期主动维持的，不是天然独立。

Worked Example：用一个最简双臂把对偶算出来

抽象的对偶讲完了，用一个可手算的最简实例把它"落地"——这能让"\(\ker(G)\) 与相对运动对偶"从抽象命题变成你能在纸上验证的具体矩阵。

设两手沿 \(x\) 轴对称把持一根杆，物体质心在原点。左手在 \(r_L=(-\ell,0,0)\)、右手在 \(r_R=(+\ell,0,0)\)，\(\ell=0.15\,\text{m}\)。只考虑平面内的力（不含力矩，用 \(3\) 维 wrench 简化，结论同样成立），grasp matrix（质心参考点）：

\[G=\begin{bmatrix}G_L & G_R\end{bmatrix},\quad G_i=\begin{bmatrix}1&0\\0&1\\-r_{iy}&r_{ix}\end{bmatrix}\Rightarrow G=\begin{bmatrix}1&0&1&0\\0&1&0&1\\0&-\ell&0&\ell\end{bmatrix}\in\mathbb{R}^{3\times4}\]

（这里 \(h_i=(f_{ix},f_{iy})\in\mathbb{R}^2\)，物体 wrench \(F=(F_x,F_y,M_z)\in\mathbb{R}^3\)，第三行是力矩 \(r\times f\) 的 \(z\) 分量。）\(G\in\mathbb{R}^{3\times4}\)，秩 3，故 \(\dim\ker(G)=4-3=1\)——一维内力。

求内力方向 \(\ker(G)\)：解 \(Gh=0\)。第一行 \(f_{Lx}+f_{Rx}=0\)、第三行 \(-\ell f_{Ly}+\ell f_{Ry}=0\) 给 \(f_{Ly}=f_{Ry}\)，第二行 \(f_{Ly}+f_{Ry}=0\) 配合得 \(f_{Ly}=f_{Ry}=0\)。于是 \(\ker(G)=\text{span}\{(1,0,-1,0)\}\)——即 \(f_{Lx}=+1,f_{Rx}=-1\)：左手向右推 \(+x\)、右手向左推 \(-x\)，两手沿杆轴相向对夹。这正是物理直觉里的"夹紧内力"，现在它是 \(\ker(G)\) 的一个具体基向量。

验证它不动物体：\(G\cdot(1,0,-1,0)^\top=(1-1,\,0,\,0)^\top=0\) ✓。对夹力合成到物体的净 wrench 为零。

对偶的相对运动方向：内力沿 \(x\) 轴（杆轴）对夹 ↔ 速度上"破坏约束的相对运动"也应沿 \(x\) 轴——即两手沿杆轴相对拉伸/压缩（\(\bar{\mathcal{V}}_R-\bar{\mathcal{V}}_L\) 的 \(x\) 分量非零）。果然：能产生 \(x\) 向内力的方向，正是能 \(x\) 向拉伸的方向（§7.3 "能拉伸的方向 = 能产生内力的方向"）。一维内力 ↔ 一维相对拉伸，同维同构，对偶在这个最简例子上**显式成立**。

本质洞察（从实例回看抽象）：这个 \(3\times4\) 的小 \(G\) 把整节的抽象对偶压缩成了"\((1,0,-1,0)\) 这个向量"——它既是 \(\ker(G)\) 的基（力空间的内力），又指向相对拉伸方向（速度空间）。当你在 \(N\) 臂、6D wrench 的大矩阵里迷失时，回到这个最简例子：内力永远是"某种相向的力组合，合成为零、纯内部较劲"，相对运动永远是"破坏刚性约束的那部分末端运动"，二者由 \(G\) 的零空间结构一一配对。维度变大只是基变多，结构不变。

从对偶看"为什么内力必须闭环"

对偶视角直接解释了一个工程上反直觉的事实：内力不能开环施加，必须闭环维护（这是 §7.4 的核心，这里先用对偶把动机讲透）。

反事实推理：如果开环施加内力（让两手按计算好的 h_int 出力）会怎样？

  你想要内力 f_int → 在 ker(G) 里选一个 wrench → 让两手按它出力。

  但 ker(G) 依赖 r_i（抓取点到质心向量），r_i 依赖物体当前位姿，
  物体一动 → r_i 变 → ker(G) 的基旋转 → 你"开环"算好的 f_int 不再落在
  当前的 ker(G) 里 → 它有了沿 Im 方向的分量 → 这个分量推动物体！
       │
       ▼
  本想"只夹紧不推物体"的内力，因为物体动了、几何变了，
  变成了"既夹紧又乱推"→ 物体轨迹被污染 → 误差累积 → 失控。

  正确做法（§7.4）：闭环测量相对位姿偏差（内力的位移共轭），
  用阻抗律 f_int = K_int · (相对位姿偏差) 实时产生内力，
  几何变了控制器自动跟上。

本质洞察：内力之所以必须闭环，是因为"内力子空间 \(\ker(G)\)"本身随构型旋转。开环把力扔进一个会动的子空间，力会"漏"到运动通道里。闭环（用相对位姿偏差反馈）相当于始终把力对准当前的 \(\ker(G)\)，无论它怎么转。这与第 2 章"共识需要持续通信而非一次性广播"异曲同工——环境/拓扑在变，必须持续校正而非开环下发。

⚠️ 常见陷阱

💡 概念误区：认为"内力是浪费，应该尽量调成零"
   新手想法："内力不推动物体，纯属内耗，最优控制应该让内力=0。"
   实际上：内力是抓取稳定性的来源。对悬空刚体，若内力为零，两手只靠摩擦
          维持抓取——稍有扰动（物体惯性、外部碰撞）就打滑脱落。正的内力
          （两手相向"夹紧"）提供摩擦裕度，是必需的。内力的目标不是零，而是
          "足够大以防滑、足够小以不压坏物体"的一个区间。
   正确理解：内力是需要主动调控到合适区间的控制量，不是要消除的误差。
            §7.4 会给出内力期望值的设计准则（摩擦锥 + 物体强度）。

🧠 思维陷阱：把 ker(G) 当成固定子空间
   新手想法："grasp matrix 的零空间是几何决定的常数子空间。"
   实际上：G 依赖各接触点到质心的向量 r_i，物体一旋转/平移，r_i 改变，
          G 改变，ker(G) 的基随之旋转。把 ker(G) 当常数会导致开环内力
          泄漏到运动通道（见上方反事实推理）。
   正确思维：ker(G) 是随构型变化的"活"子空间，每个控制周期重算。

⚠️ 编程陷阱：用 G 的力方向却忘了 G^T 的速度方向不是同一回事的转置笔误
   错误做法：写协同搬运代码时，把"物体速度→接触速度"的映射也写成 G（而非 G^T），
            或维度对不上时随手转置凑维度
   现象：力分配看似正确，但一旦物体运动，接触点速度预测错误，导致前馈补偿错误
   根本原因：F_load = G·h（力，6×6N 乘 6N×1）；V_contacts = G^T·V_obj
            （速度，6N×6 乘 6×1）。两者维度天然不同，G 和 G^T 各司其职
   正确做法：牢记对偶——力用 G（合成），速度用 G^T（分配）。维度本身就是
            最好的 sanity check：力方程左边 6 维、速度方程左边 6N 维

练习

1. [推导] 用虚功原理证明 \(\delta\bar{\mathcal{V}}_{\text{contacts}}=G^\top\delta\mathcal{V}_{\text{obj}}\) 的完整过程（本节给了骨架，补全每一步的理由）。然后说明：为什么"理想约束虚功为零"这个假设对刚性把持成立，对有摩擦滑动的接触不成立？

2. [计算] 对双臂（\(N=2\)）刚性把持，分别计算 \(\dim\ker(G)\)、\(\dim\text{Im}(G^\top)\)、\(\dim\ker(G^\top)\)，验证它们加起来符合维度定理。再对三臂（\(N=3\)）重做，观察内力维度如何随 \(N\) 增长。

3. [编程] 用 Pinocchio + NumPy 为双 Franka 共持物体的某个构型，同时计算 \(G\)（从抓取点几何）和 \(J_{\text{rel}}\)（从雅可比）。数值验证：让物体绕质心纯转动的关节速度 \(\dot q\) 满足 \(J_{\text{rel}}\dot q\approx0\)，且对应的末端速度 \(J_{\text{aug}}\dot q\) 落在 \(\text{Im}(G^\top)\) 中（即 \(G^\top\) 列空间的投影残差 \(\approx0\)）。

4. [思考] 本节说"相对运动是内力的速度共轭"。回顾 §7.2 的对称表述：\(\mathcal{V}_{\text{rel}}\)（相对 twist）和 §7.4 将引入的 \(f_{\text{int}}\)（内力 wrench）维度都是 6。论证它们构成一对"功率共轭变量"（\(P_{\text{int}}=f_{\text{int}}^\top\mathcal{V}_{\text{rel}}\) 是内力做的功率），并解释为什么刚体把持时 \(\mathcal{V}_{\text{rel}}=0\) 意味着内力不做功（既不输入也不耗散能量）。

---

§7.4 内力控制：从双臂到 N 臂 ⭐⭐⭐

这一节解决什么问题：§7.3 证明了内力（\(\ker(G)\)）和相对运动（\(\ker(G^\top)\)）是对偶的，且内力必须闭环。这一节给出具体的控制律：怎么把内力调到期望值？答案是 Caccavale 的**内/外阻抗双层结构**——外环（object impedance）让物体对外部接触柔顺、跟踪物体轨迹（绝对通道），内环（internal impedance）调控两手内力（相对通道）。我们先在双臂上把它讲透，再推广到 \(N\) 臂。这一节是"会做协同力控"的核心。注意：机械臂 D03 也讲协调力控，但它聚焦单系统双臂的工程实现（TDPA、无源性、ROS2）；本章聚焦**内力分解的多机推广**和**内/外解耦的体系结构**，两章互补。

动机：内力的精确定义与期望值设计

§7.3 说"内力落在 \(\ker(G)\)"，但工程上需要一个可计算的**具体表达**。对双臂（\(N=2\)），各手 wrench \(h_L,h_R\in\mathbb{R}^6\)（已表达在物体质心参考点）。物体净 wrench（运动力）：

\[F_{\text{load}}=G\,h=h_L+h_R\quad(\text{质心参考点下 }G=[I_6\ I_6])\]

内力定义为两手 wrench 的"反对称部分"——它合成到物体为零，纯内耗：

\[\boxed{\;f_{\text{int}}=\tfrac12\left(h_R-h_L\right)\;}\]

验证它落在 \(\ker(G)\)：把 \(h_L=\tfrac12F_{\text{load}}-f_{\text{int}}\)、\(h_R=\tfrac12F_{\text{load}}+f_{\text{int}}\) 代回 \(G h=h_L+h_R=F_{\text{load}}\)，内力项相消，确实不贡献净 wrench。这给出双臂 wrench 的标准分解：

\[h_L=\tfrac12 F_{\text{load}}-f_{\text{int}},\qquad h_R=\tfrac12 F_{\text{load}}+f_{\text{int}}\]

物理图景：\(\tfrac12F_{\text{load}}\) 是每只手分担的"搬运负载"（平分），\(\pm f_{\text{int}}\) 是叠加在上面的"对夹力"——右手 \(+f_{\text{int}}\)（向左推）、左手 \(-f_{\text{int}}\)（向右推），两手相向夹紧物体。

内力期望值怎么设计？ 这是个工程权衡，下界由防滑决定、上界由物体强度决定：

约束	不等式	含义
防滑下界	\(f_{\text{int}}^{\text{normal}}\ge \dfrac{\|F_{\text{tangent}}\|}{\mu}\)	法向夹紧力须大到让摩擦力足以平衡切向负载（摩擦锥）
抗扰裕度	\(f_{\text{int}}\ge f_{\text{slip}}\cdot(1+\gamma)\)	留 \(\gamma\)（如 50%）裕度应对惯性力、外部碰撞
强度上界	\(f_{\text{int}}\le f_{\text{crush}}\)	不超过物体被压坏的阈值（玻璃、纸盒远小于钢件）
电机上界	\(\|h_i\|\le \tau_{\max,i}\) 经雅可比映射	各臂力矩不饱和

本质洞察：内力期望值不是一个点，而是一个由"防滑下界"和"强度上界"夹出的**区间**——这是它与运动目标（一个精确轨迹点）的根本不同。运动控制追求"精确到达"，内力控制追求"落在安全区间"。把内力当成需要精确跟踪的硬目标是常见误区；它更像一个带软约束的调节量。这也解释了为什么内力控制天然适合用阻抗（柔顺、容许偏差）而非位置式硬跟踪。

Worked Example：把内力区间算成具体数字。 双臂水平端持一块 \(m=2\,\text{kg}\) 的玻璃板，两手在两侧面摩擦把持，摩擦系数 \(\mu=0.4\)，玻璃被压坏阈值 \(f_{\text{crush}}=120\,\text{N}\)。求内力区间。

切向负载（要靠摩擦平衡的力）：重力 + 加速度惯性力
  静止时：F_tangent = mg = 2 × 9.81 ≈ 19.6 N（重力沿切向，靠摩擦托住）
  搬运加速时（设 a = 3 m/s²）：F_tangent = m(g 分量 + a) 可达 ~26 N

防滑下界（摩擦锥）：
  f_int^normal ≥ F_tangent / μ = 26 / 0.4 = 65 N
  → 两手至少各夹 65 N，摩擦力 μ × 65 = 26 N 才够托住

抗扰裕度（留 γ=50%）：
  f_int ≥ 65 × 1.5 ≈ 98 N

强度上界：
  f_int ≤ f_crush = 120 N

→ 内力可行区间：[98, 120] N（很窄！）

本质洞察（区间可能很窄甚至为空）：上面算出可行区间只有 \([98,120]\) N，宽度 22 N——留给控制器的余地很小。如果玻璃更脆（\(f_{\text{crush}}=90\) N）或摩擦更差（\(\mu=0.3\) → 下界升到 \(26/0.3\times1.5\approx130\) N），区间为空——意味着"既不打滑又不压碎"在当前摩擦/几何下不可能，必须改抓取方式（加大接触面、换高摩擦衬垫、或用形封闭而非力封闭）。这解释了为什么真实双臂操作里"怎么抓"（grasp planning）和"夹多紧"（内力控制）必须联合设计：内力控制器再好，也救不了一个可行区间为空的抓取。这也是 §7.6 末"角色/抓取选择"与本节内力控制耦合的根源。

Caccavale 内/外阻抗双层结构 ⭐⭐⭐

Caccavale 等（2008）的核心思想：用**两个阻抗**分别管两个解耦通道（正是 §7.2/§7.3 的绝对/相对、运动/内力）。

外环——object impedance（物体级阻抗，管绝对通道）：让物体对外部环境接触表现出期望的柔顺。物体位姿误差 \(\tilde x_{\text{obj}}=x_{\text{obj}}-x_{\text{obj}}^{\text{des}}\)，外部 wrench \(F_{\text{ext}}\)（来自物体与环境的接触）：

\[M_o\ddot{\tilde x}_{\text{obj}}+D_o\dot{\tilde x}_{\text{obj}}+K_o\tilde x_{\text{obj}}=F_{\text{ext}}\]

物理含义：物体像一个虚拟的"质量-弹簧-阻尼"挂在目标轨迹上，外力推它时它柔顺地让步——这让双臂搬着物体去"插孔/贴合"时不会因刚性硬怼而损坏。

内环——internal impedance（内力阻抗，管相对通道）：让内力跟踪期望值，同时对相对位姿偏差表现出柔顺。相对位姿误差 \(\tilde x_{\text{rel}}\)、内力误差 \(\tilde f_{\text{int}}=f_{\text{int}}-f_{\text{int}}^{\text{des}}\)：

\[M_i\ddot{\tilde x}_{\text{rel}}+D_i\dot{\tilde x}_{\text{rel}}+K_i\tilde x_{\text{rel}}=-\tilde f_{\text{int}}\]

物理含义：把"两手相对位姿"建模成一根虚拟弹簧（刚度 \(K_i\)），内力就是这根弹簧的张力。要增大内力（夹紧），就把相对位姿目标设成"两手更靠近"，弹簧被压缩产生夹力。内力通过相对位姿偏差产生——这正是 §7.3"相对运动是内力的速度共轭"的控制落地。

两层为什么能解耦？ 因为 object impedance 作用在 \(\mathcal{V}_{\text{abs}}\) 通道（\(\text{Im}(G^\top)\)），internal impedance 作用在 \(\mathcal{V}_{\text{rel}}\) 通道（\(\ker(G^\top)\)），而 §7.3 证明这两个通道正交。控制器把总的关节力矩拆成两部分，分别投影到这两个正交子空间：

\[\tau=J_{\text{abs}}^\top F_{\text{abs}}^{\text{cmd}}+J_{\text{rel}}^\top F_{\text{rel}}^{\text{cmd}}+\tau_{\text{null}}\]

其中 \(F_{\text{abs}}^{\text{cmd}}\) 来自外环（object impedance + 重力/惯性前馈），\(F_{\text{rel}}^{\text{cmd}}\) 来自内环（internal impedance，产生内力），\(\tau_{\text{null}}\) 是冗余自任务（自碰撞回避等，投影到 \(\ker(J_{\text{coop}})\)）。

        ┌──────────────── 外环：Object Impedance ────────────────┐
        │  物体目标轨迹 x_obj^des                                  │
        │       │                                                 │
        │       ▼   M_o s² + D_o s + K_o = F_ext                  │
        │  期望物体运动 → J_abs^T → 绝对通道力矩                    │──┐
        └─────────────────────────────────────────────────────────┘  │
                                                                       ├─► τ → 关节
        ┌──────────────── 内环：Internal Impedance ───────────────┐  │
        │  期望内力 f_int^des（防滑↔强度区间内选）                  │  │
        │       │                                                 │  │
        │       ▼   相对位姿偏差 → 虚拟弹簧 K_i → 产生内力         │  │
        │  期望相对运动 → J_rel^T → 相对通道力矩                    │──┘
        └─────────────────────────────────────────────────────────┘
        ┌──────────────── 冗余：Null-space ───────────────────────┐
        │  自碰撞回避 / 操作度 → (I - J_coop^+ J_coop) → τ_null     │
        └─────────────────────────────────────────────────────────┘

本质洞察：Caccavale 双层结构是 §7.2-7.3 全部对偶理论的"控制器化身"。绝对/相对的运动学分解（§7.2）+ \(\ker(J_{\text{rel}})\)/\(\ker(G)\) 的对偶（§7.3）→ 外环管绝对、内环管相对，两环正交互不干扰。理解了前两节，这个控制器不是"又一个公式"，而是对偶理论的必然产物。反过来，如果不理解对偶，你会困惑"为什么内力用相对位姿弹簧来产生而不是直接命令力"——因为相对位姿是内力的共轭，闭环它才能对抗几何变化（§7.3 末"为什么内力必须闭环"）。

双臂协调阻抗——可运行代码框架

# ============================================================
# 双臂协调阻抗控制（Caccavale 内/外双层）——MuJoCo/Pinocchio 伪实现
# 外环 object impedance（绝对通道）+ 内环 internal impedance（相对通道）
# 注：省略重力/惯性补偿与具体积分器，聚焦双层结构与通道解耦
# ============================================================
import numpy as np
import pinocchio as pin

class DualArmCoordImpedance:
    def __init__(self, model, left_frame, right_frame):
        self.model, self.data = model, model.createData()
        self.L = model.getFrameId(left_frame)
        self.R = model.getFrameId(right_frame)
        # 外环：物体级阻抗（6x6 对角，平移 N/m，旋转 N·m/rad）
        self.Ko = np.diag([800,800,800, 40,40,40])
        self.Do = np.diag([80,80,80, 8,8,8])
        # 内环：内力阻抗（相对位姿→内力的虚拟刚度）
        self.Ki = np.diag([1500,1500,1500, 60,60,60])
        self.Di = np.diag([60,60,60, 4,4,4])

    @staticmethod
    def _skew(v):
        return np.array([[0,-v[2],v[1]],[v[2],0,-v[0]],[-v[1],v[0],0]])

    def _Xtr(self, d):                       # twist 平移算子 [v;omega]
        X = np.eye(6); X[0:3,3:6] = -self._skew(d); return X

    def coop_jacobians(self, q):
        pin.forwardKinematics(self.model, self.data, q)
        pin.updateFramePlacements(self.model, self.data)
        pin.computeJointJacobians(self.model, self.data, q)
        JL = pin.getFrameJacobian(self.model, self.data, self.L, pin.LOCAL_WORLD_ALIGNED)
        JR = pin.getFrameJacobian(self.model, self.data, self.R, pin.LOCAL_WORLD_ALIGNED)
        pL = self.data.oMf[self.L].translation
        pR = self.data.oMf[self.R].translation
        pc = 0.5*(pL+pR)
        JbL = self._Xtr(pc-pL) @ JL
        JbR = self._Xtr(pc-pR) @ JR
        J_abs = 0.5*(JbL+JbR)                # 绝对（物体运动）
        J_rel = JbR - JbL                    # 相对（内力通道）
        return J_abs, J_rel, pc

    def control(self, q, dq, x_obj_des, dx_obj_des, x_rel_des, f_int_des):
        J_abs, J_rel, pc = self.coop_jacobians(q)
        V_abs = J_abs @ dq
        V_rel = J_rel @ dq

        # ---- 外环：object impedance（绝对通道）----
        # 物体位姿误差用当前绝对帧与期望帧的 SE(3) log（此处用 6D 误差占位）
        e_obj  = x_obj_des                   # = log6(T_obj^{-1} T_obj_des) 的占位
        de_obj = dx_obj_des - V_abs
        F_abs_cmd = self.Ko @ e_obj + self.Do @ de_obj      # 物体级阻抗力（绝对 wrench）

        # ---- 内环：internal impedance（相对通道，产生内力）----
        # 期望内力 f_int_des 通过把相对位姿目标"压缩"来产生：
        #   等价地，relative impedance 给出相对通道 wrench：
        e_rel  = x_rel_des                   # 相对位姿偏差（含为产生 f_int_des 的预压缩）
        de_rel = -V_rel                      # 刚体把持期望 V_rel=0
        F_rel_cmd = self.Ki @ e_rel + self.Di @ de_rel + f_int_des   # 相对通道 wrench(含内力)

        # ---- 投影回关节力矩：两通道正交叠加 ----
        tau = J_abs.T @ F_abs_cmd + J_rel.T @ F_rel_cmd

        # ---- 冗余零空间自任务（自碰撞回避占位）----
        J_coop = np.vstack([J_abs, J_rel])
        Jc_pinv = np.linalg.pinv(J_coop, rcond=1e-6)
        N = np.eye(self.model.nv) - Jc_pinv @ J_coop
        tau += N @ (-0.1*dq)                 # 简单零空间阻尼占位
        return tau

# ============================================================
# ❌ 常见错误：开环命令内力（不闭环相对位姿）
# ============================================================
# 直接把期望内力按当前 ker(G) 基分解成两手力矩，开环下发：
# h_L = 0.5*F_load - f_int_des
# h_R = 0.5*F_load + f_int_des
# tau = J_L.T @ h_L + J_R.T @ h_R       # 开环！
# 问题：物体一动，ker(G) 旋转，f_int_des 不再落在新 ker(G) 中，
#       内力"漏"到运动通道，污染物体轨迹（见 §7.3 反事实推理）。
# ✅ 正确：用 internal impedance 闭环相对位姿偏差产生内力（上面 control()）。

推广到 N 臂 ⭐⭐⭐

双臂的 \(f_{\text{int}}=\tfrac12(h_R-h_L)\) 是 \(N=2\) 的特例。\(N\) 臂时内力不再是单个 6 维量，而是 \(6N-6\) 维子空间里的元素，需要更一般的参数化。回到 grasp matrix：

\[h=G^{+}F_{\text{load}}+W\,\lambda,\qquad \text{Im}(W)=\ker(G),\ \lambda\in\mathbb{R}^{6N-6}\]

\(W\in\mathbb{R}^{6N\times(6N-6)}\) 的列张成内力空间，\(\lambda\) 是内力参数。\(N=2\) 时 \(W\) 的一种取法直接对应 \(\pm f_{\text{int}}\)：

\[W=\tfrac1{\sqrt2}\begin{bmatrix}-I_6\\ I_6\end{bmatrix}\in\mathbb{R}^{12\times6}\quad\Rightarrow\quad h=\tfrac12\mathbf{1}\otimes F_{\text{load}}+\tfrac1{\sqrt2}\begin{bmatrix}-I_6\\ I_6\end{bmatrix}(\sqrt2 f_{\text{int}})\]

恰好还原 \(h_L=\tfrac12F-f_{\text{int}}\)、\(h_R=\tfrac12F+f_{\text{int}}\)。

\(N\) 臂内力的物理结构：\(6N-6\) 维内力可以分类（Erhart-Hirche 2017 给出严格框架）：

内力类型	双臂（\(N=2\)）	\(N\) 臂	物理含义
成对挤压	1 组（两手相向）	\(\binom{N}{2}\) 组潜在	任意两手之间的对夹
独立基数	6 维	\(6N-6\) 维	内力空间总维度

关键细节（Erhart-Hirche 2017）：\(N\ge3\) 时，"内力"的定义比双臂微妙——简单地用 grasp 伪逆 \(G^{+}\) 得到的力分配**未必无内力**，因为内力的判定依赖末端运动学（接触点不能独立移动）。严格的"无内力广义逆"需要把末端运动学约束一并纳入，不能只看 wrench。这是从双臂直觉外推到多臂时最容易踩的坑：双臂的 \(f_{\text{int}}=\tfrac12(h_R-h_L)\) 形式简单，让人以为 \(N\) 臂也能类似地"两两配对"理解，但多臂内力是一个整体的 \(6N-6\) 维子空间，配对只是它的一组（非唯一）基。

\(N\) 臂内力控制律：每只手的期望 wrench 为"分担的运动力 + 内力分量"，运动力由物体阻抗外环给出，内力由各对相对位姿的阻抗内环给出（推广双臂内环）：

\[h_i^{\text{des}}=\underbrace{(G^{+})_i\,F_{\text{load}}^{\text{cmd}}}_{\text{运动力，外环}}+\underbrace{(W\lambda^{\text{des}})_i}_{\text{内力，内环}}\]

内环的 \(\lambda^{\text{des}}\) 通过 QP 或解析在 \(\ker(G)\) 里选取（满足摩擦锥、最小化最大归一化力——这正是 §7.7 的内容）。

反事实推理：\(N\) 臂如果照搬双臂"\(\tfrac12\) 平分运动力"会怎样？双臂平分（每手 \(\tfrac12F\)）在两手对称、能力相同时合理。但 \(N\) 臂时各手位置（\(r_i\)）、能力（\(\tau_{\max,i}\)）不同，平分会让最弱/最不利位置的手先饱和（§7.1 反面教材"四臂抬钢板"）。运动力分配应该用 \(G^{+}\)（最小范数）或加权伪逆（按能力加权），而非朴素平分——双臂的 \(\tfrac12\) 只是 \(G^{+}\) 在对称情形的巧合值。

与机械臂 D03 的分工（避免重复）

主题	机械臂 D03（单系统视角）	本章 §7.4（多机视角）
内力分解 \(f_{\text{int}}=\tfrac12(h_R-h_L)\)	给出双臂形式	从 \(\ker(G)\) 推导 + 推广到 \(N\) 臂 \(W\lambda\)
object/internal impedance	工程实现细节	强调它是绝对/相对对偶的控制化身
无源性 / TDPA / 时延	详细（通信无源、波变量）	不涉及（属单系统力控工程）
ROS2 力控工程	详细	不涉及
\(N\) 臂内力的严格定义	未涉及（聚焦双臂）	Erhart-Hirche 框架，多臂内力依赖运动学
与摩擦锥/QP 分配的接口	简述	指向 §7.7 多臂分配

需要无源性证明、TDPA、波变量、ROS2 力控管线，请回机械臂 D03 与 D05-D07（二端口网络）；本章专注内力的多机推广与内/外解耦的体系结构含义。

⚠️ 常见陷阱

⚠️ 编程陷阱：内环和外环的 wrench 在不同参考点/坐标系下叠加
   错误做法：object impedance 算出的 F_abs 在世界系、internal impedance 的 F_rel
            在某末端系，直接 J_abs^T F_abs + J_rel^T F_rel 叠加
   现象：物体被搬运时内力随姿态漂移，看似"控制器有 bug 但找不到"
   根本原因：两个通道的 wrench 必须在同一参考点（通常物体质心）、同一坐标系
            表达后才能正确投影回关节空间。J_abs/J_rel 也必须对应同一参考点
   正确做法：统一所有 wrench 和雅可比到物体质心参考点（§7.2 的 X(p_c-p_i)），
            内外环一致

💡 概念误区：认为"内力阻抗刚度越大越好（夹得越紧越稳）"
   新手想法："internal impedance 的 K_i 调大，内力大，抓取就越稳。"
   实际上：K_i 过大有两个问题。(1) 内力随相对位姿偏差线性增长，K_i 大时
          微小的跟踪误差就产生巨大内力 → 压坏物体或力矩饱和。(2) 高刚度内环
          降低系统对外部扰动的柔顺性，与外环 object impedance 的"柔顺"目标矛盾，
          可能引发两环对抗、振荡。
   正确理解：K_i 要匹配物体刚度和期望内力区间——足够产生防滑内力，又不至于
            因小误差超过强度上界。通常通过"期望内力 / 可接受相对偏差"反推 K_i。

🧠 思维陷阱：把 N 臂内力当成"N-1 组双臂内力的简单拼接"
   新手想法："三臂就是 (臂1,臂2) + (臂2,臂3) 两组双臂内力。"
   实际上：N 臂内力是一个整体的 6N-6 维子空间，"两两配对"只是它的一组基，
          且配对基之间不独立（臂2 同时出现在两对里）。更重要的是，
          多臂内力的严格定义依赖末端运动学（Erhart-Hirche），不能只看 wrench 配对。
   正确思维：用 grasp 零空间 ker(G) 的整体参数化 W·λ 理解 N 臂内力，
            配对只是直觉辅助，不是严格定义。

练习

1. [推导] 从 \(h=G^{+}F_{\text{load}}+W\lambda\) 出发，对双臂（质心参考点 \(G=[I_6\ I_6]\)）求出 \(G^{+}\) 和一组 \(\ker(G)\) 的基 \(W\)，验证它还原 \(h_L=\tfrac12F-f_{\text{int}}\)、\(h_R=\tfrac12F+f_{\text{int}}\)。再说明 \(G^{+}\) 给出的运动力分配为什么是"平分"。

2. [实现] 在 MuJoCo 中实现上面的 DualArmCoordImpedance，让双 Franka 共持一根弹性梁（用软接触近似），跟踪一条物体圆弧轨迹同时维持 20N 内力。记录内力实测值随轨迹的波动，调 \(K_i,D_i\) 使波动 < 10%。

3. [设计扩展] 把 DualArmCoordImpedance 推广到三臂：实现 grasp matrix \(G\in\mathbb{R}^{6\times18}\)、伪逆运动力分配、\(\ker(G)\) 的内力参数化。三臂共持一块板，要求板水平、内力在摩擦锥内。对比"平分运动力"和"\(G^{+}\) 分配"下各臂力矩峰值。

4. [跨章综合] 回顾第 5 章 Verginis 2019 的分布式自适应搬运（无需负载惯性）。本节的内/外阻抗需要物体动力学参数（\(M_o\) 等）。论证：能否把 Verginis 的自适应思想嫁接到内环，做"无需精确物体刚度的内力调节"？这与本章 §7.5 的分布式架构有何关联？

---

§7.5 主从 vs 协调控制：一场关于体系结构的辩论 ⭐⭐⭐

这一节解决什么问题：前几节建立了"怎么算"（运动学、内力、控制律），这一节回答"谁来算、谁说了算"——这是体系结构问题。双臂/多臂协同有两条历史路线：主从（master-slave）（一只手主导、其余跟随）和**协调（coordinated / non-master-slave）**（各手地位对等、协商运动）。它们对应第 1 章的**集中式 vs 分布式 vs 去中心化**三分法。这一节把这场 40 年的辩论摆开，用第 1-2 章的图论/共识工具分析两者的取舍，并展示 leader-follower 一致性如何把主从"软化"成可扩展的分布式方案。这是本章把"协同操作"真正接回"多机协作"主干的关键一节。

阶段小结（§7.2-7.4 回顾，进入架构前的锚点）：到这里我们完成了协同操作的"数学+控制"三连：(1) §7.2 用对称表述把双臂运动拆成**绝对**（物体运动）+ 相对（内力通道）两个解耦坐标，核心对象是 \(J_{\text{abs}}\) 和 \(J_{\text{rel}}\)；(2) §7.3 用虚功原理证明速度约束 \(\ker(J_{\text{rel}})\) 与力约束 \(\ker(G)\) 是同一闭链的对偶面孔，得出"内力 = 相对运动的力共轭、必须闭环"；(3) §7.4 把这个对偶变成 Caccavale 内/外阻抗双层控制器（外环管绝对、内环管内力），并推广到 \(N\) 臂 \(h=G^{+}F+W\lambda\)。但前三节有一个未言明的假设：所有这些计算由"谁"来做、各臂"谁说了算"？ 我们默认了某个集中的求解器同时算两臂——这正是接下来要审视的体系结构问题。§7.5 拆解这个假设，§7.6 加上任务对称性维度，§7.7 把它扩展到 \(N\) 臂的分布式分配。

动机：同一个任务，两种"指挥结构"

回到两臂端玻璃。玻璃要从 A 点移到 B 点。两种组织方式：

方式一（主从）：指定右手为"主臂"（master）。给主臂一条轨迹 \(T_R^{\text{des}}(t)\)，让它去执行。左手是"从臂"（slave），它的任务是"无论主臂在哪，我都保持和主臂的固定相对位姿"——即从臂跟随主臂。控制结构是单向的：主→从。

方式二（协调）：不指定主从。给"物体"一条轨迹 \(T_{\text{abs}}^{\text{des}}(t)\)（§7.2 的绝对变量），两臂**对称地**协商各自该怎么动，共同实现物体运动，同时维持内力。控制结构是对称的：两臂地位等价。

这两种方式不是实现细节的差别，而是**体系结构的根本分歧**，直接对应第 1 章：

双臂控制方式	第 1 章架构	谁拥有"全局意图"	信息流
主从（master-slave）	集中式（主臂是中心）	主臂独占	单向 主→从
对称协调（non-master-slave）	分布式	物体目标共享给两臂	对称双向
leader-follower 一致性	去中心化（leader 只是 1 个特殊节点）	leader 持有，经一致性传播	沿通信图传播

主从控制：简单，但代价是什么 ⭐⭐⭐

主从是历史最早、实现最简单的方案。它的吸引力显而易见：只需为主臂规划轨迹，从臂的目标由"主臂位姿 + 固定相对变换"自动算出，省掉了"为物体规划再分解到两臂"的步骤。早期遥操作双臂（一只手由人操控、另一只跟随）天然是主从的。

但主从有一个根植于其结构的代价，正是 Uchiyama-Dauchez 1987 年极力反对它的原因：

本质洞察（主从的结构性缺陷）：主从控制把"内力管理"隐式地、不可控地推给了从臂的跟踪误差。主臂只管自己的位姿、完全不知道（也不关心）内力；从臂努力跟随主臂，但它的跟踪误差直接表现为相对位姿偏差，而相对位姿偏差 = 内力（§7.3 对偶）。于是**内力变成了"从臂跟踪误差"的副产品，没有任何环节显式地测量和调控它**。主臂走得快一点、从臂跟不上，内力就飙升；从臂过冲，内力就反向。内力在主从结构里是"失控的自由变量"。

用第 1 章的语言：主从是集中式架构，主臂是单点。它继承了集中式的所有优缺点——简单、有明确的全局协调者；但也有单点的脆弱：主臂的任何问题（奇异、限位、故障）整个系统跟着遭殃，且**没有为"内力"这个协同操作核心量预留控制接口**。

反事实推理：主从控制下，如果主臂经过一个奇异位形会怎样？

  主臂接近奇异 → 主臂为实现期望末端速度需要巨大关节速度 → 主臂实际运动滞后/抖动
       │
       ▼
  从臂忠实跟随"主臂实际位姿"（含滞后/抖动）
       │
       ▼
  但从臂自己未必在奇异 → 它能精确复现主臂的抖动 → 两臂相对位姿剧烈波动
       │
       ▼
  内力剧烈震荡 → 物体被反复挤压拉扯 → 可能脱手或损坏。

  对称协调下：物体轨迹是任务变量，两臂共同分担，
  单臂奇异可由冗余 + 另一臂补偿，内力由内环独立守护，不被某臂奇异绑架。

对称协调：把"物体"提升为一等公民 ⭐⭐⭐

对称协调（Uchiyama-Dauchez non-master-slave）的核心，正是 §7.2 的绝对/相对分解：

• 共同目标是"物体轨迹"\(T_{\text{abs}}^{\text{des}}\)，而非某只手的轨迹——这是"分布式"的标志：目标是共享的集体量，不属于任何单一 agent。
• 两臂对称地实现它：绝对通道（\(J_{\text{abs}}\)）共同驱动物体，相对通道（\(J_{\text{rel}}\)）由内环独立守护内力。
• 内力是显式控制量，有专门的内环（§7.4），不再是跟踪误差的副产品。

对称协调相对主从的优势，恰好是分布式相对集中式的优势（第 1 章）：

维度	主从（集中式）	对称协调（分布式）
内力	失控副产品	显式控制量（内环）
单臂奇异/故障	拖累全局	冗余 + 另一臂补偿
任务语义	"主臂去哪"	"物体去哪"（更贴合意图）
异构臂	主从角色僵化	可按能力分配绝对/相对权重（§7.6 的 \(\alpha\)）
实现复杂度	低	中（需绝对/相对分解 + 双环）
扩展到 \(N>2\)	困难（一主多从协调差）	自然（绝对=共同目标，内力=\(\ker(G)\)）

本质洞察：主从→对称协调的演进，与第 1-4 章"集中式→分布式"的大主题是同一回事。集中式有一个"什么都知道的中心"（主臂/中央 MPC），简单但脆弱、不可扩展；分布式把"全局意图"变成共享的集体目标（物体轨迹/团队目标），各 agent 对称协商，鲁棒且可扩展，代价是需要显式的协调机制（绝对/相对分解 / 共识 / ADMM）。双臂的"主从 vs 协调"是多机"集中 vs 分布"在 \(N=2\) 上的最小实例——这正是本章把双臂放进多机协作课程的深层理由。

leader-follower 一致性：主从的"分布式软化" ⭐⭐⭐

主从（刚性，集中式）和对称协调（完全对等，分布式）之间，还有一个重要的中间形态，它把第 2 章的共识直接用上：leader-follower 一致性。

思想：保留一个 leader（持有任务意图，如物体目标轨迹），但 follower 不再"刚性跟随 leader 的瞬时位姿"，而是**通过一致性协议（consensus）逐步收敛到与 leader 一致的运动**。区别在于：

• 经典主从：follower 直接复现 leader 的位姿（含 leader 的所有抖动/滞后），单向硬耦合。
• leader-follower 一致性：follower 维护自己对"物体该怎么动"的估计 \(\hat x_i\)，通过邻居通信（包括从 leader 处）做一致性更新，驱动估计误差收敛到任意小：

\[\dot{\hat x}_i=-\sum_{j\in\mathcal{N}_i}a_{ij}(\hat x_i-\hat x_j)-b_i(\hat x_i-x_{\text{leader}})\]

这正是第 2 章的 leader-following consensus（带 leader 牵引项 \(b_i\) 的 \(\dot x=-Lx\) 变体）。其收敛速率由通信图的代数连通性 \(\lambda_2\) 决定，收敛到一致所需的"信息传播"正比于图直径——前置自测第 2 题的最大值共识直觉在这里复用。

为什么这是"软化"？ 因为：

性质	经典主从	leader-follower 一致性
follower 抖动放大	是（硬复现 leader）	否（一致性滤波平滑）
扩展到 \(N\) follower	一主多从协调差	自然（多 follower 共识）
通信拓扑	必须 follower 直连 master	任意连通图即可（信息可多跳传播）
鲁棒性	leader 故障全崩	可设计 leader 切换 / 多 leader
架构归类	集中式	去中心化

跨领域类比（带边界）：双臂 leader-follower 一致性 vs 乐队的指挥与乐手。像的部分：指挥（leader）持有整体意图（曲目/物体轨迹），乐手（follower）不是机械地复制指挥的每个手部抖动，而是把指挥的拍子作为参考、结合相邻乐手的演奏，平滑地协调到一致（一致性滤波）。不像的部分：乐手之间靠听觉（高带宽、低延迟）持续微调，机器人受 DDS 通信延迟与频率约束；乐队的"内力"（声部平衡）靠艺术经验，机器人靠 §7.4 的显式内力环。不要把类比延伸到："既然乐队能无指挥即兴（完全去中心化），双臂也总能无 leader"——完全对称协调要求两臂共享物体目标且通信良好；在通信受限或任务天然有主次（一只手扶、一只手做精细操作）时，保留 leader 反而更合适，这就引出 §7.6 的非对称。

三种架构的统一视角与选型 ⭐⭐⭐

把三种架构放在第 1 章的框架里，给出选型决策：

                  谁持有"物体该怎么动"的意图？
                            │
      ┌─────────────────────┼─────────────────────┐
      ▼                     ▼                     ▼
  单个主臂独占           leader 持有，          目标共享给所有 agent，
  (集中式)              经一致性传播           对称协商
                       (去中心化)             (分布式)
      │                     │                     │
   主从控制            leader-follower 一致性   对称协调
      │                     │                     │
  ✅ 实现最简单       ✅ 可扩展到 N follower    ✅ 内力显式、最鲁棒
  ✅ 遥操作天然       ✅ 容忍通信拓扑变化       ✅ 异构臂可调权重
  ❌ 内力失控         ✅ 抖动被滤波平滑         ❌ 实现最复杂
  ❌ 主臂奇异拖累全局  ⚠️ 收敛速率受 λ2 限制     ✅ 自然扩展到 N 臂
  ❌ 难扩展到 N>2     ⚠️ leader 仍是关键节点

场景	推荐架构	理由
人遥操作一只手、另一只跟随	主从	leader 是人，天然单向，内力靠人的力觉
两工业臂固定协作、追求内力精度	对称协调	内力是质量关键，需显式控制
\(N\) 台移动机械臂、通信受限、可能掉线	leader-follower 一致性	可扩展、容忍拓扑变化、抖动滤波
一只手扶持、一只手精细操作	非对称协调（§7.6）	任务天然有主次，但要显式内力
异构臂（能力差异大）	对称协调 + 加权分配	用 \(\alpha\)/加权伪逆把重活给强臂

反事实推理：如果在"\(N\) 台移动机械臂、通信会掉线"的场景硬上对称协调（要求全员共享物体目标、实时双向通信）会怎样？通信掉线时部分 agent 收不到物体目标更新，它们的绝对通道失去参考 → 各自漂移 → 内力失控。leader-follower 一致性更鲁棒：即使某 follower 暂时只能和邻居（非 leader）通信，一致性协议仍能通过多跳把 leader 意图传过去（代价是收敛慢一点，由 \(\lambda_2\) 决定），不会立刻失控。这就是为什么通信受限的多机场景偏好去中心化的一致性架构——它把"对全局信息的依赖"降到"对邻居信息的依赖"。

⚠️ 常见陷阱

🧠 思维陷阱：认为"主从更简单所以更可靠"
   新手想法："主从结构简单、代码少，工业上应该首选。"
   实际上：主从的简单是表面的——它把内力管理这个协同操作的核心难题
          "扫到地毯下"（变成从臂跟踪误差的副产品），并没有真正解决。
          在对内力敏感的任务（搬易碎物、精密装配）里，主从因内力失控
          反而最不可靠。简单≠可靠。
   正确理解：主从适合内力不敏感、有天然 leader（如人遥操作）的场景；
            对内力敏感的任务，多花的协调控制复杂度是值得的。

💡 概念误区：把 leader-follower 一致性等同于经典主从
   新手想法："不都是有个 leader 让别人跟随吗？"
   实际上：经典主从是 follower 硬复现 leader 瞬时位姿（单向、放大抖动、
          要求直连）；leader-follower 一致性是 follower 通过共识协议
          逐步收敛到与 leader 一致（多跳传播、滤波平滑、容忍拓扑变化）。
          后者是去中心化的，前者是集中式的，鲁棒性和扩展性天差地别。
   正确理解：一致性把"刚性跟随"换成"协商收敛"，这是第 2 章共识的直接应用。

⚠️ 编程陷阱：对称协调里两臂的物体目标轨迹时钟不同步
   错误做法：两臂控制器各自从本地时钟生成 x_abs^des(t)，时钟有偏移
   现象：两臂对"物体此刻该在哪"理解不一致 → 相当于给相对通道注入了误差
        → 内力周期性波动
   根本原因：对称协调要求两臂共享同一个物体目标，时钟不同步破坏了"共享"
   正确做法：物体目标轨迹由统一时钟分发（或用一致性同步时钟），
            确保两臂在同一时刻引用同一物体目标

练习

1. [分析] 对"主臂经过奇异位形"的反事实推理（本节给了骨架），用双 Franka 在 MuJoCo 里复现：让主臂轨迹经过一个近奇异构型，记录主从控制下的内力峰值；再换对称协调控制，对比内力峰值。预测：主从内力峰值显著更高。

2. [设计] 为"3 台移动机械臂协同抬桌、其中 1 台可能临时掉线"设计 leader-follower 一致性架构：写出每个 agent 的一致性更新律（含 leader 牵引项），说明掉线时系统如何降级而不崩溃，估计收敛速率与通信图 \(\lambda_2\) 的关系。

3. [跨章综合] 回顾第 2 章 leader-following consensus 和第 1 章三大架构。论证：双臂的"主从→leader-follower 一致性→对称协调"演进，与多机的"集中式→去中心化→分布式"是同一条主线在 \(N=2\) 上的实例。指出三处精确对应和一处不对应（提示：双臂有强物理耦合即内力，一般多机协调（如编队）耦合更弱）。

4. [思考] 主从控制把内力变成"失控副产品"。但遥操作双臂（人控主臂）却广泛用主从且工作良好。为什么？（提示：人通过主臂的力觉反馈隐式地感知并调节内力——人脑充当了缺失的内力环。这说明"内力环"可以由人提供，也说明全自主双臂为什么更需要显式内力控制。）

---

§7.6 对称与非对称协调 ⭐⭐⭐

这一节解决什么问题：§7.5 的"主从 vs 对称协调"是**架构**层面的对立（谁主导）。这一节讨论一个正交的维度：**任务**本身是对称的还是非对称的？"两人抬桌"两手等价（对称），"一手扶食材、一手切"两手分工不同（非对称）。这一节用 §7.2 引入的连续参数 \(\alpha\) 把对称/非对称统一起来，给出非对称任务的扩展相对雅可比落地，并说明它如何对应到第 3 章任务分配的退化形式。理解这一节，你才能正确处理真实世界里大量"两手不平等"的操作任务。

动机：现实中大多数双手任务是非对称的

观察你自己的双手做事：拧瓶盖（一手固定瓶身、一手旋转盖）、穿针（一手持针、一手穿线）、切菜（一手按食材、一手运刀）、写字时另一只手按纸……绝大多数日常双手任务，两只手扮演不同角色：一只通常是"稳定/参考"角色（变化慢、提供支撑），另一只是"操作/精细"角色（变化快、执行主要动作）。

完全对称的任务（两人抬同一根杆、搓手）反而是少数。然而 §7.2 的标准对称表述（绝对=中点、相对=右减左）默认两臂对等——直接用它处理非对称任务会"水土不服"：

反面教材：用对称表述（绝对=两手中点）做"左手扶碗、右手搅拌"

  对称表述把"绝对运动"定义为两手中点的运动。
  但搅拌任务里，我们关心的是：
    - 碗（左手）放在哪里、保持稳定 ← 这是左手的绝对运动
    - 勺子（右手）相对碗怎么转圈 ← 这是相对运动
  "两手中点"这个量在搅拌里没有清晰的物理意义——
  它既不是碗的位置，也不是勺的位置，是个尴尬的几何中点。
       │
       ▼
  给"中点"规划轨迹，需要把"碗稳定 + 勺转圈"反向折算成中点轨迹，
  绕一大圈，且勺子转圈时中点也跟着画小圈，任务描述被扭曲。

本质洞察：对称表述的"绝对=中点"是一个**关于谁代表集体运动的民主假设**。它对真正对称的任务（两手地位等价）是最优的，但对非对称任务，"集体运动"更应该由**承担参考角色的那只手**代表。任务的对称性应当决定"绝对变量怎么定义"——这就是 §7.2 引入连续参数 \(\alpha\) 的全部意义。

用 \(\alpha\) 统一对称与非对称 ⭐⭐⭐

回顾 §7.2 末尾的加权绝对 twist：

\[\mathcal{V}_{\text{abs}}^{(\alpha)}=\alpha\,\bar{\mathcal{V}}_L+(1-\alpha)\,\bar{\mathcal{V}}_R,\qquad \mathcal{V}_{\text{rel}}=\bar{\mathcal{V}}_R-\bar{\mathcal{V}}_L\]

\(\alpha\) 是一个把"集体运动以谁为代表"参数化的连续旋钮：

\(\alpha\)	绝对变量代表	对应任务类型	例子
\(0.5\)	两手中点（民主）	对称任务	两人抬杆、搓手
\(1.0\)	左手（左手为参考）	非对称，左扶右做	左手扶碗、右手搅拌
\(0.0\)	右手（右手为参考）	非对称，右扶左做	右手持工件、左手加工
\(\in(0,1)\)	加权（按能力/惯量）	异构臂	强臂权重高

\(\alpha\) 与 §7.5 架构的关系（关键）：\(\alpha\) 是**任务**维度（任务对称性），§7.5 的主从/对称是**架构**维度（控制组织）。两者正交但相关：

• 对称任务（\(\alpha=0.5\)）+ 对称架构 = 经典 Uchiyama-Dauchez。
• 非对称任务（\(\alpha=1\)）天然倾向于让"参考手（扶持手）"承担类 leader 角色——但**这不等于主从**！即使 \(\alpha=1\)（以左手为绝对参考），内力仍由独立内环显式控制（§7.4），左手并不"无视内力"。这是非对称协调与主从的关键区别：非对称协调保留了内力的显式控制，主从没有。

本质洞察：\(\alpha\) 把"任务的非对称性"和"架构的主从性"解耦了。一个常见混淆是把"一只手扶、一只手做"直接等同于"主从控制"——错。前者是任务非对称（用 \(\alpha\) 表达），后者是架构集中式（用单向信息流表达）。你完全可以用**对称架构（两臂协商、内力显式）执行非对称任务（\(\alpha=1\)）**：扶持手当绝对参考，操作手做相对运动，但两臂仍对等地协商、内力仍被独立守护。\(\alpha=1\) 选的是"谁代表物体运动"，不是"谁单向指挥谁"。

扩展相对雅可比的落地 ⭐⭐⭐

§7.2 给出了非对称任务的扩展相对雅可比：

\[\begin{bmatrix}\mathcal{V}_{\text{rel}}\\ \mathcal{V}_{\text{abs}}^{(k)}\end{bmatrix}=J_{\text{ext}}\dot q,\qquad J_{\text{ext}}=\begin{bmatrix}J_{\text{rel}}\\ J_{\text{abs}}^{(\alpha)}\end{bmatrix}\]

对"左手扶碗、右手搅拌"（\(\alpha=1\)，左手为绝对参考）：

• \(\mathcal{V}_{\text{abs}}^{(1)}=\bar{\mathcal{V}}_L\)：直接控制左手（碗）的运动——任务描述清晰（碗放哪、稳不稳）。
• \(\mathcal{V}_{\text{rel}}=\bar{\mathcal{V}}_R-\bar{\mathcal{V}}_L\)：控制勺相对碗的运动——直接就是"搅拌动作"。

控制律拆成两个解耦任务（任务优先级写法）：

非对称协调控制（左扶右做，α=1）：

  优先级 1（高）：左手绝对运动 = 碗的期望运动（保持碗稳定/到位）
                  在 ker(J_rel 中与碗稳定相容的子空间) 实现
  优先级 2：     右手相对左手运动 = 搅拌轨迹
  内力环（并行）：相对位姿偏差的法向分量 → 维持勺压碗底的接触力
                  （非对称任务的"内力"常是单向接触力，而非对夹力）
  冗余：         自碰撞回避、关节限位

关键区别（非对称内力）：对称任务的内力是"两手对夹"（双向挤压）；非对称任务的内力常是"操作手对参考物的单向作用力"（如勺压碗底、刀压砧板、笔压纸）。这在 grasp matrix 框架里对应不同的内力子空间结构——对称是 \(\ker(G)\) 的"反对称"部分，非对称更关注"操作手→参考"方向的法向力。处理非对称任务时，把内力期望值设计成"维持必要接触力"而非"对夹力"。

把非对称协调连到第 3 章任务分配 ⭐⭐

§7.1 练习 3 已埋下伏笔：双臂的"角色分配（哪只手当参考、哪只手当操作）"是第 3 章任务分配的退化形式。现在把它讲透。

第 3 章把"\(N\) 机器人 × \(M\) 任务，谁干哪件最划算"形式化为指派问题。双臂非对称任务里也有一个角色分配：2 只手 × 2 个角色（参考/操作），谁当参考谁当操作？

维度	一般任务分配（第 3 章）	双臂角色分配（本节）
agent 数	\(N\)（大）	2
任务/角色数	\(M\)	2（参考、操作）
代价	距离/时间	哪只手当参考更稳（工作空间、惯量、当前构型）
关键约束	agent 可独立执行任务	两角色强物理耦合，不能独立
求解	匈牙利/拍卖/CBBA	简单比较（2!=2 种）+ 物理判据

关键区别：一般任务分配假设 agent 能独立执行各自任务（解耦或弱耦合）；双臂角色分配里两个角色**通过共持物体强耦合**——参考手和操作手的运动学、内力完全交织。所以双臂角色分配不能照搬第 3 章的"独立指派"求解，而要结合 §7.2-7.4 的耦合约束。但"为操作选合适的角色分工"这个**思想**是相通的：都是把"谁做什么"显式化为决策变量。

反事实推理：如果角色分配错了（让工作空间更受限的手当操作手）会怎样？操作手要执行精细动作（搅拌/拧/切），需要较大的灵活工作空间和远离奇异的构型；若把它分给一只快到限位、临近奇异的手，操作精度和范围都受损，甚至中途卡死。正确的角色分配应把"操作"角色给当前构型更灵活、操作度更高的手——这是一个（退化的）能力匹配问题，与第 3 章"把任务派给最合适的 agent"同理。在长序列任务里，角色甚至需要中途切换（regrasp/换手），这又回到机械臂 D02 的 TAMP。

对称/非对称选型小结

任务分析决策树：

  两手是否扮演相同角色（可互换）？
  ├── 是 → 对称任务 (α=0.5)
  │        绝对=中点，内力=对夹力
  │        例：两人抬杆、搓手、对称装配
  └── 否 → 非对称任务
           哪只手是"参考/稳定"角色？→ 设它为绝对参考 (α=1 或 0)
           扩展相对雅可比：控制 [相对运动 + 参考手绝对运动]
           内力=操作手对参考的接触力（常单向）
           例：扶切、持拧、按写

  正交地，无论对称/非对称，都要选架构（§7.5）：
  主从 / leader-follower 一致性 / 对称协调
  → 对内力敏感就别用纯主从（内力会失控）

⚠️ 常见陷阱

💡 概念误区：把"非对称任务"等同于"主从控制"
   新手想法："一只手扶、一只手做，那不就是扶的当主、做的当从（主从控制）吗？"
   实际上：非对称是任务属性（两手角色不同，用 α 表达"谁代表物体运动"），
          主从是架构属性（单向信息流、内力失控）。你可以用对称架构
          （两臂协商、内力显式环）执行非对称任务——扶持手当绝对参考，
          但内力仍被独立守护，两臂仍对等协商。混淆二者会让你在需要内力精度的
          非对称任务（如精密装配中一手扶一手插）里错误地用主从，导致内力失控。
   正确理解：任务对称性（α）和架构主从性是两个正交维度，分别决策。

🧠 思维陷阱：对称表述（绝对=中点）适用于所有任务
   新手想法："Uchiyama-Dauchez 对称表述是标准，所有双臂任务都用它。"
   实际上：对称表述对真正对称的任务最优，但对非对称任务，"两手中点"缺乏
          清晰物理意义，强行使用会扭曲任务描述（见本节反面教材）。
   正确思维：用 α 匹配任务对称性——对称任务 α=0.5，非对称任务 α=1/0
            （以参考手为绝对变量）。

⚠️ 编程陷阱：非对称任务里沿用"对夹内力"模型
   错误做法：扶切任务（刀压砧板）里，把内力建模成两手对夹（双向挤压）
   现象：控制器试图让"砧板和刀互相对夹"，但任务只需要"刀向下压砧板"
        的单向接触力，对夹模型产生多余的水平内力，刀打滑
   根本原因：非对称任务的内力常是单向接触力（操作手→参考），不是对夹
   正确做法：按任务定义内力方向——对称任务用对夹，非对称用单向接触力，
            在 ker(G) 里选对应的子空间

练习

1. [标注] 对以下任务标注对称性（给出建议 \(\alpha\)）和内力类型（对夹/单向接触）：①两人抬沙发 ②拧瓶盖 ③穿针引线 ④双手系鞋带 ⑤一手扶梯一手钉钉子。说明每个的"绝对参考"该选谁。

2. [实现] 用扩展相对雅可比实现"左手持板、右手在板上画直线"：\(\alpha=1\)（左手为绝对参考），优先级 1 保持板稳定，优先级 2 让右手沿板面直线运动，内力环维持右手笔尖对板的法向压力 5N。在 MuJoCo 验证。

3. [设计] 为长序列任务"把零件从左手递到右手再装配"设计角色切换：递交前左手为操作手、右手为参考；递交后角色互换。说明切换时刻如何避免内力突变（提示：切换瞬间 \(\alpha\) 不能跳变，需平滑过渡）。

4. [跨章综合] 结合第 3 章任务分配和机械臂 D02 的 TAMP。论证："长序列双臂操作中的角色分配 + 换手时机"是一个 TAMP 问题（离散角色决策 + 连续运动），而单步的角色选择是第 3 章任务分配的退化（\(N=2\)、强耦合）。给出一个需要换手的任务并说明 TAMP 如何决策换手点。

---

§7.7 多臂负载分配：\(N\ge 3\) 的优化与分布式 ⭐⭐⭐⭐

这一节解决什么问题：双臂的力分配很简单——运动力平分、内力对夹。但 \(N\ge 3\) 时，"\(N\) 只手怎么分担一个负载的力"变成一个有 \(6N-6\) 维内力自由度的**优化问题**：在满足摩擦锥、不超能力上限的前提下，怎么分配各手的 wrench 才最优（最小化最大归一化力 / 兼顾能力差异 / 留足裕度）？这一节把它形式化为 QP，再用第 4 章的 ADMM 做分布式版本（无中央节点）。这是本章难度最高、也最能体现"协同操作 = 多机资源分配"的一节，做多臂/多机搬运必读。

动机：为什么 \(N\ge 3\) 后"平分"不再可行

双臂 \(N=2\) 时，运动力平分（每手 \(\tfrac12F\)）+ 内力对夹，简单且常常够用——因为 \(N=2\) 的内力空间只有 6 维，自由度小，对称情形下平分恰好最优。

\(N\ge 3\) 后情况质变（回顾 §7.1 反面教材"四臂抬钢板"）：

三臂抬一块质心偏置的板（俯视）：

      臂1 ●            ● 臂2          臂1,臂2 离质心远（力臂大）
          ╲          ╱                臂3 离质心近（力臂小）
           ╲        ╱                 三臂电机扭矩上限还各不相同
            ╲      ╱
             ╲    ╱
        ─────●质心●─────  板
              │
            臂3 ● （靠近质心一侧）

  朴素"三等分"：每臂承担 1/3 重量。
  问题：臂3 离质心近、力臂小，承担 1/3 重量需要的关节力矩可能
        反而最大（要在小力臂上产生大力）；或者臂3 电机最弱。
        谁先到力矩上限，谁就饱和，份额甩给邻居 → 连锁过载。

本质洞察：\(N\ge3\) 时，负载分配从"一个确定的平分公式"变成"一个带约束的优化问题"，因为存在 \(6N-6\) 维的内力自由度可以利用。这个自由度既是麻烦（不再有唯一答案）也是机会（可以优化——让强臂多担、弱臂少担，留足防滑裕度，远离力矩饱和）。双臂的"平分"只是这个优化在 \(N=2\)、对称、无能力差异时的退化最优解。一旦异构或 \(N\ge3\)，必须真正去优化。

集中式 QP 形式 ⭐⭐⭐⭐

把负载分配写成标准 QP。决策变量是各手 wrench 堆叠 \(h=[h_1;\dots;h_N]\in\mathbb{R}^{6N}\)。

等式约束（必须实现物体运动）：各手合成 wrench 等于物体所需 wrench（含重力、惯性）：

\[G\,h=F_{\text{load}}^{\text{cmd}}\]

这把 \(h\) 限制在一个仿射子空间——\(G^{+}F_{\text{load}}^{\text{cmd}}+\ker(G)\)，剩下 \(6N-6\) 维内力自由度待优化。

不等式约束（物理可行）：

• 摩擦锥（接触不打滑）：每个接触力 \(f_i\) 落在摩擦锥内，\(\|f_i^{\text{tangent}}\|\le\mu_i f_i^{\text{normal}}\)，且 \(f_i^{\text{normal}}\ge f_{\min}\)（最小法向力防滑）。摩擦锥是二阶锥，可用线性化（金字塔近似）变成线性不等式 \(C_i h_i\le 0\)。
• 能力上限（电机不饱和）：各臂关节力矩 \(\tau_i=J_i^\top h_i\) 满足 \(|\tau_i|\le\tau_{\max,i}\)。

代价函数（优化目标，几选其一或加权）：

目标	代价	含义
最小接触力范数	\(\sum_i\|h_i\|^2\)	最省力，但可能不公平
最小化最大归一化力	\(\min\max_i \|h_i\|/\tau_{\max,i}\)（min-max，化为 LP/QP）	公平，远离饱和，异构友好
加权范数	\(\sum_i w_i\|h_i\|^2\)，\(w_i\propto1/\text{能力}_i\)	强臂多担
内力正则	\(\|f_{\text{int}}-f_{\text{int}}^{\text{des}}\|^2\)	把内力拉向防滑裕度区间

综合 QP：

\[\boxed{\;\min_{h}\ \tfrac12 h^\top Q h+c^\top h\quad\text{s.t.}\quad Gh=F_{\text{load}}^{\text{cmd}},\ \ C h\le d\;}\]

\(Q\) 编码代价（加权范数 + 内力正则），\(Ch\le d\) 堆叠摩擦锥线性化 + 力矩上限。这是一个凸 QP，可用 OSQP/qpOASES 实时求解。

本质洞察：多臂负载分配的 QP 和第 5 章的"\(f_{\text{contacts}}=G^{+}F_{\text{load}}+(I-G^{+}G)f_{\text{int}}\)"是同一回事的两种表述——参数化（\(f_{\text{int}}\) 自由）vs 优化（在约束下选最优 \(f_{\text{int}}\)）。第 5 章给的是"内力是自由参数"的结构性认识，本节把它升级为"在摩擦锥+能力约束下选最优内力"的可执行优化。从结构到优化，正是从"理解"到"工程"的一步。

# ============================================================
# 多臂负载分配 QP（集中式）——以三臂为例，OSQP 求解
# 决策变量 h = [h_1; h_2; h_3] ∈ R^18（每臂 6D wrench）
# ============================================================
import numpy as np
import osqp
from scipy import sparse

def build_grasp_matrix(contact_pts, com):
    """G ∈ R^{6 × 6N}，每手传 6D wrench（力+力矩），质心参考点。"""
    N = len(contact_pts)
    G = np.zeros((6, 6*N))
    for i, p in enumerate(contact_pts):
        r = np.array(p) - np.array(com)
        rx = np.array([[0,-r[2],r[1]],[r[2],0,-r[0]],[-r[1],r[0],0]])
        G[0:3, 6*i:6*i+3] = np.eye(3)        # 力 → 力
        G[3:6, 6*i:6*i+3] = rx               # 力 → 力矩 (r × f)
        G[3:6, 6*i+3:6*i+6] = np.eye(3)      # 力矩 → 力矩
    return G

def solve_load_distribution(contact_pts, com, F_cmd,
                            tau_max, mu=0.5, f_min=5.0, w=None):
    N = len(contact_pts)
    G = build_grasp_matrix(contact_pts, com)
    nh = 6*N

    # 代价：加权最小范数 + 轻内力正则（w_i 反比于能力，强臂权重小→多担）
    if w is None: w = np.ones(N)
    Wdiag = np.concatenate([w[i]*np.ones(6) for i in range(N)])
    Q = sparse.diags(Wdiag + 1e-3)           # +1e-3 保证正定
    c = np.zeros(nh)

    # 等式约束 G h = F_cmd
    A_eq, l_eq, u_eq = G, F_cmd, F_cmd

    # 不等式：法向力下界 f_normal >= f_min（假设接触法向 = 各手 +z 占位）
    # 摩擦锥金字塔近似 + 力矩上限（此处给法向下界与简化锥，工程中按真实接触系建模）
    rows = []
    lo, hi = [], []
    for i in range(N):
        e = np.zeros(nh); e[6*i+2] = 1.0     # f_i 的法向(z)分量
        rows.append(e); lo.append(f_min); hi.append(np.inf)
        # 摩擦锥金字塔： |f_x| <= mu f_z, |f_y| <= mu f_z  →  4 条线性不等式
        for sx in (+1,-1):
            r = np.zeros(nh); r[6*i+0] = sx; r[6*i+2] = -mu
            rows.append(r); lo.append(-np.inf); hi.append(0.0)
        for sy in (+1,-1):
            r = np.zeros(nh); r[6*i+1] = sy; r[6*i+2] = -mu
            rows.append(r); lo.append(-np.inf); hi.append(0.0)
    A_ineq = np.array(rows)

    A = sparse.vstack([sparse.csc_matrix(A_eq), sparse.csc_matrix(A_ineq)]).tocsc()
    l = np.concatenate([l_eq, np.array(lo)])
    u = np.concatenate([u_eq, np.array(hi)])

    prob = osqp.OSQP()
    prob.setup(P=sparse.csc_matrix(Q), q=c, A=A, l=l, u=u, verbose=False)
    res = prob.solve()
    h = res.x
    return h.reshape(N, 6), G

# --- 用法：三臂抬质心偏置的板，臂3 能力弱 ---
contact = [[0.5,0.4,0.0],[0.5,-0.4,0.0],[-0.3,0.0,0.0]]  # 臂3 靠近质心
com = [0.1,0.0,0.0]
F_cmd = np.array([0,0,120, 0,0,0])      # 提 120N（重力补偿，简化）
tau_max = [40,40,20]                     # 臂3 上限只有一半
w = np.array([1.0,1.0,3.0])              # 臂3 权重高→少担（反比能力）
h, G = solve_load_distribution(contact, com, F_cmd, tau_max, w=w)
print("各臂 wrench:\n", np.round(h,2))
print("合成验证 G h - F_cmd =", np.round(G @ h.flatten() - F_cmd, 4))  # ~0

平分 vs QP 的数值对比（定性预期）。 对上面"三臂抬偏置板、臂3 能力弱（\(\tau_{\max,3}\) 只有一半）"的场景，朴素平分与加权 QP 的关键差异在于"最弱臂的归一化负载"——即各臂承担力相对其能力的比例 \(\|h_i\|/\tau_{\max,i}\)，谁的归一化负载先到 1 谁就饱和：

指标	朴素平分（各 1/3）	加权 QP（\(w_3\) 高→臂3 少担）
臂3 承担力占比	~33%（不顾它能力弱）	显著 < 33%（QP 把重活转给臂1、臂2）
臂3 归一化负载 \(\|h_3\|/\tau_{\max,3}\)	最高，最先逼近饱和	被压低，远离饱和
系统"最大归一化负载"	由最弱臂决定，偏高	被均衡，整体下降
防滑/摩擦锥	不保证（无 \(f_{\min}\) 约束时）	显式约束，保证

本质洞察：平分优化的是"绝对力均等"，QP（加权 / min-max）优化的是"归一化负载均衡"——后者才是异构系统真正该关心的。一个直观判据：系统的搬运能力上限由"最先饱和的那只手"决定，所以应该最小化"最大归一化负载"（min-max），而非让绝对力相等。这与第 3 章任务分配里"最小化 makespan（最晚完成的 agent）"而非"总代价"的思想完全一致——多机系统的瓶颈往往是最差的那个个体，优化目标应对准瓶颈。

反事实推理：QP 里如果去掉"最小法向力 \(f_{\min}\)"约束会怎样？求解器为了最小化代价，会把某些接触法向力压到接近零——这在数学上最省力，但物理上意味着那只手"几乎没夹住"，稍有扰动就打滑脱落。\(f_{\min}\) 约束（以及内力正则把内力拉向防滑裕度）是把"防滑"这个物理必需写进优化的方式。没有它，最优解在数学上完美、在物理上危险——这是优化驱动控制的典型陷阱：目标函数没编码的约束，求解器不会替你考虑。

分布式版本：用 ADMM 把 QP 拆给各 agent ⭐⭐⭐⭐

集中式 QP 需要一个中央节点收集所有 \(r_i,\tau_{\max,i}\)、求解、下发 \(h_i\)。但真实多机系统（\(N\) 台移动机械臂、分布在多个计算节点）希望**每个 agent 只用本地 + 邻居信息**求解——这正是第 4 章 ADMM 的用武之地。

回顾第 4 章：ADMM 把一个带耦合约束的大优化拆成各 agent 的本地子问题 + 一致性更新。这里的耦合约束是 \(Gh=F_{\text{load}}^{\text{cmd}}\)（所有手的 wrench 必须合成出物体所需 wrench）——一个把所有 agent 绑在一起的全局等式约束。

ADMM 分解思路：

全局问题：min Σ_i J_i(h_i)   s.t.  Σ_i G_i h_i = F_cmd,  h_i ∈ 局部可行集(摩擦锥/力矩)
                                    └──── 耦合约束 ────┘

引入"物体 wrench 残差"作为对偶变量（拉格朗日乘子 λ，物理上是
"物体当前合成 wrench 与目标的差"对应的协调价格）：

每个 agent i 的本地子问题（只需自己的 G_i, J_i, τ_max,i + 全局 λ）：
  h_i ← argmin J_i(h_i) + λ^T (G_i h_i) + (ρ/2)||G_i h_i - z_i||²
        s.t. h_i ∈ 局部可行集

z 更新（一致性，对应"物体 wrench 该如何在 agent 间分摊"）：
  把 Σ G_i h_i = F_cmd 的残差均摊/协商

λ 更新（对偶上升，广播残差）：
  λ ← λ + ρ (Σ_i G_i h_i - F_cmd)

每个 agent 只解一个**小 QP**（自己的 6 维 wrench + 局部约束），全局协调通过交换 \(\lambda\)（物体 wrench 残差，6 维，很小）完成。\(\lambda\) 的交换可以用第 2 章的共识（如果没有中央节点广播，各 agent 通过邻居一致性估计全局残差）。

本质洞察：多臂负载分配的 ADMM 分解，\(\lambda\)（对偶变量）的物理意义是"物体 wrench 的协调价格"——它告诉每个 agent"物体现在还差多少 wrench，你该多出还是少出力"。这与第 3 章拍卖机制的"价格"、第 4 章 ADMM 的"对偶变量"是同一个思想：用一个共享的标量/向量价格协调分布式资源分配，每个 agent 只需看价格（而非别人的全部状态）就能做出协调的本地决策。协同操作的力分配，至此完全融入第 3-4 章的分布式优化框架。

跨领域类比（带边界）：ADMM 力分配的对偶变量 \(\lambda\) vs 市场的价格信号。像的部分：市场里没有中央计划者，每个买卖方只看价格就能做出协调决策，价格通过供需失衡（残差）自动调整；ADMM 里每个 agent 只看 \(\lambda\)（物体 wrench 缺口的价格）就能决定本地出力，\(\lambda\) 通过残差（\(\sum G_ih_i-F_{\text{cmd}}\)）调整。不像的部分：市场价格是分散主体自利博弈的涌现，ADMM 的 \(\lambda\) 是为最小化全局代价而设计的更新律，有收敛保证；市场可能有信息不对称、欺诈，ADMM 假设 agent 诚实协作。不要把类比延伸到："既然市场无需协调就能均衡，多臂也无需任何通信"——ADMM 仍需交换 \(\lambda\)（哪怕只是 6 维、可经共识传播），完全零通信的力分配要靠隐式力反馈（下节前沿的 decPLM），是另一条更激进的路线。

工程现实：QP 力分配的实时性与降级

考量	集中式 QP	ADMM 分布式
求解延迟	单次 QP（\(6N\) 维），毫秒级，\(N\) 大时变慢	各 agent 并行解小 QP，但需多轮 ADMM 迭代通信
通信需求	中央收集全局、下发 \(h_i\)	仅交换 \(\lambda\)（6 维），可经共识
单点故障	中央节点挂 = 全崩	无中央节点，鲁棒
某 agent 饱和/掉线	重解 QP（去掉该 agent 的列）	该 agent 退出，其余继续协商（份额自动转移）
适用规模	\(N\) 小到中（2-6）	\(N\) 中到大、分布式部署

本质洞察：负载分配的"集中 QP vs 分布式 ADMM"选择，第三次重复了本章（也是本课程）的核心主题——集中式简单但有单点、不可扩展；分布式（ADMM/共识）需要协调机制但鲁棒、可扩展。§7.5 在控制架构上、§7.7 在力分配上，都是这同一个"集中 vs 分布"权衡的具体化。当你理解了这一点，本章的"主从 vs 协调""集中 QP vs ADMM"就不是孤立的知识点，而是同一个多机协作根本矛盾的不同切面。

⚠️ 常见陷阱

⚠️ 编程陷阱：QP 力分配里摩擦锥用二阶锥但喂给只支持线性约束的求解器
   错误做法：把 ||f_tangent|| <= mu·f_normal（二阶锥）直接当线性约束塞进 OSQP
   现象：求解器报错或静默把锥当成别的约束，解出的力实际违反摩擦锥 → 打滑
   根本原因：摩擦锥是二阶锥约束，纯 QP 求解器（OSQP 处理 QP+线性约束）
            不直接支持；需要金字塔线性化（4-8 条线性不等式近似锥）或用
            SOCP 求解器（ECOS/Mosek）
   正确做法：要么金字塔线性化后用 QP（保守，锥被内接近似），
            要么用 SOCP 精确处理。线性化边数越多越接近真锥但 QP 越大

💡 概念误区：认为"分布式 ADMM 一定比集中式 QP 慢"
   新手想法："ADMM 要迭代很多轮还要通信，肯定比直接解一个 QP 慢。"
   实际上：取决于规模和部署。N 小、单机时集中 QP 更快（一次求解）；
          但 N 大、分布式部署时，集中式要把全局信息汇总到一点（通信瓶颈）+
          解一个大 QP（O(N³) 级），而 ADMM 各 agent 并行解小 QP，
          总 wall-clock 可能更短，且无单点瓶颈。
   正确理解：集中 vs 分布的选择取决于 N、是否分布式硬件、通信代价，
            不是单纯的"迭代=慢"。

🧠 思维陷阱：把 N 臂内力当成"每对相邻臂各自调内力"
   新手想法："N 臂力分配 = 让每对相邻的手各自维持对夹内力。"
   实际上：N 臂内力是整体的 6N-6 维子空间上的优化，各对内力相互耦合
          （一只手同时参与多对），且要全局满足"合成出物体 wrench"。
          局部地各调各的对夹，无法保证全局物体运动正确、也无法全局最优分配。
   正确思维：用全局 QP（或 ADMM 分解的全局一致性）在 ker(G) 整体上优化，
            而非局部成对处理。

练习

1. [实现] 跑通本节的三臂 QP 力分配代码。改变臂3 的位置（从靠近质心移到远离质心）和 \(\tau_{\max,3}\)，观察最优分配如何变化。验证：当某臂能力很弱时，QP 自动让它少担、邻居多担。

2. [推导] 把摩擦锥的金字塔线性化写清楚：给定接触法向 \(n_i\) 和摩擦系数 \(\mu_i\)，用 \(m\) 边金字塔近似摩擦锥，写出对应的 \(m\) 条线性不等式。讨论 \(m\) 增大时的精度-计算量权衡。

3. [设计扩展·研究级] 实现 §7.7 的 ADMM 分布式力分配：三臂各跑本地 QP，通过交换对偶变量 \(\lambda\)（物体 wrench 残差）协调。对比集中式 QP 的解（应收敛到一致），记录 ADMM 收敛所需轮数。再模拟"臂2 掉线"，验证其余两臂能否重新协商出可行分配。

4. [跨章综合·研究级] 把第 4 章 ADMM、第 2 章共识、本节力分配串成一个完整系统：\(N\) 台移动机械臂协同搬运，物体目标用第 2 章 leader-follower 一致性达成，力分配用本节 ADMM，且 \(\lambda\) 的全局残差通过第 2 章共识估计（无中央节点）。画出系统框图，标出每个模块来自哪一章，并指出这个系统在"耦合强度谱"（§7.1）上的位置。

---

§7.8 与单系统双臂章节的关系，及学习范式简述 ⭐⭐

这一节解决什么问题：本章一路从多机视角讲双臂/多臂的经典控制。但你可能困惑：(1) 这和 05_运动控制/20_机械臂 那套双臂子课程到底怎么分工，我该看哪个？(2) 现在不是流行 ALOHA、Diffusion Policy 这些"学双臂"的方法吗，经典控制还重要吗？这一节把这两个边界问题讲清楚，给你一张"什么内容在哪学"的总地图，并说明经典控制与学习范式的互补关系。

本章 vs 机械臂双臂子课程：一张总分工表

本项目有**两套**讲双臂的内容，立场不同、互补而非重复：

维度	05_运动控制/20_机械臂 D01-D04（单系统视角）	本章（多机协作视角）
立场	双臂 = 一台 14-DOF 机器	双臂 = \(N=2\) 多机协同的特例
运动学	\(J_{\text{aug}},J_{\text{rel}}\) 完整推导 + Pinocchio C++/Python 工程实现、奇异/阻尼细节（D01）	从对称表述推导 \(J_{\text{rel}}\) 来历，引入 \(J_{\text{abs}}\) 与 \(\alpha\)，推广到 \(N\) 臂（§7.2）
规划	约束流形采样、TSR、CBiRRT、Atlas、TAMP、MoveIt2 配置（D02）	不涉及（引用 D02）；只在角色分配处连到第 3 章任务分配（§7.6）
力控	object/internal 阻抗工程、无源性、TDPA、波变量、ROS2 力控管线（D03、D05-D07）	内力的 \(\ker(G)\) 推导与 \(N\) 臂推广、内/外解耦的体系结构含义（§7.4）
架构	较少展开主从 vs 协调的多机辨析	主从/leader-follower/对称协调与第 1 章三架构对应（§7.5）
多臂 \(N\ge3\)	聚焦双臂	grasp QP + ADMM 分布式力分配，连第 4-5 章（§7.7）
学习	ALOHA/ACT/Diffusion Policy 深入（D04）	仅简述，指向 D04（本节下面）
力空间统一	grasp matrix 简述	\(\ker(J_{\text{rel}})\)/\(\ker(G)\) 对偶（§7.3，连第 5 章）

怎么用这两套内容：

你的目标 → 看哪套：

  "我要为一台双臂机器人写完整的规控栈
   （IK、约束规划、阻抗力控、MoveIt2、ROS2 部署）"
   → 主看 05_运动控制/20_机械臂 D01-D09（单系统工程深度）
     本章作为"多机视角补充"，理解架构选型与可推广性

  "我要做多机协作（多臂/多移动机械臂/多足搬运），
   双臂只是其中最小的耦合单元"
   → 主看本章（多机视角 + N 臂推广 + 分布式）
     需要双臂运动学/力控工程细节时回查 D01/D03

  "我想理解协同操作的数学本质（对偶、内力、协调）"
   → 本章 §7.2-7.4（对称表述 + ker(G)/ker(J_rel) 对偶 + 内力推广）
     是最凝练的统一视角，D01-D03 提供工程落地

  "我要做双臂技能学习（模仿学习/RL）"
   → 主看 D04（ALOHA/ACT/Diffusion Policy），本节仅给路标

本质洞察：同一个对象（双臂）值得从两个视角各讲一遍，不是冗余而是**互补**——单系统视角给你"把这台双臂机器调好"的工程深度（API、奇异、ROS2），多机视角给你"把双臂知识长进多机系统"的结构性认识（对偶、架构、\(N\) 臂推广）。一个工程师两者都需要：前者让你今天就能让双臂跑起来，后者让你明天能把它扩展成多机协作系统而不推倒重来。视角不是非此即彼，是同一座山的两条上山路。

学习范式简述：从"建模控制"到"模仿学习" ⭐⭐

2022 年 ALOHA 之后，双臂操作出现了一条与经典控制并行的**学习范式**路线。本章主体讲经典控制（因为它可分析、可推广到多机），这里简述学习范式并说清两者关系——深入见机械臂 D04。

核心转变：经典控制要你**显式写出**相对雅可比、内力分解、阻抗律；学习范式让机器**从人类演示中学**双臂协调，不显式建模运动学/力学。

工作	年份	核心思想（多机视角注解）
ALOHA / ACT	2022	低成本遥操作采数据 + Action Chunking Transformer。遥操作天然是**主从**（人控 leader 臂、follower 复现），数据里隐含了人脑提供的内力协调
Mobile ALOHA	2024	加移动底盘，14 关节 + 2D 底盘 = 16D 动作；co-training 大幅提升成功率
Diffusion Policy	2023	DDPM 去噪生成双臂动作 chunk，天然多模态、精度高
leader-follower 动作分解	2024-2025	把双臂动作预测分解为"leader 臂预测 + follower 条件于 leader"两步——和 §7.5 的 leader-follower 架构同构，只是从控制律变成了网络结构
π₀ 等 VLA	2024-2025	视觉-语言-动作大模型直出双臂+移动控制

本质洞察：学习范式没有抛弃本章的概念，而是把它们**从"人工设计的控制律"变成了"网络的归纳偏置或隐式表征"**。最典型的是 leader-follower 动作分解：经典控制里它是 §7.5 的架构选择（leader 持意图、follower 一致性跟随），在学习里它变成网络结构（follower 分支条件于 leader 分支的预测）——同一个"主从/leader-follower"思想，换了实现载体。理解经典的对称/相对/内力概念，反而能帮你看懂学习方法在隐式地处理什么、它们的归纳偏置合不合理。

经典控制 vs 学习范式：互补而非替代：

维度	经典控制（本章）	学习范式（D04）
内力	显式建模、可精确调控	隐式（位置控制为主，力 demo 难采）——当前学习范式的短板
可分析	强（稳定性、对偶可证）	弱（黑盒）
泛化到新任务	需重新设计	数据内泛化好、外推弱
推广到多机/\(N\) 臂	自然（本章主线）	困难（多 agent 数据爆炸）
精细/接触任务	强（内力控制）	精度高但力控弱

关键开放问题（连接前沿）：双臂力控在模仿学习里如何表达？ALOHA 体系主要做**位置控制**，力控 demo 数据难采集——这正是 §7.4 的显式内力控制**不会被学习取代**的原因：对内力敏感的精细装配、易碎物操作，仍需要本章的内力建模。一个活跃方向是把经典内力控制作为"安全外壳"包裹学习策略——学习策略给运动意图，经典内力环守护内力安全。这是经典控制与学习范式融合的典型形态，也呼应第 12 章"MARL + 传统规控混合"的主题。

⚠️ 常见陷阱

💡 概念误区：认为"有了 ALOHA 这类学习方法，经典双臂控制就过时了"
   新手想法："直接学就行，何必推导相对雅可比、内力分解这些复杂数学？"
   实际上：(1) 学习范式当前主要做位置控制，对内力敏感的任务（精密装配、
          易碎物）力控仍是短板，需经典内力控制兜底。(2) 学习方法难以推广到
          多臂/N 机（数据爆炸），而本章的闭链数学天然推广。(3) 学习方法是
          黑盒，安全攸关场景需要经典控制的可分析性。
   正确理解：经典控制与学习范式互补——学运动意图、经典守内力与安全。
            理解经典概念还能帮你看懂学习方法在隐式处理什么。

🧠 思维陷阱：在两套双臂内容之间反复横跳、重复学习
   新手想法："D01 和本章 §7.2 都讲相对雅可比，我得两边都精读一遍。"
   实际上：两者立场不同、不重复。D01 给工程实现深度，§7.2 给对称表述的
          来历与多机推广。按你的目标选主线（见上方"怎么用这两套内容"），
          另一套作为补充查阅，不必两边都从头精读。
   正确思维：用总分工表定位，按目标取舍，避免重复劳动。

练习

1. [定位] 给出 5 个具体工程任务（如"双 Franka 装配齿轮箱""两台移动机械臂抬门""学习双臂叠衣服"），对每个判断应主看本章还是机械臂 D01-D04，说明判据。

2. [分析] ALOHA 的遥操作采数据是主从结构（人控 leader 臂）。结合 §7.5，论证：为什么遥操作主从在采数据时可行（人脑当内力环），但训练出的策略部署时若仍是纯位置控制，在内力敏感任务上会失败？如何用 §7.4 的内力环改进？

3. [思考·研究级] "把经典内力控制作为学习策略的安全外壳"——设计一个具体方案：学习策略输出物体运动意图（绝对通道），经典内力环（§7.4）守护内力安全（相对通道）。说明两者如何在 §7.2 的绝对/相对分解下自然结合，以及这与第 12 章"MARL+传统规控混合"的关系。

---

本章常见误解汇总

把全章散落的"概念误区/思维陷阱"收口成一张表，便于回查。左列是常见的错误认知，右列是正确理解。

误解	正确理解	出处
多机协作视角只是换说法，对实现无影响	视角决定模块边界 → 决定代码结构、容错、可扩展性；视角 (A) 加臂要重写，视角 (B) 增量扩展	§7.1
协同操作 = 协同搬运（第 5 章已讲，本章重复）	搬运关注力分配（常点接触抽象）；操作多一层运动学协调（相对雅可比、在手重定向、内力精调）	§7.1
独立控制两只手，只要目标相容就行	两个独立控制器的跟踪误差在相对位姿上累积 = 内力爆炸；必须用绝对/相对解耦后分别做运动/力控	§7.2
相对雅可比就是 \(J_R-J_L\)	必须先用平移算子统一参考点再做差；本质是 \(SE(3)\) 相对位姿的微分，不是矩阵减法	§7.2
有了 \(J_{\text{rel}}\) 投影就够，\(J_{\text{abs}}\) 没用	\(J_{\text{abs}}\) 管"物体往哪走"，缺了它能不挤压物体却无法精确控制物体位置；内力解耦也需要它	§7.2
内力是浪费，应调成零	内力是抓取稳定性来源（防滑），目标是"防滑下界↔强度上界"的区间，不是零	§7.3、§7.4
\(\ker(G)\) 是固定子空间	\(G\) 依赖 \(r_i\)，物体一动 \(\ker(G)\) 就旋转；开环内力会泄漏到运动通道	§7.3
内力可以开环施加	内力子空间随构型旋转，开环力会"漏"到运动通道；必须用相对位姿偏差闭环	§7.3、§7.4
内力阻抗刚度越大越稳	\(K_i\) 过大 → 微小误差产生巨大内力 → 压坏物体/饱和，且与外环柔顺矛盾	§7.4
\(N\) 臂内力 = \(N-1\) 组双臂内力拼接	\(N\) 臂内力是整体 \(6N-6\) 维子空间，配对只是一组（非唯一）基，且依赖末端运动学（Erhart-Hirche）	§7.4
主从更简单所以更可靠	主从把内力管理"扫到地毯下"（从臂跟踪误差副产品），内力敏感任务反而最不可靠	§7.5
leader-follower 一致性 = 经典主从	经典主从硬复现 leader 位姿（单向、放大抖动）；一致性是协商收敛（多跳、滤波、去中心化）	§7.5
非对称任务 = 主从控制	非对称是任务属性（\(\alpha\)），主从是架构属性；可用对称架构执行非对称任务且内力显式	§7.6
对称表述（绝对=中点）适用所有任务	对称表述对对称任务最优；非对称任务"中点"无清晰物理意义，应用 \(\alpha\) 以参考手为绝对变量	§7.6
\(N\) 臂力分配照搬双臂"平分"	\(N\ge3\) 有 \(6N-6\) 维内力自由度待优化；异构/偏置下平分让弱臂先饱和，须用 QP	§7.7
分布式 ADMM 一定比集中 QP 慢	取决于 \(N\)、是否分布式硬件、通信代价；\(N\) 大分布式部署时 ADMM 可能更快且无单点	§7.7
有了 ALOHA 学习方法，经典控制过时	学习当前主要做位置控制、力控是短板、难推广多机、黑盒；与经典互补（学意图+经典守内力）	§7.8

---

本章小结

本章站在**多机协作**的立场重新讲了双臂与多臂协同操作。一句话总括全章的认知主线：

双臂是"\(N=2\) 的多机协同操作"——它在最少自由度上同时具备多机系统的全部核心矛盾（共享目标一致、资源/力分配、异构容错），而把"臂"换成"腿"或加上浮动基座、把 \(N\) 从 2 加到任意，同一套闭链数学（相对雅可比、grasp matrix、内力分配、绝对/相对解耦）原样复用。协同操作的"集中 vs 分布"权衡（主从 vs 协调、集中 QP vs ADMM）与第 1-4 章的多机主题完全同构。

七节的逻辑链：

§7.1 视角重置：双臂 = N=2 多机协同（翻译表 + 耦合强度谱）
   │  （为什么这么看 → 能复用第 1-5 章全部工具）
   ▼
§7.2 协同运动学：对称表述 → 绝对/相对分解 → 相对雅可比来历 + α 统一
   │  （怎么把运动拆成"物体运动 + 相对运动"两个解耦通道）
   ▼
§7.3 约束的两个面孔：ker(J_rel)[速度] 与 ker(G)[力] 的对偶（虚功原理）
   │  （把第 5 章的力视角和 D01 的速度视角焊在一起 → 内力必须闭环）
   ▼
§7.4 内力控制：f_int=½(h_R-h_L) → Caccavale 内/外阻抗 → N 臂 Wλ 推广
   │  （对偶理论的控制器化身：外环管绝对、内环管相对）
   ▼
§7.5 主从 vs 协调：对应第 1 章集中/分布/去中心化 + leader-follower 一致性
   │  （体系结构辩论：谁持有物体意图 → 内力是否失控）
   ▼
§7.6 对称/非对称：α 统一任务对称性，扩展相对雅可比，连第 3 章角色分配
   │  （任务维度 ⊥ 架构维度，正交两旋钮）
   ▼
§7.7 多臂分配：grasp QP（摩擦锥+能力）→ ADMM 分布式（连第 4 章）
   │  （N≥3 力分配 = 多机资源分配的优化）
   ▼
§7.8 与单系统双臂章节分工 + 学习范式互补（指向 D04）

术语速查表

术语（中 / 英）	一句话定义
协同操作 / Cooperative manipulation	多个有完整运动学的末端共持负载，既分担力又协调运动、调内力
绝对变量 / Absolute variable	两末端的"代表运动"（中点或加权），代表物体整体运动
相对变量 / Relative variable	右末端相对左末端的运动，刚体把持时应为零，是内力的速度共轭
对称表述 / Symmetric formulation	Uchiyama-Dauchez 非主从框架：用绝对/相对变量对等描述双臂
相对雅可比 / Relative Jacobian	\(J_{\text{rel}}\)：关节速度 → 相对 twist 的映射，\(=\bar J_R-\bar J_L\)（统一参考点后）
绝对雅可比 / Absolute Jacobian	\(J_{\text{abs}}\)：关节速度 → 物体（绝对）twist 的映射
grasp matrix	\(G\)：接触 wrench → 物体净 wrench；其转置 \(G^\top\) 把物体速度 → 接触速度
内力 / Internal force	落在 \(\ker(G)\) 的接触力组合，不动物体、纯内部"较劲"，提供抓取稳定性
运动力 / Motion force	驱动物体运动的接触力分量，\(=G^{+}F_{\text{load}}\)
内/外阻抗 / Internal-external impedance	Caccavale 双层：外环 object impedance 管绝对，内环 internal impedance 管内力
主从控制 / Master-slave	一臂主导、另一臂跟随；集中式，内力是从臂跟踪误差的副产品（失控）
对称协调 / Coordinated (non-master-slave)	两臂对等协商实现物体运动 + 显式内力环；分布式
leader-follower 一致性	leader 持意图、follower 经共识协商收敛；去中心化，主从的"软化版"
扩展相对雅可比 / Extended relative Jacobian	非对称任务：同时控制相对运动 + 指定参考手的绝对运动
非对称参数 \(\alpha\)	绝对变量"以谁为代表"的连续旋钮：0.5 对称、1/0 非对称（参考手）
负载分配 / Load distribution	\(N\) 臂如何分担物体 wrench，\(N\ge3\) 时为带约束 QP
摩擦锥 / Friction cone	接触力不打滑的约束 \(\|f^t\|\le\mu f^n\)，二阶锥，常金字塔线性化

知识点总表

编号	知识点	核心要点	对应节	难度
K1	多机视角看双臂	\(N=2\) 特例；翻译表；耦合强度谱（通信→碰撞→力→力+运动）	§7.1	⭐⭐
K2	对称表述	绝对=中点、相对=右减左；二体问题质心-相对坐标类比	§7.2	⭐⭐⭐
K3	相对/绝对雅可比	统一参考点 \(X(p_c-p_i)\) 后 \(J_{\text{rel}}=\bar J_R-\bar J_L\)、\(J_{\text{abs}}=\tfrac12(\bar J_L+\bar J_R)\)	§7.2	⭐⭐⭐
K4	协调逆运动学	绝对通道跟踪物体 + 相对通道锁零；冗余做自任务	§7.2	⭐⭐⭐
K5	\(\alpha\) 统一对称/非对称	加权绝对变量；任务对称性的连续参数	§7.2、§7.6	⭐⭐⭐
K6	运动学-静力学对偶	虚功原理 → \(V_{\text{contacts}}=G^\top V_{\text{obj}}\)；\(G\) 力、\(G^\top\) 速度	§7.3	⭐⭐⭐
K7	\(\ker(G)\)/\(\ker(J_{\text{rel}})\) 对偶	内力（力零空间）↔ 相对运动（破坏约束的速度），同维同构	§7.3	⭐⭐⭐
K8	内力必须闭环	\(\ker(G)\) 随构型旋转，开环力泄漏到运动通道	§7.3、§7.4	⭐⭐⭐
K9	双臂内力分解	\(f_{\text{int}}=\tfrac12(h_R-h_L)\)；\(h_{L/R}=\tfrac12F\mp f_{\text{int}}\)	§7.4	⭐⭐⭐
K10	Caccavale 内/外阻抗	外环 object（绝对）+ 内环 internal（相对）；正交解耦	§7.4	⭐⭐⭐
K11	\(N\) 臂内力推广	\(h=G^{+}F+W\lambda\)；多臂内力依赖运动学（Erhart-Hirche）	§7.4	⭐⭐⭐⭐
K12	主从 vs 协调	对应集中/分布；主从内力失控；协调内力显式	§7.5	⭐⭐⭐
K13	leader-follower 一致性	第 2 章共识 + leader 牵引项；去中心化软化主从	§7.5	⭐⭐⭐
K14	对称/非对称协调	任务维度 ⊥ 架构维度；扩展相对雅可比；非对称内力常单向	§7.6	⭐⭐⭐
K15	多臂 QP 力分配	\(\min h^\top Qh\) s.t. \(Gh=F\)、摩擦锥、力矩上限	§7.7	⭐⭐⭐⭐
K16	ADMM 分布式分配	拆成各 agent 小 QP + \(\lambda\)（物体 wrench 价格）协调	§7.7	⭐⭐⭐⭐
K17	两套双臂内容分工	单系统（工程深度）vs 多机（结构推广），互补	§7.8	⭐⭐
K18	经典 vs 学习范式	学意图 + 经典守内力；力控是学习短板	§7.8	⭐⭐

---

累积项目：本章新增模块——coop_manipulation/

项目背景：本课程的累积项目是 Mini-MultiBot（第 13 章综合实战）。每章为它增加一个可复用模块。本章贡献"协同操作"模块，它既能独立运行（双臂/三臂协同），又能被第 8-9 章（多足/人形+臂）和第 13 章复用。

本章新增模块结构：

mini_multibot/
└── coop_manipulation/                ← 本章新增
    ├── kinematics/
    │   ├── coop_jacobian.py           # J_abs, J_rel, J_coop（对称表述，统一参考点）
    │   ├── extended_rel_jacobian.py   # 非对称任务的扩展相对雅可比（α 可调）
    │   └── grasp_matrix.py            # G, G^+, ker(G) 基 W（复用第 5 章并扩展到 6D wrench）
    ├── control/
    │   ├── coop_impedance.py          # Caccavale 内/外阻抗双层（双臂）
    │   ├── internal_force_ctrl.py     # 内力闭环（相对位姿偏差 → 内力）
    │   └── arch/
    │       ├── master_slave.py        # 主从（基线，对照用）
    │       ├── symmetric_coord.py     # 对称协调
    │       └── leader_follower.py     # leader-follower 一致性（调用第 2 章 consensus）
    ├── allocation/
    │   ├── load_dist_qp.py            # N 臂集中式 QP 力分配（OSQP）
    │   └── load_dist_admm.py          # 分布式 ADMM（调用第 4 章 admm + 第 2 章 consensus）
    └── tests/
        ├── test_duality.py           # 验证 ker(J_rel)/ker(G) 对偶（§7.3）
        ├── test_internal_force.py    # 验证内力闭环 vs 开环漂移
        └── test_n_arm_alloc.py       # 验证 QP/ADMM 一致 + 容错（掉线）

与前序模块的复用关系：

复用的前章模块	在本章模块中的角色
第 2 章 consensus/（共识）	leader_follower.py 的一致性更新、load_dist_admm.py 的 \(\lambda\) 全局残差估计
第 4 章 dmpc/admm	load_dist_admm.py 的分布式 QP 求解骨架
第 5 章 transport/grasp_matrix	grasp_matrix.py 的基础（本章扩展为 6D wrench + \(\ker(G)\) 参数化）
机械臂 D01 dual_arm/jacobian（如已实现）	coop_jacobian.py 可直接复用 \(J_{\text{aug}}\) 计算

本章里程碑（学完应能跑通）：

1. 双 Franka 共持杆，对称协调阻抗跟踪物体圆弧轨迹 + 维持 20N 内力，内力波动 < 10%（§7.4 练习 2）。
2. test_duality.py 通过：数值验证物体纯转动的 \(\dot q\) 满足 \(J_{\text{rel}}\dot q\approx0\) 且末端速度落在 \(\text{Im}(G^\top)\)（§7.3 练习 3）。
3. 三臂抬偏置板，QP 力分配 vs 平分，弱臂力矩峰值显著下降（§7.4 练习 3 + §7.7 练习 1）。
4. ADMM 分布式分配收敛到集中 QP 解，且"臂2 掉线"后其余两臂重新协商出可行解（§7.7 练习 3）。

---

延伸阅读

按"先读哪个、难度、为什么读"组织。⭐ 数量为阅读难度。

经典控制线（本章主线的源头）

• ⭐⭐⭐ Uchiyama & Dauchez, "Symmetric Kinematic Formulation and Non-Master/Slave Coordinated Control of Two-Arm Robots" (Advanced Robotics, 1993)。本章 §7.2/§7.5/§7.6 的理论源头。读它理解"为什么对称表述优于主从""绝对/相对变量的原始动机"。配套早期会议版 ICRA 1987 给出 hybrid position/force 协调方案。
• ⭐⭐⭐ Chiacchio, Chiaverini, Sciavicco, Siciliano, "Task space dynamic analysis of multiarm system configurations" (IJRR, 1996)。绝对/相对变量的现代形式（右减左约定、增广雅可比），后续工程普遍采用。机械臂 D01 的符号约定即源于此。
• ⭐⭐⭐ Caccavale, Chiacchio, Marino, Villani, "Six-DOF Impedance Control of Dual-Arm Cooperative Manipulators" (IEEE/ASME T-Mechatronics, 2008)。§7.4 的主线。内/外阻抗双层的严格 6-DOF 刚度定义。做协同力控必读。
• ⭐⭐⭐ Jamisola & Roberts, "A more compact expression of relative Jacobian based on individual manipulator Jacobians" (Robotics & Autonomous Systems, 2015)。§7.2 紧凑相对雅可比。工程上直接用各臂标准雅可比拼 \(J_{\text{rel}}\)，代码复用友好。
• ⭐⭐⭐⭐ Erhart & Hirche, "Internal Force Analysis and Load Distribution for Cooperative Multi-Robot Manipulation" (IEEE T-RO, 2017)。§7.4/§7.7 多臂内力的严格基础。指出 grasp 伪逆的内力依赖末端运动学、给出无内力广义逆参数化。\(N\ge3\) 必读。

多机/分布式线（连接第 1-5 章）

• ⭐⭐⭐ Khatib, "Object Manipulation with Whole-Body Control" / augmented object（IJRR 1988 及后续）。把负载作为虚拟物体插入操作空间动力学，统一多接触点。第 5 章已引，本章作运动/力解耦的动力学基础。
• ⭐⭐⭐⭐ Verginis & Dimarogonas, "Cooperative Manipulation via Internal Force Regulation: A Rigidity Theory Perspective" (2019)。§7.4/§7.7 分布式内力调节。grasp 矩阵与刚性矩阵的零空间-值空间关系；无需负载惯性。与第 5 章 Verginis 2019 同脉络。
• ⭐⭐⭐ Almeida, Lima 等, "Asymmetric Dual-Arm Task Execution using an Extended Relative Jacobian" (2019, arXiv:1905.01248)。§7.6 扩展相对雅可比。非对称任务同时控制相对运动 + 一手绝对运动。
• ⭐⭐⭐ "Quadratic-Programming-based Control of Multi-Robot Systems for Cooperative Object Transport"（arXiv 2024-2025）与 "Distributed Motion Control of Multiple Mobile Manipulators for Reducing Interaction Wrench"（arXiv 2406.05613）。**§7.7 现代 QP/分布式力分配**的工程实例，leader-follower 减少交互 wrench。

学习范式线（§7.8，深入见机械臂 D04）

• ⭐⭐ Zhao, Finn 等, "Learning Fine-Grained Bimanual Manipulation with Low-Cost Hardware" (ALOHA) (RSS, 2022) 与 Mobile ALOHA (CoRL, 2024)。双臂遥操作 + ACT 学习的奠基与移动扩展。
• ⭐⭐ Chi 等, "Diffusion Policy" (RSS, 2023)。DDPM 生成双臂动作，多模态、精度高。
• ⭐⭐ "Bimanual Robot Manipulation: Technical Survey and Applications" (2025)。双臂学习的最新综述，含 leader-follower 动作分解、VLA。

教材

• ⭐⭐⭐ Siciliano & Khatib (eds.), Springer Handbook of Robotics，"Cooperative Manipulation" 章（Caccavale & Uchiyama 执笔）。本章经典控制内容的权威综述与符号标准。
• ⭐⭐⭐ Siciliano, Sciavicco, Villani, Oriolo, Robotics: Modelling, Planning and Control。雅可比、伪逆、阻抗控制的基础（前置）。

---

本章与后续章节的关系

本章是 Part 3（协同操作）的开篇，为后续两章提供"闭链协同操作"的数学母板，并向 Part 4（MARL）输送概念。

后续章节	关系	本章铺垫的知识点
第 8 章 多足协同 loco-manipulation	直接复用——把"臂"换成"腿"，每条腿是"接触点 + 运动学"，闭链约束、grasp matrix、内力分配原样搬过去，只是雅可比里多了浮动基座、约束里多了接触维持	K3 相对/绝对雅可比、K6-K8 对偶与内力闭环、K15-K16 力分配 QP/ADMM
第 9 章 人形/四足+臂层次化全身协同	扩展——双臂装到浮动基座（人形）上，绝对/相对分解 + WBC 分层；多肢体（双臂+双腿）协同是 \(N\) 接触点的统一闭链	K2 对称表述、K10 内/外阻抗（升级为全身）、K11 \(N\) 接触点内力
第 10-11 章 MARL	对照与融合——本章经典控制可作为 MARL 的安全外壳（学意图 + 经典守内力，§7.8）；leader-follower 在 MARL 里变成 CTDE 的角色结构	K12-K13 主从/一致性架构、K18 经典 vs 学习互补
第 12 章 MARL+传统规控混合	核心呼应——§7.8 "经典内力环 + 学习运动策略"正是本章对该主题的具体贡献	K8 内力闭环作安全约束、K18 混合范式
第 13 章 Mini-MultiBot 综合实战	模块复用——coop_manipulation/ 模块直接接入综合系统	全章累积项目模块

前向预告（给读第 8 章的你）：第 8 章会发现，多足 loco-manipulation 的"足端力分配"和本章 §7.7 的"多臂负载分配"是同一个 QP——只是接触点是脚而非手，且多了"摆动腿不出力"的接触序列约束。现在只需记住：本章的闭链数学不是双臂专属，而是"任意一组末端共持负载"的通用语言——这正是把它放在"多机协作"而非"机械臂"方向的根本理由。

---

🔧 故障排查手册

协同操作调试时最常见的 5 类故障，按"症状 → 可能原因 → 排查步骤 → 相关节"组织。

故障 1：物体被无故挤压 / 两臂"对抗"（内力失控）

项	内容
症状	物体明明在做合法运动（如绕质心转），却被两臂越夹越紧或反复拉扯；力传感器读数远超期望内力
可能原因	(a) 相对雅可比用了 LOCAL_WORLD_ALIGNED 直接 \(J_R-J_L\)，没统一参考点；(b) 内力开环施加（没闭环相对位姿）；(c) 两臂控制器时钟不同步
排查步骤	1. 构造物体纯转动的 \(\dot q\)，测 \(\|J_{\text{rel}}\dot q\|\)——若显著非零，是参考点没统一（§7.2 陷阱）。2. 检查内力是否用 internal impedance 闭环（§7.4），而非开环下发。3. 检查两臂 \(x_{\text{abs}}^{\text{des}}(t)\) 是否同一时钟（§7.5 陷阱）
相关节	§7.2、§7.3（内力必须闭环）、§7.4、§7.5

故障 2：物体能搬动但轨迹漂移 / 内力随姿态缓慢增长

项	内容
症状	短时正常，长时间运行后物体偏离目标轨迹，内力单调漂移
可能原因	(a) 内力开环 → \(\ker(G)\) 随物体姿态旋转，开环力泄漏到运动通道（§7.3 反事实推理）；(b) 内外环 wrench 在不同参考点叠加；(c) 缺重力/惯性补偿，误差累积
排查步骤	1. 把内力改成相对位姿偏差闭环，观察漂移是否消失。2. 确认 \(J_{\text{abs}},J_{\text{rel}}\) 和内外环 wrench 都在同一参考点（物体质心）。3. 加重力补偿前馈
相关节	§7.3、§7.4（陷阱：参考点一致）

故障 3：主臂经过某构型时内力剧烈震荡（主从架构）

项	内容
症状	主从控制下，主臂经过特定区域时内力突然震荡、物体抖动甚至脱手
可能原因	主臂接近奇异 → 主臂实际运动滞后/抖动 → 从臂忠实复现抖动 → 相对位姿剧烈波动 → 内力震荡（§7.5 反事实推理）
排查步骤	1. 监测主臂操作度 \(w=\sqrt{\det(J_LJ_L^\top)}\)，确认震荡是否发生在 \(w\) 低谷。2. 若是，换对称协调架构（内力由内环独立守护，不被主臂奇异绑架）或 leader-follower 一致性（滤波平滑）
相关节	§7.5（主从结构性缺陷）

故障 4：\(N\ge3\) 臂搬运时某臂率先力矩饱和、连锁过载

项	内容
症状	多臂抬大件，某只手（常是离质心近或电机弱的）先到力矩上限，份额甩给邻居，引发连锁过载，物体倾斜
可能原因	(a) 用了朴素"平分"而非优化分配；(b) QP 代价没编码能力差异（缺加权/min-max）；(c) 没设最小法向力，某接触力被压到接近零打滑
排查步骤	1. 改用 §7.7 的 QP 分配（加权范数或 min-max 归一化力）。2. 在代价里按 \(1/\tau_{\max,i}\) 加权，让弱臂少担。3. 加 \(f_{\min}\) 约束防止打滑（§7.7 反事实推理）
相关节	§7.1（反面教材）、§7.7

故障 5：摩擦锥约束设了但接触仍打滑

项	内容
症状	QP 里写了摩擦锥约束，求解"成功"，但实物接触仍打滑
可能原因	(a) 二阶锥约束直接喂给只支持线性约束的 QP 求解器（OSQP），被静默错误处理；(b) 摩擦系数 \(\mu\) 估计过乐观；(c) 接触法向建模错误
排查步骤	1. 确认摩擦锥用金字塔线性化（4-8 条线性不等式）后再喂 OSQP，或换 SOCP 求解器（ECOS/Mosek）。2. 保守取 \(\mu\)（留安全系数）。3. 核对每个接触的法向方向是否与真实接触面一致（§7.7 陷阱）
相关节	§7.7（摩擦锥线性化）

---

API 速查表

本章涉及的核心计算接口（Pinocchio / NumPy / OSQP），按用途分组。

协同运动学（Pinocchio）

API / 函数	签名（简）	说明
pin.getFrameJacobian	(model, data, frame_id, ref_frame) -> 6×nv	取末端雅可比；本章用 LOCAL_WORLD_ALIGNED（仅对齐轴向，参考点仍在末端）
twist_translate(d)	(d) -> 6×6	自定义 twist 平移算子 \(X(d)=\begin{bmatrix}I&-[d]_\times\\0&I\end{bmatrix}\)，统一参考点（本章 §7.2 必备）
J_abs = 0.5*(Jbar_L+Jbar_R)	—	绝对雅可比（统一参考点后）
J_rel = Jbar_R - Jbar_L	—	相对雅可比（统一参考点后，右减左）
np.linalg.pinv(J, rcond)	(J, rcond=1e-6) -> n×6	Moore-Penrose 伪逆（严格零空间投影用）；奇异鲁棒用阻尼伪逆

grasp matrix 与内力（NumPy）

API / 函数	签名（简）	说明
build_grasp_matrix	(contact_pts, com) -> 6×6N	\(G\)，每手 6D wrench，质心参考点；\(G_i=\begin{bmatrix}I&0\\ [r_i]_\times&I\end{bmatrix}\)
G_pinv = G.T @ inv(G@G.T)	—	grasp 伪逆（运动力分配 \(G^+F\)）
W = null_space(G)	scipy.linalg.null_space(G) -> 6N×(6N-6)	\(\ker(G)\) 的正交基（内力参数化 \(W\lambda\)）
f_int = 0.5*(h_R - h_L)	—	双臂内力（\(N=2\)）

力分配 QP（OSQP）

API / 函数	签名（简）	说明
osqp.OSQP().setup	(P, q, A, l, u)	标准 QP；\(P=Q\) 代价、\(A\) 含等式 \(Gh=F\) + 不等式（摩擦锥线性化 + 力矩上限）
solve_load_distribution	(contact_pts, com, F_cmd, tau_max, mu, f_min, w) -> N×6	本章封装的 \(N\) 臂 QP 分配
摩擦锥金字塔	|f_x|<=μf_z, |f_y|<=μf_z	4 条线性不等式近似二阶锥（边数越多越精确）

控制律（本章自定义类）

类 / 方法	说明
DualArmCoordImpedance.coop_jacobians(q)	返回 \(J_{\text{abs}},J_{\text{rel}}\)（统一参考点）
DualArmCoordImpedance.control(...)	Caccavale 内/外阻抗：\(\tau=J_{\text{abs}}^\top F_{\text{abs}}+J_{\text{rel}}^\top F_{\text{rel}}+\tau_{\text{null}}\)
leader_follower.py（项目模块）	leader-follower 一致性，调用第 2 章 consensus
load_dist_admm.py（项目模块）	分布式 ADMM 力分配，调用第 4 章 admm + 第 2 章 consensus

---

研究实践建议

按"入门 → 进阶 → 研究"三层给出建议，帮你把本章知识转化为研究/工程能力。

入门（建立直觉，1-2 周）：

• 先在 Python/MuJoCo 里把"两个独立控制器各控一只手"跑起来，亲眼看到内力失控（物体被挤压）——这个反面教材比任何公式都让你记住"为什么要协调"。
• 实现对称表述的 \(J_{\text{abs}},J_{\text{rel}}\)，用本章的"物体纯转动 \(\dot q\) 验证 \(\|J_{\text{rel}}\dot q\|\approx0\)"做 sanity check。统一参考点是最容易错也最关键的一步，务必验证。
• 把第 5 章的 grasp matrix 代码捡起来，亲手验证 \(\ker(G)\) 维度和内力不动物体——这是 §7.3 对偶的实验基础。

进阶（工程落地，2-4 周）：

• 实现 Caccavale 内/外阻抗双层（§7.4），在共持弹性梁上调内力到目标区间。重点体会"内力靠相对位姿偏差闭环产生"，而非开环下发。
• 实现三臂 QP 力分配（§7.7），制造一个"质心偏置 + 异构能力"的场景，对比平分 vs QP，量化弱臂力矩峰值下降——这是"\(N\ge3\) 必须优化"的最有力证据。
• 把主从、对称协调、leader-follower 三种架构都实现一遍（项目 arch/ 目录），在"主臂过奇异"场景对比内力峰值——亲手感受架构选择的后果。

研究（前沿探索，长期）：

• 分布式内力调节：沿 Verginis-Dimarogonas 的刚性理论，做"无需负载惯性/刚度"的分布式内力调节，与本章 §7.7 ADMM 结合。开放问题：通信延迟下的内力稳定性。
• 多臂内力的严格定义：深读 Erhart-Hirche，研究 \(N\ge3\) 时"无内力广义逆"的运动学依赖，这是从双臂直觉外推到多臂最易踩坑、也最有理论空间的地方。
• 经典 + 学习融合：把本章的显式内力环作为学习策略的"安全外壳"（§7.8），让学习给运动意图、经典守内力安全——这是对内力敏感的双臂操作落地的关键，也是第 12 章混合范式的前沿。开放问题：力 demo 数据稀缺下，如何让学习策略"知道"内力约束。
• 零通信协同（极限去中心化）：参考第 5 章 decPLM，研究多臂/多足通过接触力隐式协调（不显式通信）搬运——这是 §7.5 "通过接触力隐式协调"的激进版本，对通信受限场景意义重大。

给博士生的一句话：双臂/多臂协同操作是一个"老问题"（40 年历史），但它恰好坐落在三个热点的交汇处——多机协作（分布式、可扩展）、力控（内力、接触）、具身智能（学习范式）。在这个交汇处，"用多机视角重审协同操作"（本章立场）能让你看到别人看不到的连接（如内力分配 = 多机资源分配、leader-follower = 一致性）。最有价值的研究往往不是发明全新方法，而是在两个成熟领域的交界处建立别人没注意到的桥——本章本身就是这样一座桥的示范。

---

版本信息速查

项目	版本 / 说明
文档版本	v1.0（2026-06）
文档类型	算法工程教学（多机协作视角）
章节定位	第 7 章，Part 3（协同操作）开篇
配套代码	mini_multibot/coop_manipulation/（累积项目模块）
Pinocchio	≥ 3.0（computeFrameJacobian、log6、SE3 API）
OSQP	≥ 0.6（力分配 QP）；SOCP 备选 ECOS/Mosek
仿真	MuJoCo ≥ 3.0 / Isaac（双臂、三臂、多移动机械臂场景）
关键前置文档	第 1 章（架构/图论）、第 2 章（共识）、第 4 章（ADMM/分布式 MPC）、第 5 章（grasp matrix）、05_运动控制/20_机械臂 D01/F01
互补文档	05_运动控制/20_机械臂 D01-D04（单系统双臂工程深度）
下游文档	第 8 章（多足 loco-manip）、第 9 章（人形+臂）、第 12-13 章

本章一句话：把双臂当成"\(N=2\) 的多机协同操作"，你就拥有了一把能打开协同搬运（第 5 章）、共识（第 2 章）、分布式优化（第 4 章）、多足/人形协同（第 8-9 章）的万能钥匙——因为它们共享同一套闭链数学，而双臂是这套数学最小、最锋利、最适合用来理解全部矛盾的实例。