精读笔记
Problem Setting
《Robust Pivoting Manipulation Using Contact Implicit Bilevel Optimization》（IEEE Transactions on Robotics / 2024）关注的是一个很窄但力学上尖锐的问题：二维两点外接触 pivoting 中，在物体参数和接触参数不准时，如何规划一条能维持滑动接触并避免失稳的轨迹。
关键矛盾是：pivoting 需要 controlled slip，而 slip 意味着接触力在摩擦锥边界上，不像 sticking 那样还有 cone interior slack 可用。传统 contact-implicit TO 可以找到满足准静态平衡和摩擦约束的 nominal 解，但这类解常常贴着接触失效边界；稍微有质量/CoM/摩擦/接触点误差，某个法向力变成零或符号错误，接触模式就崩。
以前方法卡住的点不是不能优化接触轨迹，而是没有一个直接服务于 manipulation 的 mechanical stability margin。随机/鲁棒轨迹优化可以采样参数，但很难解释和利用“摩擦力重分配能吸收多少未知 wrench”。这篇论文实际要补的是这个缺失的力学量。
Motivation
作者的动机不是泛泛地“接触操作有不确定性”，而是指出已有路线对鲁棒性的建模位置错了：大多把不确定性放到动态 rollout 或 stochastic complementarity 里，但没有显式问当前接触构型本身有多少抗扰余量。
核心观察来自多接触系统的静力学：外部扰动不一定立即导致失稳，因为多点接触的法向力和摩擦力可以重新分配。只要重新分配后仍满足非负法向力和滑动摩擦关系，操作就能保持。换句话说，摩擦不是只是 hybrid dynamics 的麻烦源，它也提供了一种可优化的 stability resource。
关键缺口是 manipulation 中缺少类似 locomotion 的 wrench cone margin / stability polygon 的概念，尤其是在滑动接触而非站立/支撑接触中。论文的方向就是把“接触力可重分配范围”变成规划目标。
Core Idea
论文真正核心是 frictional stability margin：给定一条 nominal pivoting 轨迹及其机器人接触力，问这个状态下质量误差、CoM 偏差、摩擦扰动或 finger contact location 偏差最多可以多大，仍能通过调整环境接触力满足准静态平衡并维持接触。这个 margin 不是 cost shaping，而是由力学约束直接诱导出的鲁棒性证书。
然后用 CIBO 把这个证书嵌入规划：下层在每个时刻求最大正/负扰动范围，上层优化轨迹并最大化整条轨迹的最小 margin。这样，优化器被迫选择那些不仅能完成旋转、而且能在关键姿态保留接触力重分配空间的动作，例如改变手指接触位置、增大合适方向的法向力、利用 patch contact 提升 wrench 空间。
和 prior 的本质区别在于：它不是在 nominal TO 外面加鲁棒采样，也不是在线 feedback 修正接触状态；而是把“可承受不确定性的力学边界”作为 trajectory optimization 的内生目标。这个 inductive bias 很强：好的轨迹应该最大化接触系统的吸扰能力。
Method
方法的关键机制可以压缩为几件事。
第一，作者建立二维准静态 pivoting 平衡方程。环境两点 A/B 是受控滑动接触，因此摩擦力满足等式关系，而不是普通 friction cone inequality。这个选择直接决定了鲁棒性分析必须围绕法向力非负和滑动模式保持展开。
第二，针对不同不确定源推导解析 margin。质量不确定对应额外重力和力矩，CoM 不确定对应重力力臂偏移，摩擦不确定被转成接触切向力扰动，finger contact location 不确定对应机器人力矩臂误差。每一种情况下，保持接触的条件最终变成扰动变量的上下界。这里最重要的不是公式本身，而是把不确定性投影到“是否仍存在可行接触力重分配”这个问题上。
第三，CIBO 将 margin 最大化写成 bilevel optimization。lower level 对每个时刻求最大正向/负向稳定余量；upper level 选择状态、接触力、机器人输入，并最大化时间维度上的最小余量。因为 lower level 被设计成 LP，作者可以用 KKT 条件把 bilevel 问题转成单层 MPCC/NLP。这是可计算性的关键，也是方法边界的来源。
第四，对非凸物体，论文没有真正做接触模式搜索，而是采用 mode-sequence-based optimization：用户给定模式顺序，优化各模式内轨迹和持续时间。这显著降低了人工调参相比顺序两阶段求解的难度，但没有解决模式发现问题。
第五，patch contact 被近似成两个等力分布点接触。它的作用是扩大可用 wrench 空间，从而自然提高 margin；这部分更像工程上更真实/更有利的接触模型，而不是 CIBO 的核心创新。
Key Insight / Why It Works
这篇论文最重要的 insight 是：在接触丰富操作里，鲁棒性可以通过“接触力重分配空间”来度量，而不一定要通过不确定动力学 rollout 来度量。对于准静态多接触系统，只要扰动后的平衡方程仍有非负法向力解，物体就不会立即失去外接触；摩擦提供的是一种 wrench absorption capacity。
CIBO 有效的主要原因不是 bilevel 形式本身，而是 lower-level margin 把物理上最脆弱的失效模式显式暴露给 optimizer：哪个时刻、哪个方向的扰动会先让 A 或 B 的法向力归零。上层最大化最小 margin，相当于把 trajectory optimization 从“找任意可行解”改成“远离接触失稳边界”。这比普通 cost regularization 更直接。
最可能的核心贡献是 frictional stability margin 的力学化表达，以及把该 margin 沿时间取 worst-case 后用于规划。KKT reformulation 是必要的计算工具，但不是主要科学贡献；mode-based nonconvex extension 和 patch contact 更偏 engineering extension。
从归因看，增益大概率来自 better inductive bias，而不是 scaling/data。它没有学习模型，也没有数据覆盖优势；提升来自把优化目标对准了硬件失败的真实原因：nominal 解太接近接触失效边界。不过硬件成功中的一部分增益可能来自更大的法向力、更保守的动作、stiffness controller 的 compliance 和实际 patch contact，这些因素与 margin objective 纠缠，文中未充分拆分。
需要注意的是，所谓 robustness 是局部且结构化的：它不是对任意扰动鲁棒，而是对被解析推导覆盖的参数方向有 margin。多个不确定源联合存在时，分别最大化 margin 不等价于联合鲁棒；这点作者也承认 lower-level 会变非凸。这里没有 hidden data 或 benchmark leakage 问题，但有明显的 modeling bias：只要真实失效模式不在这些 margin 里，证书就可能失效。
Relation To Prior Work
这篇最接近三条谱系：contact-implicit trajectory optimization / MPCC，robust contact planning，以及 legged locomotion 中的 contact wrench margin。它的本质位置是把 locomotion 里的“多接触静力稳定余量”迁移到 manipulation 的滑动接触 pivoting 中。
相对于 LCP/MPCC 接触轨迹优化，新增信息是优化目标从 nominal feasibility 变成 worst-case frictional stability margin。相对于 stochastic complementarity / sampling-based robust manipulation，它不是靠采样不确定参数，而是推导扰动可吸收区间。相对于 shared grasping / in-hand pivoting / table-top manipulation feedback control，它强调的是预规划轨迹本身的机械余量，而不是依赖 tactile/vision feedback 把系统推回安全区域。
看似新的 bilevel 结构其实是已有“上层设计变量 + 下层 margin/certificate”的重组，KKT 化也常见；真正实质创新在于把 sliding pivoting 下的接触力重分配边界写成可优化的 margin，并证明它能指导轨迹选择。非凸物体的 mode sequence optimization 是实用增强，但仍依赖人工模式先验，不是接触模式规划上的根本突破。
Dataset / Evaluation
评估覆盖了仿真/优化分析、不同对象参数、非凸 peg、斜面、patch contact、真实机械臂实验，以及 vision-feedback MPC。任务范围比单一 demo 更丰富，但仍集中在二维 pivoting primitive，环境接触几何明确，物体几何已知，接触模式高度受控。
实验最能支持的结论是：在该 pivoting setting 下，显式最大化 frictional stability margin 的轨迹比 baseline contact-implicit TO 更抗实际误差。尤其是真机中 baseline nominally feasible 但失败，而 CIBO 成功，说明 margin 对真实部署有相关性。
但 evaluation 不能充分支持强泛化 claim。对象数量有限，场景变化主要是尺寸/质量/形状参数变化，而非未知几何、未知接触模式或复杂三维环境。非凸物体实验仍给定 mode sequence；MPC 扰动恢复也要求扰动不破坏接触。计算评估显示 CIBO 可解，但 complementarity 数量上升后时间明显增加，实时性只在相对简单设置下成立。
Limitation
最大限制是建模前提很硬：二维、准静态、刚体、平面外接触、点接触或非常简化的 patch contact、已知几何和姿态。只要进入动态操作、冲击、滚动/粘滑切换、复杂 compliance 或三维 generalized friction cone，当前 margin 推导都需要重做。
第二，鲁棒性是单源不确定性的。质量和 CoM 同时不确定时会出现非线性耦合；接触模式不确定或几何不确定会让 lower-level 变非凸。CIBO 的可解性本质上依赖作者把 lower-level 保持为 LP，这也是它的上限。
第三，非凸物体部分没有自动模式发现。所谓 mode-based optimization 只是把两阶段手工规划改成给定 mode sequence 下的一体化优化。对于复杂形状，核心难题被转移到用户提供物理可行模式序列。
第四，硬件增益归因不完全干净。CIBO 往往会选择更大的法向力、更合适的 finger motion；真实系统还有 patch contact、stiffness compliance、摩擦实际分布等未建模因素。文中未充分说明 margin objective 与这些工程因素各自贡献多少。
第五，MPC 部分展示了闭环可用性，但状态反馈主要来自视觉，无法准确估计手指-物体 slip。对于大参数 mismatch 或 slip 主导的失败，当前闭环并没有完整接触状态观测，恢复能力有明显边界。
Takeaway
- 第一，接触操作中的鲁棒性可以被重新表述为“当前接触构型能吸收多少未知 wrench”，这比单纯 nominal feasibility 更接近硬件失败机制。
- 第二，摩擦不只是需要规避的不确定约束；在多接触准静态操作中，它是可规划的稳定资源。
- 这个 insight 可迁移到 pushing、grasping、one-point pivoting 等其他 manipulation primitive。
- 第三，bilevel margin optimization 的价值在于把物理 certificate 放进 planning loop；但要保持可计算，lower-level 必须足够简单。
一句话总结
这篇论文把两点滑动 pivoting 的鲁棒规划从 nominal contact-implicit feasibility 推进到显式最大化摩擦稳定余量，是一类 mechanics-derived robustness certificate 嵌入 trajectory optimization 的代表性工作。
