Vehicle shift optimal control