https://5dok.org/document/yr3drjjp-methods-for-direct-optimal-control-of-multibody-systems.html