If you are able to use some part of this code. You will be able to integrate without having to rewritte too much, and based on a built OCP I believe.
https://github.com/pyomeca/bioptim/blob/3a3fa6bc12e9e02cbd36234e8ad53133adc035ef/bioptim/optimization/solution.py#L1313C9-L1411C1