Simultaneous Trajectory Optimization and Contact Selection for Contact-rich Manipulation with High-Fidelity Geometry

TL;DR

The paper tackles the scaling bottleneck of contact-implicit trajectory optimization (CITO) in contact-rich manipulation by introducing STOCS, which embeds simultaneous trajectory optimization and contact selection inside an infinite-programming framework. STOCS dynamically instantiates salient contact points and times via an exchange method, using an outer Oracle (MVO or TAMVO with SD/TS) to keep the inner MPCC small while handling high-fidelity 3D geometries represented as dense point clouds and environment SDFs. Key contributions include the Time-active Maximum Violation Oracle (TAMVO) and spatial/temporal smoothing techniques that accelerate convergence, enabling feasible planning for tens of thousands of vertices in 3D. The approach enables perception-to-action pipelines from raw sensor data, reduces geometry simplification requirements, and significantly broadens the practical applicability of CITO in manipulation tasks.

Abstract

Contact-implicit trajectory optimization (CITO) is an effective method to plan complex trajectories for various contact-rich systems including manipulation and locomotion. CITO formulates a mathematical program with complementarity constraints (MPCC) that enforces that contact forces must be zero when points are not in contact. However, MPCC solve times increase steeply with the number of allowable points of contact, which limits CITO's applicability to problems in which only a few, simple geometries are allowed to make contact. This paper introduces simultaneous trajectory optimization and contact selection (STOCS), as an extension of CITO that overcomes this limitation. The innovation of STOCS is to identify salient contact points and times inside the iterative trajectory optimization process. This effectively reduces the number of variables and constraints in each MPCC invocation. The STOCS framework, instantiated with key contact identification subroutines, renders the optimization of manipulation trajectories computationally tractable even for high-fidelity geometries consisting of tens of thousands of vertices.

Figures (9)

• Figure 1: STOCS accepts as input the high-fidelity geometry of the object (represented by a dense point cloud) and the environment (represented by a signed distance field), the robot's contact point, and start and goal poses of the object (left). The STOCS algorithm first generates an initial trajectory by linearly interpolating between the start and goal poses, and then it iterates between selecting contact points and solving a finite-dimensional MPCC to decide a step direction until the convergence criteria are met (center). As output (right), STOCS produces the pose of the object, active object-environment contact points (green dots) and forces (green lines), and manipulation force (red lines). Nonpenetration, Coulomb friction, complementarity, and quasi-dynamic stability are enforced throughout the trajectory. [Best viewed in color.]
• Figure 2: Comparing various Oracles. The object trajectory is depicted as moving from left to right (as indicated by the black arrow) and undergoing clockwise rotation (as indicated by the arrow on the star). (b) In Maximum Violation Oracle (MVO), the closest point on the object is added ot the candidate set at every time step. (c) The Time-Active Maximum Violation Oracle, without Spatial Disturbance and Spatial Disturbances, introduces the closest point only at the current time step. The Time Smoothing technique with $n_s=1$, demonstrated in (d), extends constraint imposition to the closest points identified at adjacent time steps. (e) presents the Spatial Disturbance technique applied at a specific time step, with only disturbed rotation illustrated. [Best viewed in color.]
• Figure 3: Pivoting trajectories for 3 objects solved by STOCS. The object's poses, the contact between the object and the robot (red triangle), and object-environment contact points (green dots) are plotted for each time step. [Best viewed in color.]
• Figure 4: Trajectories planned by STOCS on 2D examples. The progression from the start to the end of the trajectory is indicated by a dark to light gradient. For clarity, the instantiated object-environment contact points are depicted only at the first and the last time steps. [Best viewed in color.]
• Figure 5: 3D test objects (first row) and their point clouds (second row) used in the experiments. Not drawn to scale.