Quadratic integral penalty methods for numerical trajectory optimization