https://jack.kawell.us/posts/ompl-moveit-custom-motion-planning/