MotionPlanner: Fix horrible bug.