MotionPlanner: Optimization.