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

TL;DR

This paper presents STOCS, a novel method that integrates contact point selection into trajectory optimization, enabling efficient planning for complex, contact-rich manipulation tasks with detailed geometries.

Contribution

STOCS extends contact-implicit trajectory optimization by dynamically selecting salient contact points, reducing computational complexity for high-fidelity geometries.

Findings

Enables trajectory optimization with detailed geometries.

Reduces solve times significantly.

Maintains accuracy in contact-rich manipulation planning.

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.