A direct method for trajectory optimization of rigid bodies through contact
Local Convergence of SQP Methods for Mathematical Programs with Equilibrium Constraints