A Unified Complementarity-based Approach for Rigid-Body Manipulation and Motion Prediction

TL;DR

The paper addresses the challenge of planning in unstructured environments by proposing Unicomp, a unified discrete-time framework that couples free-space motion with frictional contact through a single complementarity-based formalism. It integrates linear and nonlinear complementarity problems (LCPs/NCPs/MCPs) with an Equivalent Contact Point (ECP) representation and an ellipsoidal limit surface derived from maximum power dissipation to model planar patch contact, enabling principled transitions between contact modes without fixed-contact assumptions. The resulting discrete-time model, suitable for real-time optimization, jointly predicts motion and contact forces via a single MCP/NCP formulation and extends to multi-body and obstacle-avoidance scenarios. Experimental results across non-prehensile tasks, interactive planar pushing on non-convex patches, and comparisons with MuJoCo demonstrate stable, physically consistent behavior at interactive speeds, highlighting Unicomp’s potential as a practical foundation for robust manipulation planning in cluttered, contact-rich environments.

Abstract

Robotic manipulation in unstructured environments requires planners to reason jointly about free-space motion and sustained, frictional contact with the environment. Existing (local) planning and simulation frameworks typically separate these regimes or rely on simplified contact representations, particularly when modeling non-convex or distributed contact patches. Such approximations limit the fidelity of contact-mode transitions and hinder the robust execution of contact-rich behaviors in real time. This paper presents a unified discrete-time modeling framework for robotic manipulation that consistently captures both free motion and frictional contact within a single mathematical formalism (Unicomp). Building on complementarity-based rigid-body dynamics, we formulate free-space motion and contact interactions as coupled linear and nonlinear complementarity problems, enabling principled transitions between contact modes without enforcing fixed-contact assumptions. For planar patch contact, we derive a frictional contact model from the maximum power dissipation principle in which the set of admissible contact wrenches is represented by an ellipsoidal limit surface. This representation captures coupled force-moment effects, including torsional friction, while remaining agnostic to the underlying pressure distribution across the contact patch. The resulting formulation yields a discrete-time predictive model that relates generalized velocities and contact wrenches through quadratic constraints and is suitable for real-time optimization-based planning. Experimental results show that the proposed approach enables stable, physically consistent behavior at interactive speeds across tasks, from planar pushing to contact-rich whole-body maneuvers.

Figures (7)

• Figure 1: Our real-time software framework allows the system to perform non-prehensile tasks like planar pushing, along with safe whole-body maneuvers.
• Figure 2: ECP visualized
• Figure 3: Tool-driven pushing. The tool force indicates when pushing is active, while $(\Lambda_n,\boldsymbol{\Lambda},s,\rho_t,\rho_r)$ are computed at the block--ground ECP. Despite non-smooth excitation from tool contact, the same complementarity signatures hold: $s\approx 1$ in sliding, $s<1$ in sticking, and impulses vanish when contact breaks.
• Figure 4: Interactive planar pushing with non-convex support at $1000\,\mathrm{Hz}$. The green sphere denotes the impedance-controlled tool and the red sphere the user-specified target. For each pushing action (column), we show a snapshot (top) and the corresponding CoM (red) and object--ground ECP (blue) trajectories with pose stacks (bottom). (a) Table-like object; (b) dumbbell-like object.
• Figure 5: Comparison of Unicomp with Mujoco. During sliding and breaking modes, phantom forces arise in Mujoco, whereas our plots remain stable and smooth.