MotionPlanner: small memory optimization