第5章 人手与灵巧操作：从生物学到机器人
原课程: Robots That Learn, UC Berkeley CS294-291, Spring 2026 本章基于 Lecture 4A: Human Hand and Dexterous Manipulation + Robotic Hands 精读论文: C. Piazza, G. Grioli, M.G. Catalano, and A. Bicchi, A Century of Robotic Hands, Annual Review of Control, Robotics, and Autonomous Systems, 2019 补充参考: R.M. Murray, Z. Li, S.S. Sastry, A Mathematical Introduction to Robotic Manipulation (CRC, 1994), Ch 4-5; A. Bicchi & V. Kumar, “Robotic Grasping and Contact: A Review” (ICRA, 2000)
一、本章概要
1.1 核心问题与动机
人手是自然界最精密的末端执行器：21+ 自由度 (DOF)、17,000 个触觉感受器、30 余块肌肉协同驱动，能够在< 100 ms 内完成从”拿起一枚鸡蛋”到”握住一把锤子”的模式切换。这一能力——灵巧操作 (dexterous manipulation)——正是机器人与环境交互的终极目标。
本章回答一个贯穿全书的核心问题：如何用数学语言精确描述”抓取”这一动作？
从第 3 章继承的运动学雅可比矩阵 \(J(\theta)\) 将在此被”堆叠”为多指手的抓取矩阵 (grasp matrix)，成为连接手指运动与物体运动的桥梁。而力封闭 (force closure) 和形封闭 (form closure) 这两个核心概念，则为”什么是好的抓取”提供了严格的数学定义。
与此同时，本章通过精读 Piazza et al. (2019) 的综述论文 A Century of Robotic Hands，勾勒出人造手从 1912 年第一个假肢钩到 2018 年的软体自适应手之间 106 年的演化全景，揭示两条核心设计趋势——软体化 (soft robotics) 与架构简化 (architecture simplification)——及其背后的工程哲学。
1.2 本章结构
Ch03 机器人机构学 ──→ Ch05 人手与灵巧操作 ──→ Ch06 本体感觉与触觉感知
(单臂运动学) (多指手抓取) (触觉反馈)
本章分为六个部分：
- 第 2 节 建立抓取的数学基础——从接触运动学到抓取矩阵、力封闭与形封闭
- 第 3 节 精读 Piazza et al. (2019)，提炼机器人手设计的百年方法论
- 第 4 节 给出完整 Python 代码实现：抓取矩阵构造、力封闭判定、抓取质量评估
- 第 5 节 总结核心 Takeaway 与前沿方向
- 第 6 节 提供分层练习与思考题
二、核心概念与数学基础
2.1 人手解剖学：一个 21+ DOF 的柔性末端执行器
2.1.1 自由度分布
人手可以抽象为以下运动学链：
| 部位 | 关节 | 运动类型 | 单指 DOF | 拇指 DOF |
|---|---|---|---|---|
| 食指/中指/无名指/小指 | MCP (掌指关节) | 屈伸 + 内收外展 | 2 | — |
| PIP (近端指间关节) | 屈伸 | 1 | — | |
| DIP (远端指间关节) | 屈伸 | 1 | — | |
| 拇指 | CMC (腕掌关节) | 屈伸 + 内收外展 + 对掌 | — | 3 |
| MCP (掌指关节) | 屈伸 | — | 1 | |
| IP (指间关节) | 屈伸 | — | 1 | |
| 手腕 | 腕关节 | 屈伸 + 桡尺偏斜 | 2 | — |
| 前臂 | 桡尺关节 | 旋前/旋后 | 1 | — |
总 DOF 计算：4 指 \(\times\) 4 DOF + 拇指 5 DOF + 手腕 2 DOF + 前臂 1 DOF = 24 DOF (不含掌弓的柔性变形)。
关键启示：人手的高自由度被肌肉的协同激活 (synergistic activation) 高度约简——神经科学研究表明，日常抓取动作中仅需 2-3 个主成分即可解释 > 80% 的手部姿势变化 (Santello et al., 1998)。这一发现直接催生了机器人手设计中的 hand synchronized motion (手同步运动) 策略 (Piazza et al. 2019, Section 4.2)。
2.1.2 抓取分类学 (Grasp Taxonomy)
Cutkosky (1989) 将人手抓取分为两大类：
- 力抓取 (Power Grasp): 手指与手掌共同包裹物体，接触面积大，适用于需要大输出力的场景（如握锤子）
- 精确抓取 (Precision Grasp): 仅指尖接触物体，适用于精细操作（如捏针、扭钥匙）
这一分类在机器人抓取规划 (grasp planning) 中具有直接的工程指导意义：不同的抓取类型对应不同的接触模型和力封闭条件。
2.2 接触运动学：从手指到物体
2.2.1 接触模型分类
机器手指与物体的接触可以分为以下三类 (Salisbury, 1982; Murray et al., 1994):
| 模型 | 符号 | 可传递力 | 可传递力矩 | 约束数 | 实例 |
|---|---|---|---|---|---|
| 无摩擦点接触 (PwoF) | PC | 法向力 \(f_n \geq 0\) | 无 | 1 | 指尖轻触 |
| 有摩擦点接触 (PwF) | PCwF | 法向力 + 切向力 (摩擦锥内) | 无 | 3 | 橡胶指尖 |
| 软手指接触 (Soft Finger) | SF | 法向力 + 切向力 | 绕法向的力矩 | 4 | 人体指尖 |
摩擦锥 (Friction Cone): 对于有摩擦点接触，接触力 \(f_i = [f_{i,x}, f_{i,y}, f_{i,n}]^T\) 必须满足库仑摩擦约束：
\[\sqrt{f_{i,x}^2 + f_{i,y}^2} \leq \mu_i f_{i,n}, \quad f_{i,n} \geq 0\]其中 \(\mu_i\) 为静摩擦系数。在线性化近似中，用 \(k\)-面棱锥 (通常 \(k=4,6,8\)) 逼近圆锥，将非线性约束转化为线性不等式：
\[F_i = \{f_i \mid f_i = \sum_{j=1}^{k} \alpha_{i,j} \mathbf{v}_{i,j}, \; \alpha_{i,j} \geq 0\}\]其中 \(\mathbf{v}_{i,j}\) 是摩擦锥的第 \(j\) 条棱边向量。
2.2.2 抓取映射 (Grasp Map)
设第 \(i\) 个手指与物体的接触点在物体坐标系 \(\{o\}\) 中的位置为 \(c_i \in \mathbb{R}^3\) (相对于物体质心)，手指施加的接触力为 \(f_i \in \mathbb{R}^p\) ( \(p \in \{1,3,4\}\) 取决于接触模型)。该力对物体产生的力旋量 (wrench) 为：
\[w_i = G_i f_i\]其中 \(G_i \in \mathbb{R}^{6 \times p}\) 称为第 \(i\) 个接触点的局部抓取矩阵 (partial grasp matrix)：
\[G_i = \begin{bmatrix} I_{3 \times 3} \\ \hat{c}_i \end{bmatrix} \quad \text{或针对完整形式} \quad G_i = \begin{bmatrix} I_{p \times p} \\ \hat{c}_i \end{bmatrix}\]其中 \(\hat{c}_i\) 是 \(c_i\) 的反对称矩阵，满足 \(\hat{c}_i f_i = c_i \times f_i\) (力 \(\times\) 力臂 = 力矩)。
2.2.3 抓取矩阵 (Grasp Matrix)
将所有 \(m\) 个接触的力旋量叠加，得到物体的总力旋量：
\[\boxed{w = \sum_{i=1}^{m} G_i f_i = G f}\]其中 抓取矩阵 \(G \in \mathbb{R}^{6 \times \sum p_i}\) 将所有接触力映射到物体力旋量：
\[G = [G_1 \; G_2 \; \cdots \; G_m]\] \[f = [f_1^T \; f_2^T \; \cdots \; f_m^T]^T\]物理含义: \(G\) 描述了”每个接触以何种几何方式影响物体的受力”。\(G\) 的列空间 \(\text{col}(G)\) 是所有可实现力旋量 (achievable wrenches) 的空间，而 \(\text{null}(G^T)\) 对应于内力 (internal forces)——手指可以相互挤压而不改变物体净受力。
2.2.4 手雅可比 (Hand Jacobian)
第 3 章建立的单臂雅可比 \(J(\theta)\) 在此被推广为手雅可比 \(J_h \in \mathbb{R}^{3m \times n}\) (假设 \(m\) 指，\(n\) 个关节，\(p=3\) 接触模型)，将关节速度映射到所有指尖速度的堆叠：
\[\dot{x}_{\text{tip}} = \begin{bmatrix} \dot{x}_1 \\ \dot{x}_2 \\ \vdots \\ \dot{x}_m \end{bmatrix} = J_h(\theta_q) \dot{\theta}_q\]其中 \(\theta_q \in \mathbb{R}^n\) 是所有手指关节角度的联合向量。每个手指的运动学是第 3 章 PoE 前向运动学的直接应用。
力域对偶：根据虚功原理，指尖力 \(f\) 与关节力矩 \(\tau\) 的关系为：
\[\tau = J_h^T f\]这与单臂的 \(\tau = J^T \mathcal{F}\) 完全同构——多指手仅仅是高维版本。
2.3 抓取质量：力封闭、形封闭与抓取度量
2.3.1 力封闭 (Force Closure)
定义：一个抓取是力封闭的，当且仅当对任意外旋量 \(w_{\text{ext}} \in \mathbb{R}^6\)，存在一组满足摩擦锥约束的接触力 \(f_i\)，使得：
\[-G f = w_{\text{ext}}\]直观意义：无论外界对物体施加什么样的力和力矩，手指都能通过调整接触力来抵消它们——物体被”锁定”在手中。
数学判定条件:
- \(\text{rank}(G) = 6\) (可生成所有 6 维力旋量)
- 存在 \(f > 0\) (所有接触力在摩擦锥内部) 使得 \(G f = 0\) (即存在严格正内力的零空间向量)
等价地，若将摩擦锥线性化为 \(\{f_i = B_i \alpha_i \mid \alpha_i \geq 0\} \subset \mathbb{R}^p\) (其中 \(B_i \in \mathbb{R}^{p \times k}\))，则力封闭等价于：
\[\text{cone}(G \tilde{B}) = \mathbb{R}^6\]其中 \(\tilde{B} = \text{diag}(B_1, \ldots, B_m)\) 是块对角基矩阵，\(\text{cone}(\cdot)\) 表示由列向量张成的凸锥。
计算力封闭：可通过线性规划 (LP) 求解。构造 LP：
\[\begin{aligned} \max_{\alpha, d} \quad & d \\ \text{s.t.} \quad & G \tilde{B} \alpha = 0, \\ & \sum_j \alpha_{i,j} = d \quad \forall i = 1, \ldots, m, \\ & \alpha_{i,j} \geq 0 \quad \forall i, j \end{aligned}\]若最优值 \(d^* > 0\)，则存在严格正内力，力封闭成立。详见第 4 节代码实现。
2.3.2 形封闭 (Form Closure)
定义：一个抓取是形封闭的，当且仅当物体在任何方向上的微小运动都至少被一个手指的几何约束所阻止，而不依赖于摩擦力。
数学条件：对于无摩擦点接触 (PwoF)，\(m\) 个接触点满足形封闭的充要条件是：接触法向量锥的极锥 (polar cone) 仅为 \(\{0\}\)。即不存在非零运动旋量 \(\mathcal{V} \in \mathbb{R}^6\) 使得对所有接触点：
\[n_i^T (v + \omega \times c_i) \geq 0\]直观上，形封闭要求接触点的几何排列使物体”无处可去”——即使表面完全光滑。
力封闭 vs 形封闭:
- 形封闭 \(\Rightarrow\) 力封闭 (但反之不成立)
- 形封闭要求至少 7 个无摩擦点接触 (Reuleaux, 1875; Markenscoff et al., 1990)
- 力封闭在有摩擦时仅需 2 个软手指接触 (对 3D 物体)
- 工程实践中，我们追求力封闭——摩擦力的存在使这一目标更容易达成
2.3.3 抓取质量度量 (Grasp Quality Metrics)
判定”力封闭成立”只是最低门槛。”哪个力封闭抓取更好？”需要定量度量：
1. \(\epsilon\)-度量 (Ferrari & Canny, 1992)
在力旋量空间 \(\mathbb{R}^6\) 中，所有可实现的力旋量构成一个凸多面体 (polytope) \(\mathcal{P}\)。\(\epsilon\) 定义为 \(\mathcal{P}\) 中从原点沿各方向的最小距离——即 \(\mathcal{P}\) 内切球的半径：
\[\epsilon(G) = \min_{w \in \partial \mathcal{P}} \|w\|_2\]该度量的物理含义：最坏情况扰动 (worst-case disturbance) 下，手指能够抵抗的最大外力。\(\epsilon\) 越大，抓取越鲁棒。
2. 最小奇异值 \(\sigma_{\min}(G)\) (Li & Sastry, 1988)
抓取矩阵 \(G\) 的最小奇异值度量了抓取在各向同性上的”最薄弱方向”：
\[Q_{\text{msv}} = \sigma_{\min}(G)\]该度量计算简单，但忽略了摩擦锥约束——仅在接触力无界的假设下有效。
3. 体积度量 \(V(\mathcal{P})\)
可达到力旋量多面体的体积 \(V(\mathcal{P})\) 直接度量了抓取的”力域覆盖能力”。计算时通常使用凸包 (convex hull) 或 Quickhull 算法。
4. 任务导向度量
若已知预期外力的方向分布（如举起物体时的重力方向），可定义投影度量：
\[Q_{\text{task}} = \min_{w \in \mathcal{W}_{\text{task}}} \max \{\alpha \mid \alpha w \in \mathcal{P}\}\]其中 \(\mathcal{W}_{\text{task}}\) 是任务相关的力旋量集合。
2.4 从接触力到关节力矩：完整的因果链
将以上概念串联为完整的力传递链：
关节力矩 τ ──→ 指尖力 f ──→ 物体力旋量 w ──→ 物体加速度
↑ ↑ ↑
J_h^T G Newton-Euler
正向: 给定关节力矩 \(\tau\)，由 \(f = (J_h^T)^{\dagger} \tau + f_{\text{int}}\) (其中 \((J_h^T)^{\dagger}\) 是伪逆，\(f_{\text{int}} \in \text{null}(J_h^T)\) 是内力分量)，再经过抓取矩阵 \(G\) 得到物体所受合力旋量 \(w = G f\)，最终通过牛顿-欧拉方程 \(w = M_o(\theta_o) \ddot{\theta}_o + C_o(\theta_o, \dot{\theta}_o) \dot{\theta}_o\) 决定物体运动。
逆向 (抓取规划): 给定物体期望运动 \(\ddot{\theta}_o\)，逆向求解所需关节力矩 \(\tau\)。这里存在冗余: (1) 多指手提供的接触力维度 > 6 (内力空间); (2) 手指关节数 » 指尖自由度。这种冗余恰恰是灵巧操作的来源——如同人手可以改变握力 (内力) 而不改变物体运动。
三、论文精读：A Century of Robotic Hands (Piazza et al., 2019)
3.1 论文定位与贡献
Piazza et al. (2019) 发表於 Annual Review of Control, Robotics, and Autonomous Systems，系统梳理了 1912 年至 2018 年间 199 款人造手的设计演化。论文的核心论点可以凝练为一句话：
人造手设计在过去一个世纪经历了从”尽可能复制人手的每个关节”到”有原则地简化 (principled simplification)“的范式转移——软体材料与欠驱动架构取代了刚性精密机构，在鲁棒性、适应性和成本上取得了压倒性优势。
这一结论不仅是对历史的回顾，更预示了现代机器人手的设计哲学：不追求完美的人手复制品，而追求能在真实世界中可靠工作的具身末端执行器。
3.2 方法论：199 款手的数据库分析
Piazza et al. 构建了涵盖 1912-2018 共 106 年的综合性数据库 (Supplemental Table 1)，对每款手标注了以下维度：
| 维度 | 分类选项 | 说明 |
|---|---|---|
| 关节类型 | Rigid / Flexible / Dislocatable / Soft Continuous | 从刚性铰链到连续体柔性变形 |
| 驱动原理 | Rigid / Impedance Control / SEA / Variable Stiffness / Agonist-Antagonist / Variable Impedance | 从工业伺服到仿生拮抗对 |
| 传动架构 | Fully Actuated / Coupled / Underactuated | DOF = DOA / DOF > DOA 但耦合 / DOF > DOA 且被动适应 |
| 应用领域 | Prosthetics & Rehabilitation / Industrial / Human-Robot Interaction | 三大应用类 |
这种系统化的分类方法使得原本不可比较的异构设计变得可比较——这是本文对学术界最重要的方法论贡献。
3.3 两条核心趋势的证据
3.3.1 趋势一：软体化 (Softening)
论文的 Figure 8 展示了 1978-2018 年间关节类型和驱动原理的百分比变化：
- 2000 年前: 刚性关节占比 > 90%，刚性驱动占比接近 100%
- 2010 年后: 柔性连续体关节 (Soft Continuous) 快速增长，软驱动 (Soft Actuation) 占比在 2018 年达到 ~35%
论文将关节柔性分为四个层次 (Figure 4):
- Rigid: 传统销轴铰链
- Flexible: 集成弹性元件 (如弹簧)
- Dislocatable: 可严重脱位的弹性关节
- Soft Continuous: 连续体变形 (如硅胶)
驱动柔性的五个层次 (Figure 5, transformed):
\[\begin{array}{c\vert c} \text{驱动类型} & \text{核心机制} \\ \hline \text{Rigid Actuator} & \text{位置伺服，无柔顺} \\ \text{Active Impedance Control} & \text{软件模拟柔顺 (受带宽限制)} \\ \text{Series Elastic Actuator (SEA)} & \text{物理弹簧串联，}\tau = K(\theta_m - \theta) \\ \text{Variable Stiffness Actuator} & \text{可调物理刚度，第二电机调谐} \\ \text{Agonist-Antagonist VSA} & \text{拮抗对，仿生，非线性刚度} \\ \text{Variable Impedance Actuator} & \text{刚度和阻尼均可调} \end{array}\]关键洞察: 软体化并非为了”更像人手”，而是为了工程性能——柔顺手在抓取未知物体时通过被动变形自适应物体轮廓，大幅降低了对感知和控制精度的需求 (§4.1.1)。
3.3.2 趋势二：架构简化 (Architecture Simplification)
论文的 Figure 7(c) 展示了传动架构类型随时间的累积分布：
- 全驱动 (Fully Actuated): DOF = DOA，每个关节有独立电机。代表性作品：DLR Hand II (13 DOF, 13 motors)
- 耦合驱动 (Coupled): 多个关节由一个电机通过连杆/腱驱动，关节间运动比例固定
- 欠驱动 (Underactuated): DOF > DOA，关节间的运动分配由被动元件 (弹簧、差速器) 根据接触力自动决定。代表性作品：Yale Multigrasp Hand, Pisa/IIT SoftHand
论文特别重点讨论了 Hand Synchronized Motion (§4.2)——一种受神经科学中”姿态协同 (postural synergies)”启发的设计方法：
神经科学研究表明，人脑使用 特征抓取空间 (eigengrasp space) 的基向量来组织手部动作。前 2-3 个主成分 (PC1 = 开合, PC2 = 精确抓取配置) 即可解释 > 80% 的手姿方差。
将这一思想工程化：用一个或两个电机驱动多关节手指，通过机械耦合实现预定义的协同模式 (synergy pattern)。这就是所谓的 hand synchronized motion——”手同步运动”。
Pisa/IIT SoftHand (Catalano et al., 2014) 是这一思路的标志性实现：19 DOF 的拟人手仅由 1 个电机 驱动，通过差速机构和弹性腱传递动力，实现了自适应包络抓取 (enveloping grasp)。该手在 2016 年 Robotic Grasping and Manipulation Competition 中夺得 Hand-in-Hand 竞赛冠军。
3.4 应用领域的驱动因素
Piazza et al. 将 10 个应用领域归为三大类 (§2.11)：
| 大类 | 子领域 | 核心需求 |
|---|---|---|
| Prosthetics & Rehabilitation | 假肢、辅助机器人 | 重量轻、安全、控制简单、交互自然 |
| Industrial | 监督操作、自主操作、物流 | 鲁棒性、效率、适应不同物体 |
| Human-Robot Interaction | 遥操作、远程互动、社交机器人、娱乐、服务机器人 | 安全、外观自然、运动柔和 |
Figure 10 和 Figure 12 的交叉分析表明：不同领域对关节类型和传动架构的偏好存在显著分化——软连续体关节在 Human-Robot Interaction 中占主导，而欠驱动架构在所有领域中都快速增长。
3.5 范式的后果：从刚性操作到软体操作
论文 Section 5 深入探讨了软体化和简化带来的范式转变：
操作范式的根本变化 (Figure 13):
- 传统刚性操作: “胆怯的手 (timid hand)”——规划器小心翼翼地避开环境接触，仅允许指尖触碰物体的特定点。在 Figure 13b 的例子中，要从靠墙的杯子中抓起物体，手必须在窄缝中规划一条无碰撞路径。
- 软体操作: “大胆的手 (daring hand)”——环境接触不再是障碍，而是被主动利用来帮助塑形手掌。手-物-环境三者间的复杂交互构成了 enabling constraints——它们不再是需要规避的约束，而是被招纳为功能的一部分。
这一范式转变对抓取规划和控制的含义是深远的：经典的”规划-执行”模式需要转变为”交互-适应”模式。这意味着传统的基于模型的抓取规划（需要精确的物体姿态和几何）正在被数据驱动的学习方法所补充甚至取代——这为后续章节的强化学习和模仿学习奠定了应用场景。
3.6 论文的局限性与未回答问题
尽管该综述覆盖了 199 款手，但其分析存在几个值得注意的局限：
- 忽略了传感维度：论文明确排除对触觉/力传感器的分析 (§1)。但在实际系统中，传感和控制是不可分割的——软体手的优势恰在于”用机械智能替代感知/控制复杂度”。
- 缺少定量性能对比：各款手的性能（抓取成功率、负载能力、寿命）未纳入比较，这使得论文的”趋势”论证停留在定性层面。
- LEAP Hand 等最新进展未被纳入：论文截止于 2018 年，而 2020 年代的低成本灵巧手设计（如 LEAP Hand, Shaw et al. 2023）进一步推动了”廉价灵巧手”趋势。
- 抓取质量与手设计的脱节：论文未讨论抓取质量的形式化度量（如力封闭）如何反向指导手的设计——”握得住”和”设计好”之间的桥梁尚未架设。
四、算法与代码实现
以下代码基于 NumPy 实现，完整覆盖本章的核心数学结构。代码按”接触模型 → 抓取矩阵 → 力封闭判定 → 抓取质量 → 完整示例”的逻辑组织。
4.1 基础数学工具
import numpy as np
from scipy.spatial import ConvexHull
from scipy.linalg import null_space
from itertools import product
import warnings
def skew(v):
"""构造反对称矩阵 [v] ∈ so(3)
对应第 3 章公式: [v] = [[0, -v3, v2],
[v3, 0, -v1],
[-v2, v1, 0]]
用途: 叉积运算 v × w = [v] w, 以及抓取矩阵中力矩分量的构造
Args:
v: (3,) 向量
Returns:
(3,3) 反对称矩阵
"""
return np.array([
[ 0, -v[2], v[1]],
[ v[2], 0, -v[0]],
[-v[1], v[0], 0]
])
def adjoint(T):
"""计算 SE(3) 变换矩阵 T 的伴随表示 Ad_T ∈ R^{6×6}
对应第 3 章 2.3.2 节公式:
Ad_T = [[R, 0],
[[p]R, R]]
用途: 将接触力旋量在不同坐标系间变换。例如，将指尖力从接触点
局部坐标系变换到物体质心坐标系
Args:
T: (4,4) SE(3) 齐次变换矩阵
Returns:
(6,6) 伴随矩阵
"""
R = T[:3, :3]
p = T[:3, 3]
Ad = np.zeros((6, 6))
Ad[:3, :3] = R
Ad[3:, 3:] = R
Ad[3:, :3] = skew(p) @ R
return Ad
4.2 接触模型定义
class ContactModel:
"""抓取接触模型 (Murray et al. 1994, Ch 5)
三种接触类型:
- 'PwoF' (Point without Friction): 无摩擦点接触, p=1 (仅法向力)
- 'PwF' (Point with Friction): 有摩擦点接触, p=3 (3D 力向量)
- 'SoftFinger': 软手指接触, p=4 (3D 力 + 绕法向的力矩)
摩擦锥线性化: 使用 k 条棱边近似库仑摩擦锥
"""
def __init__(self, position, normal, mu=0.5, contact_type='PwF',
num_cone_edges=6):
"""
Args:
position: (3,) 接触点在物体质心坐标系中的位置
normal: (3,) 接触面外法向 (单位向量, 指向物体内部)
mu: 静摩擦系数
contact_type: 'PwoF' | 'PwF' | 'SoftFinger'
num_cone_edges: 摩擦锥线性化的棱边数 (默认 6)
"""
self.position = np.asarray(position, dtype=float)
self.normal = np.asarray(normal, dtype=float)
self.normal = self.normal / np.linalg.norm(self.normal)
self.mu = mu
self.contact_type = contact_type
self.num_cone_edges = num_cone_edges
def local_grasp_map(self):
"""构造局部抓取映射 G_i
将接触力 f_i 映射到物体力旋量 w_i = G_i f_i
对应 2.2.2 节公式:
G_i = [I_{p×p}; \hat{c}_i]
Returns:
G_i: (6, p) 局部抓取矩阵
B_i: (p, k) 摩擦锥基矩阵 (接触力 = B_i * α, α ≥ 0)
p: 接触力维度
"""
c_hat = skew(self.position)
if self.contact_type == 'PwoF':
# 无摩擦点接触: 仅法向力
# G_i: (6,1), 将法向力 f_n 映射到力旋量
p = 1
G_i = np.vstack([self.normal.reshape(3, 1),
(np.cross(self.position, self.normal)).reshape(3, 1)])
B_i = np.ones((1, 1)) # k=1: 法向力 = α_1 * 1
return G_i, B_i, p
elif self.contact_type == 'PwF':
# 有摩擦点接触: 3D 力向量
# G_i: (6,3), 力旋量 = [I; \hat{c}] f
p = 3
G_i = np.vstack([np.eye(3), c_hat])
B_i = self._friction_cone_basis()
return G_i, B_i, p
elif self.contact_type == 'SoftFinger':
# 软手指: 3D 力 + 绕法向力矩
# G_i: (6,4), 力旋量 = [I 0; \hat{c} n] (f, τ_n)
p = 4
G_i_top = np.eye(4) # 前 4 行对应力分量
# 力矩分量: f 产生 \hat{c}×f, τ_n 沿法向产生纯力矩
G_i_bottom = np.zeros((3, 4))
G_i_bottom[:, :3] = c_hat # 力 × 力臂
G_i_bottom[:, 3] = self.normal # 绕法向的力矩分量
G_i = np.vstack([G_i_top, G_i_bottom])
# B_i: 摩擦锥基 (前 3 行) + 独立的力矩通道 (第 4 行)
B_i_force = self._friction_cone_basis() # (3, k)
B_i = np.zeros((4, self.num_cone_edges + 2))
B_i[:3, :self.num_cone_edges] = B_i_force
B_i[3, self.num_cone_edges] = 1.0 # 正力矩
B_i[3, self.num_cone_edges + 1] = -1.0 # 负力矩 (力矩可正可负)
return G_i, B_i, p
def _friction_cone_basis(self):
"""生成摩擦锥的线性化基矩阵
在接触局部坐标系中:
z 轴 = 接触法向
锥角 = arctan(μ)
使用 k 条均匀分布的边向量逼近圆锥:
v_j = [μ cos(2πj/k), μ sin(2πj/k), 1]^T (在局部坐标系)
旋转到世界坐标系后返回 B_i
Returns:
B_i: (3, k) 摩擦锥基矩阵 (世界坐标)
"""
# 构建局部坐标系: z = n, x,y 由 Gram-Schmidt 确定
n = self.normal
# 选择任意不平行于 n 的向量
if abs(n[0]) < 0.9:
t1 = np.array([1.0, 0.0, 0.0])
else:
t1 = np.array([0.0, 1.0, 0.0])
t1 = t1 - np.dot(t1, n) * n
t1 = t1 / np.linalg.norm(t1)
t2 = np.cross(n, t1)
# 旋转矩阵: 局部坐标 → 世界坐标
R_local_to_world = np.column_stack([t1, t2, n])
# 在局部坐标系中生成 k 条棱边
k = self.num_cone_edges
basis_edges = []
for j in range(k):
angle = 2 * np.pi * j / k
# 局部坐标: 切向分量 (μ cos θ, μ sin θ) + 法向分量 (1)
v_local = np.array([self.mu * np.cos(angle),
self.mu * np.sin(angle),
1.0])
# 转换到世界坐标
v_world = R_local_to_world @ v_local
basis_edges.append(v_world)
return np.column_stack(basis_edges) # (3, k)
4.3 抓取矩阵与力封闭判定
class GraspAnalyzer:
"""抓取分析与质量评估
核心功能:
1. 构造完整抓取矩阵 G (2.2.3 节)
2. 力封闭判定 (2.3.1 节) — 线性规划方法
3. 抓取质量度量 (2.3.3 节) — ε-度量, 最小奇异值, 体积度量
"""
def __init__(self, contacts):
"""
Args:
contacts: List[ContactModel], 所有接触点的列表
"""
self.contacts = contacts
self.m = len(contacts)
# 构造完整抓取矩阵和各接触的摩擦锥基
self._build_grasp_matrix()
def _build_grasp_matrix(self):
"""构造完整抓取矩阵 G 和块对角基矩阵 \tilde{B}
对应 2.2.3 节:
G = [G_1 G_2 ... G_m] ∈ R^{6 × Σp_i}
\tilde{B} = diag(B_1, ..., B_m) ∈ R^{Σp_i × Σk_i}
\tilde{G} = G \tilde{B} ∈ R^{6 × Σk_i} (摩擦锥基下的等效抓取矩阵)
其中 \tilde{G} 的每一列对应一个"基本力旋量" (primitive wrench)
"""
G_blocks = []
B_blocks = []
p_list = []
for contact in self.contacts:
G_i, B_i, p_i = contact.local_grasp_map()
G_blocks.append(G_i)
B_blocks.append(B_i)
p_list.append(p_i)
self.G = np.hstack(G_blocks) # (6, Σp_i)
self.p_list = p_list
self.total_p = sum(p_list)
# 构造块对角基矩阵 \tilde{B}
self.B_tilde = np.zeros((self.total_p, sum(B.shape[1] for B in B_blocks)))
row_offset = 0
col_offset = 0
for B_i in B_blocks:
pi, ki = B_i.shape
self.B_tilde[row_offset:row_offset+pi, col_offset:col_offset+ki] = B_i
row_offset += pi
col_offset += ki
self.total_k = col_offset
# 基本力旋量矩阵 (primitive wrench matrix)
self.G_tilde = self.G @ self.B_tilde # (6, Σk_i)
def check_force_closure(self, verbose=False):
"""力封闭判定 — 线性规划方法
数学形式:
max d
s.t. G_tilde * α = 0
Σ_j α_{i,j} = d ∀i (每个接触的摩擦锥基系数和为 d)
α ≥ 0
若最优值 d* > 0, 则力封闭成立 (存在严格正内力)
解释: d* > 0 意味着对于每个接触 i, 存在一个非零的 α_i
使得 Σ α_{i,j} > 0 (所有接触的摩擦锥内部都有正力),
且合力旋量为零 (G_tilde α = 0), 即存在严格正内力。
Returns:
dict: {
'is_force_closure': bool,
'd_opt': float, 最优 d 值
'alpha': (Σk_i,) 最优 α
'internal_wrench_space_dim': int, 内力空间维数
}
"""
from scipy.optimize import linprog
# 设计变量的组织形式:
# x = [α; d] ∈ R^{total_k + 1}
# 等式约束: G_tilde * α = 0 (6 个方程)
# 每个接触的 α 和 = d (m 个方程, 因为 d 在最后)
A_eq = np.zeros((6 + self.m, self.total_k + 1))
A_eq[:6, :self.total_k] = self.G_tilde
# 对每个接触 i, 其 α 系数的和为 d
col_offset = 0
for i, contact in enumerate(self.contacts):
_, B_i, _ = contact.local_grasp_map()
ki = B_i.shape[1]
A_eq[6 + i, col_offset:col_offset + ki] = 1.0
A_eq[6 + i, -1] = -1.0 # -d 移到左边: Σα_i = d => Σα_i - d = 0
col_offset += ki
b_eq = np.zeros(6 + self.m)
# 目标: maximize d → minimize -d
c = np.zeros(self.total_k + 1)
c[-1] = -1.0
# 边界: α ≥ 0
bounds = [(0, None)] * self.total_k + [(None, None)]
try:
with warnings.catch_warnings():
warnings.simplefilter("ignore")
result = linprog(c, A_eq=A_eq, b_eq=b_eq, bounds=bounds,
method='highs')
if result.success:
d_opt = result.x[-1]
alpha = result.x[:-1]
is_fc = d_opt > 1e-8
if verbose:
print(f" 力封闭 LP 求解成功: d* = {d_opt:.4f}")
print(f" 力封闭判定: {'是' if is_fc else '否'}")
if is_fc:
# 验证 G_tilde * α ≈ 0
residual = np.linalg.norm(self.G_tilde @ alpha)
print(f" 内力残差 |Gα| = {residual:.2e}")
else:
is_fc = False
d_opt = 0.0
alpha = np.zeros(self.total_k)
if verbose:
print(f" 力封闭 LP 求解失败: {result.message}")
except Exception as e:
is_fc = False
d_opt = 0.0
alpha = np.zeros(self.total_k)
if verbose:
print(f" 力封闭 LP 异常: {e}")
# 计算内力空间维数 (null(G))
rank_G = np.linalg.matrix_rank(self.G)
internal_dim = self.total_p - rank_G
return {
'is_force_closure': is_fc,
'd_opt': d_opt,
'alpha': alpha,
'internal_wrench_space_dim': internal_dim
}
def check_form_closure(self, verbose=False):
"""形封闭判定 (仅适用于无摩擦点接触 PwoF)
方法: 检查所有接触法向 {n_i} 的正包 (positive span) 是否等于 R^6
对于 3D 物体 (6 维力旋量空间), 需要至少 7 个无摩擦点接触
(Mishra, Schwartz & Sharir 1987; Markenscoff, Ni & Papadimitriou 1990)
实际实现: 构造法向力旋量矩阵 N = [w_{n,1} ... w_{n,m}] (6×m)
其中 w_{n,i} = [n_i; c_i × n_i]
若 N 的列向量正包 = R^6, 则形封闭成立
判定: 检查是否存在 d > 0 使得 N d = 0 且 d_i > 0
这与力封闭的 LP 方法相同 (但不涉及摩擦锥)
Returns:
dict: {
'is_form_closure': bool,
'd_opt': float
}
"""
# 仅检查 PwoF 接触
pwof_indices = [i for i, c in enumerate(self.contacts)
if c.contact_type == 'PwoF']
if len(pwof_indices) < 7:
if verbose:
print(f" 形封闭需要至少 7 个 PwoF 接触, 当前仅 {len(pwof_indices)} 个")
return {'is_form_closure': False, 'd_opt': 0.0}
# 构造法向力旋量矩阵
N = np.zeros((6, len(pwof_indices)))
for j, i in enumerate(pwof_indices):
c = self.contacts[i]
N[:3, j] = c.normal
N[3:, j] = np.cross(c.position, c.normal)
# LP: max d s.t. N * α = 0, Σα_j = d, α ≥ 0
from scipy.optimize import linprog
m_pwof = len(pwof_indices)
A_eq = np.zeros((6 + 1, m_pwof + 1))
A_eq[:6, :m_pwof] = N
A_eq[6, :m_pwof] = 1.0
A_eq[6, -1] = -1.0
b_eq = np.zeros(7)
c = np.zeros(m_pwof + 1)
c[-1] = -1.0
bounds = [(0, None)] * m_pwof + [(None, None)]
try:
result = linprog(c, A_eq=A_eq, b_eq=b_eq, bounds=bounds, method='highs')
if result.success:
d_opt = result.x[-1]
return {'is_form_closure': d_opt > 1e-8, 'd_opt': d_opt}
except Exception:
pass
return {'is_form_closure': False, 'd_opt': 0.0}
def grasp_quality_epsilon(self, num_samples=10000):
"""计算 ε-度量 (Ferrari & Canny 1992)
方法: 在基本力旋量空间中, 随机采样接触力的凸组合,
计算可实现力旋量集合, 并求内切球半径
具体步骤:
1. 将每个接触的摩擦锥基系数归一化 (Σα_i = 1) 并随机采样
2. 计算对应的力旋量 w = G_tilde α
3. 求这些力旋量构成的凸包
4. ε = 原点到凸包面的最小距离
Args:
num_samples: 随机采样数量
Returns:
float: ε 值 (0 表示力封闭不成立)
"""
m = self.m
total_k = self.total_k
if total_k == 0:
return 0.0
# 随机采样: 为每个接触的摩擦锥基系数随机分配权重
wrench_samples = []
col_offset = 0
k_per_contact = []
for contact in self.contacts:
_, B_i, _ = contact.local_grasp_map()
k_per_contact.append(B_i.shape[1])
for _ in range(num_samples):
alpha = np.zeros(total_k)
col = 0
for i in range(m):
ki = k_per_contact[i]
if ki > 0:
# 均匀随机采样归一化权重
w_i = np.random.rand(ki)
w_i = w_i / (w_i.sum() + 1e-12)
alpha[col:col+ki] = w_i
col += ki
w = self.G_tilde @ alpha
wrench_samples.append(w)
wrench_samples = np.array(wrench_samples) # (num_samples, 6)
# 使用凸包计算内切球半径
try:
hull = ConvexHull(wrench_samples)
# 每个超平面: A x + b ≤ 0 (对于凸包内部的点)
# 原点 (0) 在凸包内部时, 对每个面: b_i ≥ 0
# ε = min_i b_i / ||a_i||
min_dist = float('inf')
for eq in hull.equations:
a = eq[:-1] # 法向量
b = eq[-1] # 截距
# 原点到面的距离 = |b| / ||a|| (因为原点代入为 b)
# 实际上对于内部点, eq 的符号约定是 a·x + b ≤ 0
# 所以原点处的值为 b, 距离 = b / ||a||
if np.linalg.norm(a) > 1e-10:
dist = b / np.linalg.norm(a)
min_dist = min(min_dist, dist)
return max(0.0, min_dist) if min_dist != float('inf') else 0.0
except Exception:
# 凸包计算失败 (可能样本不足或共面)
return 0.0
def grasp_quality_msv(self):
"""计算最小奇异值质量度量 σ_min(G_tilde)
对应 2.3.3 节公式:
Q_{msv} = σ_min(G_tilde)
注意: 该度量忽略了摩擦锥约束
Returns:
float: σ_min
"""
if self.G_tilde.shape[1] == 0:
return 0.0
_, S, _ = np.linalg.svd(self.G_tilde)
return S[-1] # 最小奇异值
4.4 多指手雅可比与指尖力反解
class MultiFingerHand:
"""多指灵巧手运动学/静力学模型
将第 3 章 PoE 运动学推广到多指手:
- 手雅可比 J_h 将关节速度映射到指尖速度
- 力域对偶: τ = J_h^T f
- 接触力反解: f = (J_h^T)^+ τ + F_null * λ
"""
def __init__(self, finger_S_lists, finger_M_list, finger_base_poses):
"""
Args:
finger_S_lists: List[List[(6,)]] 每个手指的旋量轴列表
finger_M_list: List[(4,4)] 每个手指的零位形末端位姿
finger_base_poses: List[(4,4)] 每个手指的基座在世界系中的位姿
"""
self.num_fingers = len(finger_S_lists)
self.finger_S_lists = finger_S_lists
self.finger_M_list = finger_M_list
self.finger_base_poses = finger_base_poses
self.finger_n_joints = [len(S_list) for S_list in finger_S_lists]
self.total_joints = sum(self.finger_n_joints)
def hand_jacobian(self, theta):
"""计算手雅可比 J_h
将第 3 章各手指的空间雅可比分块堆叠:
J_h = [J_{s,1}^T J_{s,2}^T ... J_{s,m}^T]^T ∈ R^{3m × n}
(假设 PwF 接触模型, p=3, 因此每指贡献 3 行)
Args:
theta: (total_joints,) 所有手指关节角度, 按手指顺序拼接
Returns:
J_h: (3 * num_fingers, total_joints) 手雅可比矩阵
"""
J_h = np.zeros((3 * self.num_fingers, self.total_joints))
row_offset = 0
col_offset = 0
for finger_idx in range(self.num_fingers):
n_j = self.finger_n_joints[finger_idx]
theta_finger = theta[col_offset:col_offset + n_j]
# 基座变换的伴随表示
T_base = self.finger_base_poses[finger_idx]
Ad_base = adjoint(T_base)
# 构造该手指的 PoE 前向运动学 (内联实现)
T_acc = np.eye(4)
J_finger = np.zeros((6, n_j))
for j in range(n_j):
S_j = self.finger_S_lists[finger_idx][j]
J_finger[:, j] = adjoint(T_acc) @ S_j
T_acc = T_acc @ self._se3_exp(S_j * theta_finger[j])
T_total = T_acc @ self.finger_M_list[finger_idx]
# 空间雅可比变换到基座坐标系 → 世界坐标系
J_finger_world = Ad_base @ J_finger
# 取线速度部分 (前 3 行) — 点接触模型仅关心位置速度
J_h[row_offset:row_offset+3, col_offset:col_offset+n_j] = J_finger_world[:3, :]
row_offset += 3
col_offset += n_j
return J_h
@staticmethod
def _se3_exp(S_theta):
"""se(3) 指数映射 (内联轻量实现)
Args:
S_theta: (6,) 旋量 × 角度
Returns:
T: (4,4) SE(3) 齐次变换
"""
omega_theta = S_theta[:3]
v_theta = S_theta[3:]
theta = np.linalg.norm(omega_theta)
T = np.eye(4)
if theta < 1e-12:
T[:3, 3] = v_theta
return T
omega_hat = omega_theta / theta
W = np.array([[0, -omega_hat[2], omega_hat[1]],
[omega_hat[2], 0, -omega_hat[0]],
[-omega_hat[1], omega_hat[0], 0]])
# Rodrigues: R = I + sinθ W + (1-cosθ) W²
R = np.eye(3) + np.sin(theta) * W + (1 - np.cos(theta)) * (W @ W)
# G(θ) = Iθ + (1-cosθ)W + (θ-sinθ)W²
G = np.eye(3) * theta + (1 - np.cos(theta)) * W + \
(theta - np.sin(theta)) * (W @ W)
T[:3, :3] = R
T[:3, 3] = G @ (v_theta / theta)
return T
def fingertip_forces_from_joint_torques(self, theta, tau, return_details=False):
"""从关节力矩反解指尖力: f = (J_h^T)^+ τ + F_null λ
关键性质: J_h^T ∈ R^{n × 3m} 是非方阵 (通常 n < 3m 或 n > 3m)
- 若 n > 3m (手指关节数 > 3×手指数): 超定, 关节力矩可分配
- 若 n < 3m: 欠定, 存在指尖内力空间
此处使用 Moore-Penrose 伪逆求解最小范数解
Args:
theta: (total_joints,) 关节角度
tau: (total_joints,) 关节力矩
return_details: 返回内力空间基
Returns:
f: (3*num_fingers,) 指尖力向量
"""
J_h = self.hand_jacobian(theta) # (3m, n)
J_h_T = J_h.T # (n, 3m)
# 最小范数解: f_min = (J_h^T)^+ τ
J_h_T_pinv = np.linalg.pinv(J_h_T) # (3m, n)
f_min = J_h_T_pinv @ tau
if not return_details:
return f_min
# 内力空间: null(J_h^T)
# F_null 的列 = J_h^T 的零空间基 (手指间内力, 不产生物体运动)
F_null = null_space(J_h_T) # (3m, dim_null)
return f_min, F_null
4.5 完整示例：三指手的三棱柱抓取
def three_finger_prismatic_grasp_example():
"""完整示例: 三指拟人手抓取三棱柱物体
场景:
- 物体: 等边三角形棱柱, 边长 0.06m, 中心在原点
- 三根手指均匀分布在物体周围 (120° 间隔)
- 每个手指使用有摩擦点接触 (PwF) 模型
分析流程:
1. 构造接触模型
2. 构造抓取矩阵
3. 力封闭判定
4. 抓取质量评估
"""
print("=" * 70)
print(" 三指三棱柱抓取分析")
print("=" * 70)
# ---- 物体参数 ----
R = 0.06 # 外接圆半径 (m)
mu = 0.5 # 摩擦系数 (橡胶-塑料)
# ---- 定义三个接触点 (120° 间隔) ----
contacts = []
for idx in range(3):
angle = 2 * np.pi * idx / 3
position = np.array([R * np.cos(angle), R * np.sin(angle), 0.0])
# 法向指向物体中心
normal = -position / np.linalg.norm(position)
contact = ContactModel(position, normal, mu=mu,
contact_type='PwF', num_cone_edges=6)
contacts.append(contact)
# ---- 构造抓取分析器 ----
analyzer = GraspAnalyzer(contacts)
# ---- 力封闭判定 ----
print("\n[1] 力封闭判定")
result = analyzer.check_force_closure(verbose=True)
# ---- 抓取质量评估 ----
print("\n[2] 抓取质量度量")
epsilon = analyzer.grasp_quality_epsilon(num_samples=5000)
msv = analyzer.grasp_quality_msv()
print(f" ε-度量 (Ferrari-Canny): {epsilon:.6f}")
print(f" 最小奇异值 σ_min(G̃): {msv:.6f}")
print(f" 内力空间维数: {result['internal_wrench_space_dim']}")
# ---- 形封闭判定 (将指尖转为 PwoF 检查) ----
contacts_pwof = [
ContactModel(c.position, c.normal, contact_type='PwoF')
for c in contacts
]
analyzer_pwof = GraspAnalyzer(contacts_pwof)
fc_result = analyzer_pwof.check_form_closure(verbose=True)
print(f" 形封闭判定 (3×PwoF): {'是' if fc_result['is_form_closure'] else '否'}")
print(f" (注: 形封闭对 3D 物体需要 ≥7 个无摩擦点接触, 此处 3 个不足以形封闭)")
# ---- 抓取矩阵数值展示 ----
print(f"\n[3] 抓取矩阵 G_tilde 的形状: {analyzer.G_tilde.shape}")
print(f" 秩: {np.linalg.matrix_rank(analyzer.G_tilde)}")
print(f" G_tilde 前 6×3 (前 3 个基本力旋量):")
np.set_printoptions(precision=3, suppress=True)
print(analyzer.G_tilde[:, :3])
return analyzer, result
def precision_grasp_vs_power_grasp():
"""对比分析: 精确抓取 vs 力抓取
演示不同接触配置对力封闭和抓取质量的影响:
- 精确抓取: 三指尖接触 (与 three_finger_prismatic_grasp 相同)
- 力抓取: 增加手掌接触 (4 接触点)
"""
print("\n" + "=" * 70)
print(" 精确抓取 vs 力抓取对比")
print("=" * 70)
R, mu = 0.06, 0.5
# ---- 精确抓取 (3 指) ----
contacts_precision = []
for idx in range(3):
angle = 2 * np.pi * idx / 3
pos = np.array([R * np.cos(angle), R * np.sin(angle), 0.0])
normal = -pos / np.linalg.norm(pos)
contacts_precision.append(
ContactModel(pos, normal, mu=mu, contact_type='PwF'))
analyzer_precision = GraspAnalyzer(contacts_precision)
result_precision = analyzer_precision.check_force_closure()
eps_precision = analyzer_precision.grasp_quality_epsilon(num_samples=5000)
# ---- 力抓取 (3 指 + 手掌) ----
contacts_power = list(contacts_precision)
# 手掌接触: 在物体底面中心偏下
palm_pos = np.array([0.0, -0.02, -0.02])
palm_normal = np.array([0.0, 1.0, 0.0]) # 法向指向上 (进入物体)
palm_normal = palm_normal / np.linalg.norm(palm_normal)
contacts_power.append(
ContactModel(palm_pos, palm_normal, mu=mu, contact_type='PwF'))
analyzer_power = GraspAnalyzer(contacts_power)
result_power = analyzer_power.check_force_closure()
eps_power = analyzer_power.grasp_quality_epsilon(num_samples=5000)
# ---- 结果对比 ----
print(f"\n{'指标':<30} {'精确抓取 (3指)':<20} {'力抓取 (3指+掌)':<20}")
print("-" * 70)
print(f"{'力封闭':<30} {'是' if result_precision['is_force_closure'] else '否':<20} "
f"{'是' if result_power['is_force_closure'] else '否':<20}")
print(f"{'d* (LP最优值)':<30} {result_precision['d_opt']:<20.6f} "
f"{result_power['d_opt']:<20.6f}")
print(f"{'ε-度量':<30} {eps_precision:<20.6f} {eps_power:<20.6f}")
print(f"{'内力空间维数':<30} {result_precision['internal_wrench_space_dim']:<20} "
f"{result_power['internal_wrench_space_dim']:<20}")
print(f"\n[分析]")
print(f" 力抓取因增加了手掌接触, 内力空间扩大, 可抵抗的外力范围更广。")
print(f" 精确抓取牺牲了鲁棒性, 但提供了更高的操作精度。")
print(f" 这与 Cutkosky (1989) 的抓取分类学一致。")
def hand_jacobian_example():
"""手雅可比与指尖力反解示例
模拟一个简单的三指手, 每个手指有 3 个关节 (共 9 DOF),
计算手雅可比并反解指尖力
"""
print("\n" + "=" * 70)
print(" 手雅可比与指尖力反解")
print("=" * 70)
# 简化: 每个手指是平面内的 3-DOF 连杆, 所有关节轴平行于 z
# 手指 1 基座在 (0, 0.02, 0), 手指 2 在 (-0.017, -0.01, 0), 手指 3 在 (0.017, -0.01, 0)
def make_finger_skews_3dof(base_pos, L=0.04):
"""构造 3-DOF 手指的空间旋量轴"""
S1 = np.array([0, 0, 1, -base_pos[1], base_pos[0], 0]) # 绕 z 通过基座
# 关节 2: 轴通过关节 1 末端 (base_pos + (L, 0, 0) 在零位形)
j2_pos = np.array([base_pos[0] + L, base_pos[1], 0])
S2 = np.array([0, 0, 1, -j2_pos[1], j2_pos[0], 0])
j3_pos = np.array([base_pos[0] + 2*L, base_pos[1], 0])
S3 = np.array([0, 0, 1, -j3_pos[1], j3_pos[0], 0])
return [S1, S2, S3], np.eye(4) # M = I
finger1_S, finger1_M = make_finger_skews_3dof(np.array([0.0, 0.03, 0.0]))
finger2_S, finger2_M = make_finger_skews_3dof(np.array([-0.026, -0.015, 0.0]))
finger3_S, finger3_M = make_finger_skews_3dof(np.array([0.026, -0.015, 0.0]))
hand = MultiFingerHand(
finger_S_lists=[finger1_S, finger2_S, finger3_S],
finger_M_list=[finger1_M, finger2_M, finger3_M],
finger_base_poses=[np.eye(4), np.eye(4), np.eye(4)]
)
# 关节配置: 手指弯曲接近物体
theta = np.array([0.5, 0.8, 0.3, # 手指 1
0.5, 0.8, 0.3, # 手指 2
0.5, 0.8, 0.3]) # 手指 3
J_h = hand.hand_jacobian(theta)
print(f"\n 总关节数: {hand.total_joints}")
print(f" 手指数: {hand.num_fingers}")
print(f" 手雅可比 J_h 形状: {J_h.shape}")
print(f" J_h 的秩: {np.linalg.matrix_rank(J_h)}")
# 关节力矩 → 指尖力
tau = np.ones(hand.total_joints) * 0.1 # 均匀 0.1 Nm
f, F_null = hand.fingertip_forces_from_joint_torques(theta, tau, return_details=True)
print(f"\n 指尖力 f = (J_h^T)^+ τ:")
for i in range(hand.num_fingers):
fi = f[3*i:3*i+3]
print(f" 手指 {i+1}: f = [{fi[0]:.3f}, {fi[1]:.3f}, {fi[2]:.3f}] N")
print(f" 内力空间 (null(J_h^T)) 维数: {F_null.shape[1] if F_null.size > 0 else 0}")
print(f"\n [注] 内力空间对应手指间可相互挤压而不改变物体运动的自由度。")
# ---- 运行全部分析 ----
if __name__ == "__main__":
three_finger_prismatic_grasp_example()
precision_grasp_vs_power_grasp()
hand_jacobian_example()
4.6 LEAP Hand 与简化设计原理（代码注释）
def leap_hand_design_principles():
"""LEAP Hand 的设计原理说明
LEAP Hand (Shaw et al., 2023, "LEAP Hand: Low-Cost, Efficient,
and Anthropomorphic Hand for Robot Learning") 是"有原则简化"
理念的最新体现:
设计策略:
1. 16 DOF 的拟人手仅用 4 个伺服电机驱动 (高度欠驱动)
2. 关节间通过腱绳 (tendon) 和差速机构耦合
3. 3D 打印的柔性关节实现了被动适应性
4. 总成本 < $2000 (相比 Shadow Hand 的 ~$150,000)
这与 Piazza et al. (2019) 的两条趋势高度吻合:
- 软体化: 柔性关节通过材料弹性实现自适应
- 架构简化: 欠驱动 + 协同运动显著减少电机数量
"""
print("LEAP Hand 设计哲学:")
print(" 1. 机械智能 (Mechanical Intelligence) > 控制智能")
print(" — 用弹簧和差速器自动分配手指运动, 而非用代码")
print(" 2. 柔性关节 = 内置的鲁棒性")
print(" — 碰撞不会损坏关节, 而是弹回")
print(" 3. 欠驱动 = 自动适应")
print(" — 手指接触物体后, 未接触的手指继续运动, 自动包裹物体")
print(" 4. 低成本 = 可规模化")
print(" — $2000 的手可以让更多实验室开展灵巧操作研究")
print("\n参见: Piazza et al. (2019) Section 4.2 (Architecture Simplification)")
五、核心 Takeaway 与延伸阅读
5.1 五个核心洞察
-
抓取矩阵 \(G\) 是”手指-物体交互”的通用语言。每一个接触以 \(G_i = [I; \hat{c}_i]\) 的形式贡献于 \(G\)，其列空间决定了可以实现哪些力旋量，其零空间 \((G^T)\) 决定了手指间可以如何”内耗”而不改变物体状态。\(G\) 的数学结构直接继承了第 3 章的旋量理论——抓取本质上是一组手指相对于物体的旋量轴的”并联”。
-
力封闭是抓取成功性的硬判据。一个抓取要么是力封闭的，要么不是——这不是一个连续可优化的量。\(d^* > 0\) 的 LP 判定给出二元的”握得住吗？”答案。而一旦力封闭成立，\(\epsilon\)-度量等质量指标才进入”握得有多好？”的讨论。这种二阶段结构——先判定再优化——是所有抓取规划算法的底层逻辑。
-
Piazza et al. 揭示了”机械智能”的先决性。论文最核心的启示是：在硬件层面做出正确设计决策（欠驱动、软材料、协同运动），可以大幅降低软件和控制层面的复杂度。一个 1 电机的 Pisa/IIT SoftHand (19 DOF) 比一个 13 电机的 DLR Hand II (13 DOF) 更鲁棒——不是因为算法好，而是因为机械结构本身”知道”如何适应。
-
生物协同 = 维数约简 = 设计约束。人手 24+ DOF 但日常仅需 2-3 个协同模式——这一神经科学发现为机器人手设计提供了直接的约束：与其给每个关节独立电机，不如在硬件层面固化为数不多的协同模式。这就是 hand synchronized motion 的工程学本质：用机械耦合编码神经协同。
-
从接触力学到灵巧操作的鸿沟正在由学习弥合。力封闭理论告诉我们”能不能握”，但没有告诉我们”怎么握更好”。传统方法是基于几何和力学模型的优化，但在非结构化环境中（未知物体形状、未知摩擦系数），解析方法捉襟见肘。这正是后续章节（Ch07-Ch12 的强化学习、行为克隆）发挥作用的地方：数据驱动的方法可以直接学到一个把 RGB 图像映射到抓取配置的策略——而这个策略的输出，最终都会经过 \(G\) 和 \(J_h\) 这些我们在本章建立的数学结构。
5.2 开放问题与研究前沿
- 可微抓取 (Differentiable Grasping): 如何将力封闭 LP 判定嵌入到可微的神经网络训练管线中？当前的主流方案是用隐式表示或松弛约束（如将摩擦锥约束松弛为罚函数）。
- 软体手操作的形式化: Piazza et al. 指出软体手利用环境约束来塑造抓取——但如何形式化这个”enabling constraints”概念？是否存在类似力封闭的”软体封闭”理论？
- Sim-to-Real 的抓取迁移: 物理仿真中的摩擦和接触动力学仍不可靠。如何确保在 Isaac Gym 中学到的抓取策略能迁移到真实软体手上？
- 联合设计 (Co-design): 是否可以同时优化手的机械参数（连杆长度、弹簧刚度、电机排布）和控制策略？这是具身智能中的”形态共优化”问题。
- 触觉-抓取闭环: 本章未涉及感知——但正如第 6 章将展示的，触觉反馈可以大幅提升抓取的鲁棒性。如何将触觉信号整合到力封闭的实时维护中？
5.3 推荐延伸阅读
| 优先级 | 文献 | 内容 |
|---|---|---|
| ★★★★★ | Murray, Li & Sastry (1994), Ch 4-5 | 多指手抓取的经典数学框架（力封闭、形封闭、抓取规划） |
| ★★★★★ | Bicchi & Kumar (2000), “Robotic Grasping and Contact: A Review” | 抓取理论的全面综述，覆盖到 2000 年的所有主要结果 |
| ★★★★☆ | Ferrari & Canny (1992), “Planning Optimal Grasps” | \(\epsilon\)-度量的原始论文，抓取质量的经典定义 |
| ★★★★☆ | Catalano et al. (2014), “Adaptive Synergies for the Pisa/IIT SoftHand” | Hand Synchronized Motion 的标志性实现 |
| ★★★☆☆ | Pratt & Williamson (1995), “Series Elastic Actuators” | SEA（串联弹性致动器）的奠基性论文 |
| ★★★☆☆ | Deimel & Brock (2016), “A Novel Type of Compliant and Underactuated Robotic Hand” | RBO Hand 2 的设计哲学 |
| ★★★☆☆ | Shaw et al. (2023), “LEAP Hand” | 低成本灵巧手的最新代表 |
| ★★★☆☆ | Salisbury & Craig (1982), “Articulated Hands: Force Control and Kinematic Issues” | Stanford/JPL Hand 的经典论文，接触模型与力控制的奠基 |
六、练习与思考
6.1 基础练习
练习 5.1 (接触运动学) 证明：对于无摩擦点接触 (PwoF)，一个 \(k\)-顶点凸多面体物体至少需要多少个接触点才能实现形封闭？给出 2D 和 3D 情形的答案并证明。（提示：3D 的结果来自 Markenscoff, Ni & Papadimitriou (1990) 的定理——对于非光滑物体，形封闭至少需要 7 个 PwoF 接触。）
练习 5.2 (抓取矩阵) 考虑一个长方体物体（尺寸 \(2a \times 2b \times 2c\)），两个手指分别在相对的面上施加有摩擦点接触。写出抓取矩阵 \(G\) 并找出其零空间。这两指抓取是否可能是力封闭的？
练习 5.3 (摩擦锥) 证明：对于摩擦系数 \(\mu = 0.5\)，使用 \(k=4\) 条棱边线性化的摩擦锥，其与原锥的 Hausdorff 距离为 \(1 - \cos(\pi/4) / \sqrt{1+\mu^2}\)。推导这一距离公式并讨论棱边数 \(k\) 的选择如何影响力封闭判定的保守性。
练习 5.4 (Hand Jacobian) 对于 4.5 节的三指手，推导手雅可比 \(J_h\) 在关节角度 \(\theta = 0\) 时的显式表达式，并验证：当所有手指的末端处于同一平面时，\(J_h\) 的秩最多为 5（即无法生成绕该平面法向的纯力矩）。
6.2 编程练习
练习 5.5 (力封闭比较器) 编写一个函数，输入不同的接触配置（接触点数 \(m\)、摩擦系数 \(\mu\)、接触位置），输出力封闭判定结果和 \(\epsilon\)-度量。对于固定物体（如单位球），绘制 \(\epsilon\) 随 \(m\) 和 \(\mu\) 的变化热力图。讨论：更多的接触点是否一定产生更好的抓取质量？
练习 5.6 (实时力封闭维护) 模拟一个物体在抓取中受到来自 \(x\) 方向的逐渐增大的外力。随着 \(F_{\text{ext}}\) 的增大，计算维持力封闭所需的最小指尖力（通过 LP 求解 \(\min \sum \|f_i\|\) 约束 \(Gf = -w_{\text{ext}}\)）。绘制”外力-最小抓取力”曲线，找出抓取失效的临界外力。
练习 5.7 (LEAP Hand 仿真) 基于第 3 章的 PoE 运动学框架，构建一个简化的 4 指欠驱动手的运动学模型。使用一个协同参数 \(s \in [0,1]\) 控制所有手指的开合（\(s=0\) 全开，\(s=1\) 全闭），用 hand synchronized motion 的思想将 16 DOF 约简为 1 DOF。
6.3 思考题
思考 5.1 (软体 vs 刚性: 理论极限) Piazza et al. 认为软体简化手比刚性精密手更鲁棒。但理论上，一个完全驱动的刚性手配合完美的感知和控制应该能够实现软体手的所有功能——甚至更多。那么”软体优势”是根本性的（来自物理的被动力学特性无法被主动控制复制），还是暂时性的（当前的感知和控制精度不足）？为你的论点提供数学论证或反例。
思考 5.2 (协同的代价) Hand synchronized motion 用 1-3 个电机控制 15-20 DOF 的手。这种协同约简的一个根本局限是：某些独立的手指运动（如竖中指）无法被表达。对于需要独立手指运动的任务（如弹钢琴、手语），协同方法是否从根本上不可行？如何在”简化”和”表达能力”之间找到最优平衡？
思考 5.3 (抓取与学习的结合点) 本章建立的力封闭理论和第 3 章的运动学/动力学为机器人抓取提供了完整的模型基方法 (model-based approach)。然而，近年来基于学习的方法 (learning-based approach) 在抓取任务中取得了显著进展。讨论：在你看来，模型基方法和学习基方法在抓取问题上的互补关系是什么？什么样的系统架构能够最有效地结合两者的优势？（提示：考虑”学习抓取点 + 力封闭验证”的流水线结构。）
章节备注:
- 本文的抓取矩阵和力封闭代码实现参考了 Murray, Li & Sastry (1994) Ch. 5 和 Bicchi & Kumar (2000) 的数学框架
- 论文精读部分基于 Piazza et al. (2019) 的全文及 Supplemental Table 1
- 代码以 NumPy/SciPy 实现，未依赖特定机器人中间件，可直接在任何 Python 3.8+ 环境中运行
- 第 2.3 节的力封闭 LP 判定在摩擦锥线性化假设下给出了充分条件；非线性锥的精确判定需使用二阶锥规划 (SOCP)