https://asmedigitalcollection.asme.org/computationalnonlinear/article-abstract/7/1/011009/465853/Natural-Coordinates-in-the-Optimal-Control-of?redirectedFrom=fulltext