https://gereshes.com/2019/10/28/an-introduction-to-direct-methods-in-optimal-control/