initial trajectory interpolation bug fix