Discrete Bimanual Manipulation for Wrench Balancing

01 Motivation

双臂系统能通过共同抓取一个物体来突破单臂的抓取力与负载上限。但作者指出：如果被抓物体质量分布不均，两臂承受的 wrench 会明显不同——"a stark increase in the wrenches experienced by the other"——这会显著增大超出单臂最大抓取力或 payload 的风险。当托盘上不断放置/移走物体时，其 center of mass (COM) 会移动，抓取点也需随之调整。

"We consider the problem of balancing the wrenches experienced by a dual-arm robot grasping a rigid tray. … Our approach is based on sequential sliding motions of the grasp points on the surface of the object, to attain a more balanced configuration."

为什么不能用现成方案？作者逐一排除：(1) regrasp 重新抓取需要松开抓取，在托盘还载着物体时会造成掉落，不可行；(2) 用非抓握式（non-prehensile）平衡抓取会导致托盘大幅倾斜，同样不适合载物场景。因此他们转向 dexterous manipulation：在不松开抓取的前提下移动接触点。由于用的是并联夹爪（parallel grippers，本身不灵巧），方案依赖 extrinsic dexterity——利用双臂系统的冗余度，让一只夹爪的手指沿托盘表面滑动而不改变托盘位姿。

system overview — **Fig.1 系统总览。**力/力矩测量 (p_i, f_i, τ_i) 被送入 estimator 得到 COM 估计 p̂_com，用来初始化规划流程；vision 提供托盘坐标系 Σ_t 的精确位姿，其余量都在该系下表达。优化器求出目标抓取点 (p₁*, p₂*)，交给 DMG 生成手内滑动路径，再经 robot motion planning 转成关节轨迹执行。

2接触点的双臂灵巧操作（非单手）

DMG把 Dexterous Manipulation Graph 扩到双抓取

视觉+力矩两种感知联合规划与执行

Discrete一次只动一只手臂，逐段执行

02 Method

方法分三步：先把 wrench 分布几何化建模并写成一个优化问题（求目标抓取点）；再用 Kalman filter 从力/力矩量在线估计 COM；最后用扩展后的 DMG 规划两只夹爪的手内滑动路径，逐一顺序执行。

1 · Wrench 的几何建模

两夹爪在 p₁、p₂ 抓取托盘，COM 在托盘表面的投影记为 p_com，受力 f_com。定义虚拟连杆（virtual sticks）r₁ = p_com − p₁，r₂ = p₂ − p_com，抓取点相对位置 p_r = p₂ − p₁。各点受力按投影比例分配：f₁ = (1−r′₁)f_com，f₂ = (1−r′₂)f_com，其中 r′_i = (r_i^⊤p_r)/‖p_r‖²。两点的力矩由 r_a = p_com − (r′₁p₂ + r′₂p₁) 决定，τ_i = r_a × f_com。

wrench quantities — **Fig.2 wrench 平衡相关量示意。**两夹爪在 p₁、p₂ 抓取托盘；p_com 是 COM 在托盘表面的投影，力 f_com 作用于此。r₁、r₂ 为虚拟连杆，r′₁、r′₂ 是它们在相对位置 p_r 上投影的大小，r_a 决定两抓取点的力矩。

Definition 1（Balanced Wrenches）：当两抓取点测得的 wrench 满足 f_e = f₂ − f₁ = 0 且 τ_e = (τ₁+τ₂)/2 = 0 时，即为平衡。于是"平衡 wrench"变成寻找抓取配置 (p₁*, p₂*) 的优化问题（式8）：min α‖f_e‖² + β‖τ_e‖²，约束 g_i(p_i) ≤ 0 限定抓取点落在托盘可达区域；α、β > 0 补偿力与力矩单位不同的权重。

在物体不滑动、不滚动的假设下（ḟ_com = 0），误差完全由抓取点与 COM 的几何位置决定：f_e = (r′₁ − r′₂)f_com，τ_e = r_a × f_com。据此改写为 Problem 1（式10）：min α(r′₁ − r′₂)² + β‖r_a‖² + γ c(p_r)。其中 c(p_r) 是一个非线性项（式11）：当 ‖p_r‖ 小于阈值 d_min 时施加惩罚，避免解把两抓取点挤得太近（几何上不可行）。

2 · COM 估计（Kalman filter）

Problem 1 依赖 p_com，而它会随托盘上物体的增减而移动，无法预知。作者用 Kalman filter 在线估计：过程模型 ṗ_com = 0、观测模型 z = C p̂_com（式12–14），观测量由实测力/力矩构造，并假设施加力沿托盘表面法向 n，从而由测量法向投影得到 r′_i。任务开始后即冻结当前估计，直至任务完成。

3 · 双抓取版 DMG + 顺序执行

DMG（Dexterous Manipulation Graph）原本描述"单个夹爪的一根手指如何沿物体表面移动"：图节点是接触点 + 可连续旋转的朝向区间，边表示手指可在两点间滑动。本文把它扩展到两只夹爪同时握持——为每只夹爪各维护一套 in-hand path，且规划时要同时考虑对侧手指（避免自碰撞、避开托盘上的障碍）。优化器（SLSQP）先给出目标抓取点，DMG 再用 Dijkstra 搜索最短滑动路径，并把跨越障碍的边加大代价以保证无碰撞。

