Energy optimal control