discontinuity smoothing with trajopt