Add trajopt version of fit_ThinPlateSpline