MotionPlanner: optimize critical section (at the expense of a little
more work outside it)