Staged Contact Optimization: Combining Contact-Implicit and Multi-Phase Hybrid Trajectory Optimization

Trajectory optimization problems for legged robots are commonly formulated with fixed contact schedules. These multi-phase Hybrid Trajectory Optimization (HTO) methods result in locally optimal trajectories, but the result depends heavily upon the predefined contact mode sequence. Contact-Implicit Optimization (CIO) offers a potential solution to this issue by allowing the contact mode to be determined throughout the trajectory by the optimization solver. However, CIO suffers from long solve times and convergence issues. This work combines the benefits of these two methods into one algorithm: Staged Contact Optimization (SCO). SCO tightens constraints on contact in stages, eventually fixing them to allow robust and fast convergence to a feasible solution. Results on a planar biped and spatial quadruped demonstrate speed and optimality improvements over CIO and HTO. These properties make SCO well suited for offline trajectory generation or as an effective tool for exploring the dynamic capabilities of a robot.

I. Introduction

Trajectory optimization is a common method of deter-mining optimal behaviors for legged robot systems, solving for body and joint state trajectories with feed-forward motor torques, often written as a nonlinear programming problem (NLP) [1]. Standard trajectory optimization problems are set up for continuous systems. For legged robots, the problem becomes much more complex due to their inherently nonsmooth impact dynamics, typically modeled as a hybrid dynamical system (i.e. a system with both continuous and discrete states) [2], [3]. In order to locomote, legged robots impact and push off of the ground changing their discrete contact mode. This changes their dynamics and creates discontinuities in velocity that cannot be directly handled in a standard trajectory optimization problem.