Overview Video

Abstract

Non-prehensile manipulation of diverse objects remains a core challenge in robotics, driven by unknown physical properties and the complexity of contact-rich interactions. Recent advances in contact-implicit model predictive control (CI-MPC), with contact reasoning embedded within the trajectory optimization, have shown promise in tackling the task efficiently and robustly, yet demonstrations have been limited to narrowly-curated examples. In this work, we showcase the broader capabilities of CI-MPC through precise planar pushing tasks over a wide range of object geometries, including multi-object domains. These scenarios demand reasoning over numerous inter-object and object-environment contacts to strategically manipulate and de-clutter the environment, challenges that were intractable for prior CI-MPC methods. To achieve this, we introduce Consensus Complementarity Control Plus (C3+), an enhanced CI-MPC algorithm integrated into a complete pipeline spanning object scanning, mesh reconstruction, and hardware execution. Compared to its predecessor C3 , C3+ achieves substantially faster solve times, enabling real-time performance even in multi-object pushing tasks. On hardware, our system achieves overall 98% success rate across 33 objects, reaching pose goals within tight tolerances. The average time-to-goal is approximately 0.5, 1.6, 3.2, and 5.3 minutes for 1-, 2-, 3-, and 4-object tasks, respectively.

Uncut Single Object Manipulation Videos (1X speed)

Tight tolerances: 2cm, 0.1rad (5.7 degrees)

Push T - 28 consecutive SE(2) goals

Click on a thumbnail to play video

Uncut Two-Object Manipulation Videos (3X speed)

Tight tolerances: 2cm, 0.1rad (5.7 degrees)

Uncut Three-Object Manipulation Videos (10X speed)

Tight tolerances: 2cm, 0.1rad (5.7 degrees)

Uncut Four-Object Manipulation Videos (10X speed)

Tight tolerances: 2cm, 0.1rad (5.7 degrees)

System Diagram

We present the Push Anything framework (above), a pipeline integrating object perception with a novel controller. Our framework operates in two phases. In the offline phase, we build an object library by scanning objects to generate meshes (via BundleSDF) and URDFs, assuming same mass and inertia. In the online phase, our controller uses robot and object state estimates (the latter using FoundationPose) to compute end effector trajectories. Following the approach in Yang and Posa, these trajectories are tracked by a low-level operational space controller (OSC).

Our controller is implemented in C++ within the Drake systems framework. We use three computers in our experiments: (i) an Intel Core i9-13900KF (13th-gen, 32 threads) dedicated to the sampling-based controller, (ii) an Intel Core i7-9700K running the operational-space controller and robot drivers on a real-time kernel for Franka communication, and (iii) an Intel Core i9-14900K paired with an NVIDIA GeForce RTX 4090 for FoundationPose. All computers communicate via LCM.

Sampling-based MPC Controller using Enhanced Local CI-MPC

Our controller is built upon the original framework from Approximating Global CI-MPC. In this approach, candidate end effector positions are sampled and evaluated by solving the CI-MPC problem, augmented with a travel cost from the current location. The candidate with the lowest overall cost is selected, and if it differs from the current pose, the robot first moves along a collision-free path to that position before executing CI-MPC. As illustrated in the figure beside, by steering the system toward configurations that expand the reach of local MPC, this strategy helps the controller overcome local minima and achieve goals that require long-horizon reasoning. The success of the sampling-based CI-MPC framework hinges on two key components: a robust sampling strategy for selecting candidate end effector locations, and a fast, effective local CI-MPC. While this approach is effective, applying it to arbitrary objects, particularly in multi-object scenarios, requires significant adaptations.

Sampling Strategy: Applicable to Any Mesh

We pre-process object meshes by storing body-frame face locations, areas, and normal vectors. Given world-frame object pose estimates, we generate a candidate end effector location by first performing several random sampling steps in series: 1) select an object uniformly, 2) select a stored face of this object weighted by area, and 3) sample a point lying on this face. This surface point is then projected a fixed distance along the face's outward normal vector then projected to a fixed world height (see below). We reject samples which, even after projection away from one face, are too close to any of the objects. This can occur due to object non-convexity, the presence of multiple objects, or the selection of a face whose normal is too vertical. We repeat this process until the desired number of end effector candidates is obtained.

Above is a visualization of the sampling strategy for end effector locations. The gray plane indicates the ground, and the orange planes represent local tangent planes to the mesh surfaces. Blue arrows project surface samples outwards from the mesh along the face normals, then purple arrows project those to a fixed height in the world, generating candidate samples (green dots). Samples located too close to object surfaces (e.g. red dot) are discarded.

C3+ Algorithm: Fast, Effective Local CI-MPC

C3+ is an enhanced version of C3, which leverages the Alternating Direction Method of Multipliers (ADMM) to solve a Quadratic Program with Complementarity Constraints (QPCC), where the system dynamics are modeled as a Linear Complementarity System (LCS). C3 decouples the contact scheduling problem, which involves solving the complementarity constraints, across planning steps, enabling parallelization where each subproblem solves a small-scale Mixed-Integer Quadratic Program (MIQP). Building on this idea, our proposed C3+ algorithm introduces a new slack variable that further decouples the contact problem across individual contacts. This reformulation makes it equivalent to solving multiple independent 1D MIQPs, each has a simple closed-form solution, thereby transforming the costly, coupled exponential-time MIQP into a constant-time analytical computation and achieving a substantial overall speedup. More detailed derivations can be found in our manuscript.

Experimental Results

Single-Object Pushing

We evaluated our method in 701 hardware trials, testing 25 objects, with each object run until 28 successful trials were obtained. The system achieved a 99.9% success rate (700/701), with the only failure occurring when the large egg carton was pushed out of the robot’s reach. More details can be found in the plot below.

Multi-Object Pushing

We conducted a total of 227 trials, comprising 10 experiments for the 2-object case, 6 for the 3-object, and 5 for the 4-object case. Each experiment was run until 10 successful trials were achieved. Our method achieved a 92.5% success rate (210/227), with detailed results under both tight and loose tolerances presented in the table below. All failures occurred when an object moved beyond the robot’s reach. The mean time-to-goal for these tasks was approximately 96.4s, 191.1s, and 315.7s for the 2-, 3-, and 4-object settings, respectively. These numbers do not scale linearly with the number of objects because goal assignments are permuted across objects in each trial, requiring object rearrangements that introduce additional time.

C3+ vs. C3 Solve Time Comparison

We benchmark our CI-MPC algorithm C3+ against its predecessor, C3, to highlight its substantial speedup. Solve times are reported for 1-, 2-, 3-, and 4-object scenarios, totaling 103,959, 42,306, 78,129, and 40,161 solves, respectively. As shown in the table below, C3+ achieves faster overall performance: while the quadratic step is slightly slower, the projection step is four to five orders of magnitude faster.

Acknowledgments

This work was supported by an NSF CAREER Award under Grant No. FRR-2238480 and the RAI Institute.

Credits