Moving all velocity clipping to safe_motion_planner