cartesian trajectory interpolator fix