Tutorial on OMPL's Constrained State Space for position or orientation constraints

[gautz]

Description

Tutorial that goes together with the pull request on the main MoveIt repo: https://github.com/ros-planning/moveit/pull/2742

Co-authored-by: @JeroenDM [email protected]

This PR is consists of:

• a rebase of #518
• an extension to support orientation constraints

Required PR in MoveIt: -https://github.com/ros-planning/moveit/pull/2742

Checklist

• [ ] Required by CI: Code is auto formatted using clang-format
• [ ] While waiting for someone to review your request, please consider reviewing another open pull request to support the maintainers

[welcome[bot]]

Thanks for helping in improving MoveIt and open source robotics!

[gautz]

I changed a bit the position constrained tutorial and added an example for collision-avoidance. It consists of:

• position constraints represented with a box defining error bounds (goal translated+rotated)
• position constraints represented with a plane. The infinitesimal dimension is defined thanks to use_equality_constraints (goal translated+rotated)
• position constraints represented with a plane with Obstacle collision avoidance (goal translated)
• position constraints represented with a line. The infinitesimal dimensions is defined thanks to use_equality_constraints (goal translated)

Computation times are mostly 1s except for the collision avoidance 15-30s

[JeroenDM]

@gautz

Solving also fails for collision-avoidance examples around 1/4th of the time. My plan was to test Atlas based solving to see whether it improves performance and reliability. I don't have more ideas, except maybe tuning some numerical parameters.

Maybe we can add a note to the tutorial that performance is still spotty and needs improvement. But it still provides a valid alternative to try when solving constraint problems? (And using Atlas could be one of these future improvements.)

Do you plan to work on an orientation constraints PR ? Otherwise I could try to cherry-pick relevant commits and open a PR this week.

I'm sorry I hoped to have some more time now but, alas, I don't. If you cherry-pick (again) I'll try to be very active in the review :) Thanks again for keeping this alive!