Rapid and Robust Trajectory Optimization for Humanoids