MotionPlanner: optimizations