Trapezoidal velocity profile trajectory formula