modified so trajopt plans trajectory back