Path constrained trajectory optimization using sparse sequential quadratic programming