discontinuity smoothing with trajopt
merge with jonathan to get faster graph search