LinearPlanner: optimization
MotionPlanner: reorder some code