Contact Implicit Trajectory Optimization