MotionPlanner: optimization
MotionPlanner: optimization
MotionPlanner: avoid unneeded planning while not stepping