Trapezoidal trajectory generation