Trapezoidal trajectory generation in VelmOS system