https://123dok.net/document/qvl7jjry-hpp-a-new-software-for-constrained-motion-planning.html