Overview
Make contact forces part of the plan.
Contact makes manipulation possible. Its nonsmooth dynamics also make trajectory optimization difficult.
Conventional shooting methods determine contact forces inside the dynamics, leaving trajectory improvement dependent on contact sensitivities. Contact-implicit methods expose those forces, but introduce complementarity constraints that complicate optimization.
Lifted multiple shooting bridges these approaches. It introduces contact impulses as explicit optimization variables and separately enforces their consistency with the contact model. The resulting trajectory optimization problem can be solved directly with nonlinear program solvers.
Read the full abstract +
Planning through contact requires accounting for the coupled dynamics of a robot and its environment, including inertial effects, contact forces, and friction. However, the nonsmoothness and multimodality of contact dynamics and contact geometry makes gradient-based optimization challenging. In this work, we propose lifted multiple shooting, a trajectory optimization transcription that can be solved directly using nonlinear program solvers. By explicitly introducing contact forces as optimization variables, our formulation allows the optimizer to satisfy motion consistency using any friction-feasible contact forces, while separately enforcing their attainability through the contact model. Our formulation supports a broad class of contact constitutive relations, with sensitivities computed through implicit differentiation. To enable efficient planning over long horizons and for high-dimensional systems, we further introduce IP-PAS, a parallel-in-time primal-dual interior-point solver that achieves O(log T) parallel computational depth. We evaluate the proposed methods on six fully dynamic contact-rich planning tasks, including nonprehensile pushing and in-hand manipulation, without contact-guiding costs. Lifted multiple shooting improves success rates by up to 81.5% over standard multiple shooting, while IP-PAS achieves solution quality comparable to serial methods with up to a 3.1× speedup and scales to horizons of 40,000 timesteps.