https://123dok.net/document/7qvrmdry-h-rrt-c-haptic-motion-planning-with-contact.html