  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.
