Given a set of sensor inputs, how would you structure a cost function for a motion planner to balance comfort and efficiency?