Fix oscillation corner cases
Always try trajectory with 0 rot vel