IEEE Transactions on Robotics

Simultaneous Trajectory Optimization and Contact Selection for Contact-Rich Manipulation With High-Fidelity Geometry

作者：Mengchao Zhang, Devesh K. Jha, Arvind U. Raghunathan, Kris Hauser · 机构：Department of Mechanical Science and Engineering, University of Illinois at Urbana-Champaign, Urbana, IL, USA; Mitsubishi Electric Research Laboratories (MERL), Cambridge, MA, USA; School of Computing and Data Science, University of Illinois at Urbana-Champaign, Urbana, IL, USA

会议/期刊：IEEE Transactions on Robotics · 发布日期：2025 · 页码：2677-2690

路径规划运动规划优化操作

精读笔记

Problem Setting

这篇论文处理的是 contact-rich manipulation trajectory optimization 在高保真几何下的接触选择问题。对象不是“给定几个接触点规划轨迹”，而是物体表面可能任意点与环境接触，接触时间也未知。困难点在于：几何越真实，潜在接触点越多；CITO 的表达能力依赖把这些潜在接触点放入互补约束，但 MPCC 的可解性又随候选接触数迅速崩溃。

以前方法卡在两个极端：hybrid TO 把 mode sequence 外包给人或离散搜索，3D 高保真几何下 mode 数组合爆炸；CITO 免去了显式 mode sequence，但只是在连续优化内部做 mode selection，前提仍是候选接触集合很小。这里的关键矛盾是：真实最优解通常只需要少量支撑接触，但优化器事先不知道这些接触在哪里、何时发生。

Motivation

作者真正想补的缺口不是新的摩擦模型或新的 MPCC 松弛，而是 CITO 的接触候选集构造方式。全量几何进入优化是理论上干净但数值上不可用；人工简化几何或手工指定接触点则破坏了 CITO 的吸引力。

核心观察很 active-set：高保真几何提供的是大量潜在约束，但最优轨迹的 KKT 条件通常由少数 active contact 支撑。因此，与其把所有点都作为可能接触，不如在优化过程中逐步发现“当前轨迹最需要解释/最违反”的接触点。这就是为什么他们采用 infinite programming / exchange method：完整几何保留在 oracle 里，MPCC 只看一个小的、动态增长的接触子集。

Core Idea

STOCS 的核心是把 CITO 从“固定候选接触集合上的一次大 MPCC”改成“接触候选生成 + 小 MPCC 反复求解”的闭环。物体表面被视为一个大的或连续的索引域，接触力是定义在该域上的函数；但在数值求解时，假设真正非零的接触力只集中在有限支撑点上。于是每次只实例化少数时空接触点，求解对应有限 MPCC，再由 oracle 查找当前轨迹下最需要加入的接触。

本质区别在信息流：prior CITO 在优化前固定“哪里可能接触”，STOCS 让轨迹本身反过来决定“哪些几何点值得进入优化”。这引入了一个强 inductive bias：接触是稀疏的、时空局部的、由当前违反/接近关系揭示的。这个 bias 很适合准静态/准动态 manipulation，因为力平衡通常由少量接触支撑；也正是它让方法比全量 MPCC 更 scalable。

Method

方法层面的必要机制可以压缩成四个。

1. Infinite-programming formulation：完整接触域不再直接离散成巨大 MPCC，而被保留为约束/变量的索引空间。它解决的是高保真几何与有限优化规模之间的冲突。核心变化是：几何复杂度不再线性进入每一次 NLP，而是通过 oracle 查询进入。

2. Finite-support MPCC：给定当前候选接触集合，只为这些点创建接触力、摩擦锥、距离互补、摩擦-速度互补和力矩平衡约束。它的作用是保持 CITO 的 contact-implicit 优点，但把规模限制在 active-set 级别。这里不是新动力学模型，而是规模控制。

3. Exchange loop + warm start + merit line search：外层逐步加入接触点，内层只跑少量 NLP 迭代，已有接触力继续 warm-start。它解决的是非线性大步可能造成穿透/约束恶化的问题。merit function 中加入全局最大穿透项，是为了避免只对已实例化点可行、对真实几何不可行。

4. Oracle 设计：MVO 加入每个时间步的最近/最深穿透点；TAMVO 则只把接触点加到相关时间附近，并用 spatial disturbance 和 temporal smoothing 扩展局部时空邻域。它解决的是 naive active-set 过密或过窄的问题。TAMVO 的实质是把“接触在时间上局部持续、在空间上对轨迹扰动敏感”编码进候选生成。

Key Insight / Why It Works

最关键的 insight 是：contact-rich manipulation 的困难并不一定来自真实接触数量大，而来自潜在接触数量大。真实轨迹的支撑接触稀疏，但传统 CITO 为了不漏掉接触，把潜在接触全部变量化，导致数值问题爆炸。STOCS 的有效性主要来自把“潜在接触空间”与“优化变量空间”解耦。

这本质上是 scaling + active-set inductive bias，而不是更强的物理建模。它没有让 MPCC 本身更容易，也没有解决非凸性；它只是让每次 MPCC 看到的问题小得多，并且通过 oracle 尽量保证小问题包含当前必要的接触。若 oracle 找得准，收益巨大；若 oracle 漏掉关键接触，优化会停在错误局部解或不可行。