关键的"离散"体现在执行上：一次只滑动一只夹爪，另一只保持静止并牢牢抓住托盘。这种 discrete coordinated motion 既简化了规划，又通过始终有一只手固定抓取来减少托盘的意外运动。Algorithm 1（Find In-Hand Paths）比较"先动谁"的两种顺序，取合并路径更短者。最终路径经插值满足接触约束，用 TRAC-IK 求关节位形，再生成两臂的关节轨迹并顺序执行。

balancing solutions — **Fig.3 同一个 p_com 下，α/β 取值不同会得到很不一样的平衡解。**只优化力 (β=0，蓝三角)、只优化力矩 (α=0，红方块)、两者兼顾 (α=10, β=100，绿圆)。图 (b) 中同时最小化力与力矩的绿色解把两抓取点挤得很近，可能不可行——正是式(11) 的距离代价 c(p_r) 要惩罚的情形。

03 Experiments

平台为 ABB YuMi 协作双臂机器人（单臂 payload 极限约 500 g）。每个手腕装一枚 OptoForce HEX 六轴力/力矩传感器；托盘位姿由外部 Microsoft Kinect2 检测 AprilTag 标记得到。托盘为矩形铝面板，面积 300×200 mm、重 186 g；托盘+夹爪+力矩传感器合计约 600 g，已超过 YuMi 单臂 500 g 的 payload——因此必须双臂共抓。全程 γ = 1000、d_min = 0.15 m；观测协方差噪声取 0.1·I，过程噪声取 0.01·I。作者分两种任务考察：只平衡力（β=0）与只平衡力矩（α=0）。

任务	权重设置	力矩误差 τ_e	力误差 f_e	平均执行时间	抓取稳定性
Force balancing	β=0, α≤10	0.15 → 0.025 N·m（降低）	下降（见 Fig.4a/b）	45 s（38–53 s）	稳定
Torque balancing	β=100, α=0	明显下降（Fig.4c）	~1 → 2 N（略升）	20 s（15–27 s）	仍稳定

Force balancing：展示了 15 次连续执行（Fig.4b，均值以红线标出）。作者强调"The successful force balancing experiments do not increase the torque error, but actually contribute to lowering it"——报告的所有成功执行都把力矩误差从平均初值 0.15 N·m 降到平均终值 0.025 N·m。

Torque balancing：20 次运行（Fig.4c）。执行时间更短（因计算出的路径更短）。尽管力误差平均从约 1 N 升到 2 N，抓取仍保持稳定。相较之下，只平衡力的问题里，只有那些"恰好也降低了力矩"的轨迹才成功。

experimental results — **Fig.4 力平衡系统实验结果。**(a) 单次执行的力误差 f_e（上）与力矩误差 τ_e（下）随时间演化，蓝/黄箭头分别标记一次滑动轨迹的起止时刻；(b) 同一目标下 15 次连续执行（均值红线）；(c) 力矩平衡任务的 20 次连续执行。注意 (c) 的 τ_e 纵轴量级远小于 (a)(b)。

Discussion — 平衡力矩才是关键

作者的核心观察：最小化力矩对维持托盘稳定平衡至关重要。只平衡力时，成功的执行无一例外都是那些同时降低了力矩的轨迹；而"平衡了力却增大力矩"的轨迹会因超出 payload 导致系统失败（托盘掉落）。这解释了为何 Problem 1 中的 β‖r_a‖² 项不可或缺。

04 Limitations

Note: 本文无独立的 "Limitations" 章节；以下要点部分为作者在 Experiments/Discussion/Future work 中明确陈述（stated），部分为根据方法设计推断（inferred），已逐条标注。

依赖 IK 解，遇动态障碍即放弃（stated）

作者明确表示系统依赖所需路径存在 IK 解，"if there are no IK solutions for a desired trajectory on the tray, we do not execute the balancing task"；避开动态障碍留作 future work。

准静态假设：物体不在托盘上滑动/滚动（inferred / stated）

建模时设 ḟ_com=0，即假设 COM 在运动中不变、托盘上物体不滑不滚。这限制了对易滚动或松散负载的适用性。

离散顺序执行，非在线连续调整（stated）

为简化规划并减少托盘意外运动，作者刻意采用 discrete、一次动一臂的执行方式；当 COM 变化时需由用户提示系统重新执行流程，而非全程在线跟随。

接触面小导致轻微俯仰/倾斜（stated）

Fig.4 中部分轨迹受托盘相对 YuMi 指尖的小幅 pivoting/tilting 影响；作者建议改用接触面更大的指尖可缓解并保证全程牢固抓取。

规模验证有限（inferred）

实验只在单一 YuMi + 一块 300×200 mm 铝托盘上验证；未报告不同形状/材质物体、多物体或更大负载下的泛化。