Technical Summary: Simultaneous Contact Selection and Planning for Contact-Rich Manipulation with Cascaded Optimization

1. Problem Statement

Contact-rich manipulation requires robots to intentionally make and break contact with objects to drive them toward desired states. This involves planning a sequence of approaching trajectories, contact locations, contact forces, and transitions between contact modes (contact, separation, sliding, sticking).

Existing approaches face significant limitations:

• Hierarchical and Mixed-Integer Methods: Early works separated continuous-state planning from discrete contact mode searching or used mixed-integer programs. These often suffer from poor scalability due to the combinatorial explosion of contact trajectories, limiting manipulation strategies and robustness.
• Learning-Based Methods: While capable of handling nonsmooth dynamics via offline reinforcement learning (RL), they often struggle with generalization across diverse object types and new task settings.
• Contact-Implicit Model Predictive Control (CIMPC): Recent model-based methods like CIMPC enable online hybrid planning. However, they typically lack active contact selection. They rely on hand-crafted cost functions or given reference trajectories, constraining manipulation to a narrow distribution of behaviors. This makes them prone to local optima, especially when the robot's initial guess is poor or when facing objects with complex geometries. Furthermore, the complementarity inherent in contact dynamics introduces nonsmoothness and sparse gradients, making the unified design of contact selection and planning difficult.

The core challenge addressed is the lack of a unified framework that can autonomously reason about and generate diverse contact location sequences and manipulation trajectories in real-time, particularly under inaccurate dynamics and perceptual noise.

2. Methodology: SCSP Framework

The authors propose Simultaneous Contact Selection and Planning (SCSP), a novel cascaded optimization framework comprising two modules: Contact Selection Optimization (CSO) and Contact Planning Optimization (CPO).

A. Contact Selection Optimization (CSO)

CSO is designed to perform a global search for optimal contact locations on the object surface, addressing the gradient sparsity and nonsmoothness issues inherent in hybrid planning.

1. Discretization via Sampling and Exchange: To handle the nonsmoothness of object geometry, CSO samples candidate contact points on the object surface.
   • Approximate Convex Decomposition (ACD): The object geometry is decomposed into convex polygons to mitigate interference from local details and improve reachability/stability.
   • Farthest-Point Sampling (FPS): Points are uniformly sampled on the convex approximation.
   • Valid Region Selection: An online procedure filters out unreachable, occluded, or semantically unsuitable regions to form a valid candidate set.
2. Surrogate Contact Model (SCM): To resolve the computational complexity and complementarity constraints of the original contact dynamics, the authors introduce a SCM.
   • Assumptions: The model assumes a fixed contact set over short intervals and decouples multi-contact interactions into independent spring-damper systems.
   • Formulation: This allows the environment contact force to be expressed as an explicit, piecewise-linear function of the robot contact forces.
   • Optimization: The problem is reformulated as a Mixed-Integer Quadratic Programming (MIQP) problem. The inner loop (contact force optimization) is a Quadratic Program (QP) solvable in sub-millisecond timescales, while the outer loop (contact location selection) is solved efficiently.
3. Objective: The CSO objective prioritizes reaching a set of statically stable poses ($X_g$) before minimizing the final pose error, preventing the generation of unstable contact priors.

B. Contact Planning Optimization (CPO)

CPO takes the optimal contact locations predicted by CSO as references and generates executable approaching and contact trajectories online.

1. Ranking Strategy: To enhance robustness, CPO does not blindly follow the CSO output. It evaluates the nearest point ($p_{near}$) to the current robot configuration against the CSO-predicted optimal point ($\hat{p}^*_r$).
   • An improvement ratio ($\rho$) is computed to determine if $p_{near}$ is a sufficiently good suboptimal reference.
   • A sliding window strategy controls the switching between $\hat{p}^*_r$ and $p_{near}$ to avoid frequent, unstable contact changes.
2. Prior-Guided Hybrid Planning: The trajectory optimization is decomposed into three stages within a unified objective:
   • Lifting Phase: Guides the end-effector away from the object and toward an intermediate waypoint above the target contact location using repulsive and attractive potentials.
   • Placing Phase: Drives the end-effector to the reference contact location ($p_{ref}$) to establish contact.
   • Switching: The transition between phases is controlled by the ranking trigger variable $\gamma$.
3. Solvers: The framework supports different solvers, including Operational Space Control (OSC) for point-contact planning and sampling-based MPC (e.g., MPPI) for full-order redundant robot planning.

3. Key Contributions

1. Cascaded Optimization Framework: A novel SCSP framework that integrates active contact selection (CSO) with contact planning (CPO), enabling diverse manipulation strategies and robustness under model inaccuracies.
2. Efficient Contact Selection Algorithm: A method transforming nonsmooth contact selection into a discrete-continuous problem using mesh sampling and a Surrogate Contact Model, reducing the problem to a tractable MIQP for fast global search.
3. Prior-Guided Contact Planning: A formulation using a ranking strategy to evaluate contact priors and a unified objective for hybrid planning (approaching, contacting, switching), applicable to various robot configurations.
4. Experimental Validation: Extensive simulations and real-world experiments demonstrating that SCSP is the first model-based method to achieve real-world 6D nonprehensile manipulation of arbitrarily complex object geometries using only vision perception (no markers or motion capture).

4. Experimental Results

The framework was validated on nonprehensile manipulation tasks (on-ground rotation and flipping) using a fingertip and a Franka Emika Panda robot.

• Simulation:
  • Success Rate: SCSP achieved significantly higher success rates (e.g., 97% for fingertip rotation, 95% for robot rotation) compared to baselines like I-MPC, CF-MPC, and Sampling methods.
  • Robustness: It outperformed learning-based methods (DyWA) in most scenarios, particularly in handling complex geometries and avoiding local minima.
  • Ablation: Removing the ranking strategy (w/o RS) led to a significant drop in success rate, confirming the necessity of evaluating contact priors.
• Real-World:
  • SCSP successfully manipulated irregular objects (Duck, Bunny, Hand, Xbox Controller) on smooth and rough surfaces using only a single-view camera (FoundationPose) for state estimation.
  • It maintained high success rates (up to 100% for rotation) despite inaccurate dynamics (rough mass/friction estimates) and perceptual noise.
  • Baseline methods (DyWA, Sampling) frequently failed due to contact slippage, unstable contact selection, or inability to adapt to execution errors.
• Generalization: The framework was successfully extended to multi-contact tasks (soccer dribbling, tilted pushing) and long-horizon tasks (opening drawers, microwave doors), demonstrating its ability to perform online physical reasoning.

5. Significance and Claims

The paper claims that SCSP overcomes the theoretical limitations of existing contact-implicit methods by enabling active contact selection. While recent RL approaches have shown success, SCSP aims to make diverse and robust contact planning more interpretable.

The authors emphasize that the accuracy of the world model (CSO) is less critical than how it interfaces with the planner. The ranking strategy in CPO allows the system to utilize sub-optimal but feasible contact locations, mitigating the instability caused by model errors in the CSO. This suggests that a unified framework for simultaneous contact selection and planning is feasible and effective for complex, real-world manipulation tasks without relying on exact perception or pre-defined contact sequences.

The work positions SCSP as a general framework capable of handling arbitrarily complex object geometries, diverse robot configurations, and various contact-rich tasks, bridging the gap between model-based efficiency and the adaptability required for complex manipulation.