TAMVO+SD+TS 是论文里最实质的工程/算法贡献之一。TAMVO 单独只看当前最近点，在 3D 中不足以满足力矩平衡，因为支撑接触可能不是单个最近点；SD 相当于在当前轨迹邻域做局部 lookahead，TS 则承认接触模式通常跨多个时间步连续存在。这个设计不是理论上必然最优，但很符合 manipulation 的局部几何结构。

需要直接判断的是：这篇的提升很大程度是规模组织方式带来的，而不是求解器或动力学模型上的突破。所谓 high-fidelity geometry planning 成立，是因为高保真几何主要用于 oracle 查询和 signed distance，而不是被完整纳入优化。这个思想非常有价值，但它也意味着上限由 active-set discovery 决定。

Relation To Prior Work

最接近的谱系是 CITO、semi-infinite / infinite programming、active-set constraint generation，以及此前用于 grasp pose optimization 的 IP 方法。STOCS 不是从零发明 contact-implicit optimization；它把已有 CITO 的互补约束保留下来，把 contact candidate set 的生成方式从固定集合改成 exchange-method 式的在线实例化。

和 hybrid trajectory optimization 的本质差异是：STOCS 不枚举 contact modes，也不要求 mode sequence；但它仍然通过 active contact set 间接形成 mode。和传统 CITO 的本质差异是：CITO 在一个预定义有限接触集合上隐式选 mode，STOCS 在高维/连续几何域中动态构造这个集合。

看似新的部分里，IP/exchange method 和 maximum-violation constraint generation 并不新；实质新增在于把它们嵌入 trajectory-level MPCC，并处理时空接触索引、力平衡和 oracle 的时空局部选择。TAMVO+SD+TS 更像针对接触轨迹优化的 domain-specific active-set heuristic，但这个 heuristic 正是让方法在 3D 高保真几何上可用的关键。

Dataset / Evaluation

实验覆盖了 2D 和 3D，任务包括 pushing、pivoting、rolling、rotating、peg-in-hole 等，几何来自密集点云、YCB/Google scanned objects、在线模型和 SDF 环境。任务种类足以验证论文的核心 claim：在高保真几何表示下，动态接触选择能把 CITO 推到 vanilla MPCC 难以处理的规模。

评估比较合理地包括了 vanilla MPCC 对照、不同 oracle 的消融，以及 3D 多对象多环境任务。但它验证的是离线数值规划的可扩展性，不是闭环执行能力。没有真实机器人实验，也没有系统评估感知噪声、摩擦误差、质量/CoM 误差和接触模型 mismatch。对于“可用于 raw RGB-D point cloud”这一愿景，实验还只是前置条件验证，不是 deployment-level 证明。

消融中一个有意思的点是：更少 index points 不总是更快，说明性能不只是变量数问题，也和 active-set 是否给优化器提供正确支撑结构有关。这支持了 oracle 质量的重要性，但文中未充分说明不同任务下求解时间差异的归因。

Limitation

第一，方法仍是局部非凸 trajectory optimization。它对初始轨迹、时间步长、目标姿态区域和 solver 行为敏感。STOCS 减少了规模，不提供全局最优或全局可行性保证。

第二，robot-object contact 被预定义且假设 sticking，这绕开了一个同样关键的接触选择问题。论文只让 object-environment contact implicit；真正 dexterous manipulation 中，手/夹爪与物体的接触切换、滑移、重抓取并没有被解决。这个限制很实质。

第三，物理假设偏强：刚体、准静态/准动态、忽略显著冲击和恢复、已知摩擦/质量/CoM。现实中这些参数误差会直接影响力矩平衡和摩擦可行性。文中也承认需要 robust optimization，但目前没有解决。

第四，scalability 被转移而不是消失。全量 MPCC 的瓶颈被转移到 oracle 查询、SDF 质量、点云采样密度、active-set 增长速度和局部接触覆盖。如果任务需要大面积接触、柔顺接触、多个同时支撑区域，稀疏支撑假设可能失效，MPCC 规模仍会膨胀。

第五，文中未充分说明带互补约束的 exchange method 在这些启发式 oracle、线搜索和有限 NLP 迭代下的理论收敛性质。实际成功更多是数值经验而非严格保证。

Takeaway

  • 1. 对高保真几何接触规划，关键不是把所有几何细节都塞进优化，而是让几何以 oracle/constraint-generation 的形式按需进入优化。
  • 2. CITO 的下一步演化很可能不是更复杂的互补模型，而是更好的 active-set discovery：哪些接触点、哪些时间、哪些局部扰动最值得实例化。
  • 3. TAMVO 的时空局部性假设值得迁移：在 trajectory optimization 中，一个约束在某个时间活跃，通常也在相邻时间和相邻状态扰动下有信息价值。
  • 这是比随机采样或全局最近点更强的 inductive bias。

一句话总结

Simultaneous Trajectory Optimization and Contact Selection for Contact-Rich Manipulation With High-Fidelity Geometry（IEEE Transactions on Robotics / 2025）把 CITO 从全量候选接触 MPCC 推进到 active-set / exchange-method 式的时空接触生成框架，真正贡献是用稀疏支撑接触假设解决高保真几何下的规模瓶颈。