[OMPL] also check constraints in StateValidityCallback

[MatthijsBurgh]

This allows the IK solver to continue finding a valid solution, which also satisfies the kinematic constraints.

In the old situation the IK solver would return a solution, which it thinks is valid, but it is not. The sampler will keep calling the IK solver. Though for IK solvers, which are not that random. There is a high chance of returning the same solution over and over. Which keeps failing the constraints.

By checking the constraints in the callback the IK solver will continue and return a valid solution.

Please backport to Jazzy and others

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[mergify]

Please target the main branch for development, we will backport the changes to jazzy for you if approved and if they don't break API.

[MatthijsBurgh]

@henningkayser @tylerjw Please review this PR.

[MatthijsBurgh]

@EzraBrooks do you know who can review this PR?

[EzraBrooks]

I believe @v4hn

[MatthijsBurgh]

@v4hn Could you please review this PR?

[MatthijsBurgh]

@ahcorde can you please take a look at this PR or assign it to another active developer?

[codecov]

Codecov Report

✅ All modified and coverable lines are covered by tests.
✅ Project coverage is 46.17%. Comparing base (f6acd91) to head (d3b324d).
⚠️ Report is 30 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3586      +/-   ##
==========================================
+ Coverage   46.17%   46.17%   +0.01%     
==========================================
  Files         720      720              
  Lines       59179    59180       +1     
  Branches     7595     7595              
==========================================
+ Hits        27320    27323       +3     
+ Misses      31692    31691       -1     
+ Partials      167      166       -1     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[ahcorde]

@nbbrooks can you also help here?

[MatthijsBurgh]

@ahcorde @EzraBrooks can we backport to Jazzy and newer distros?

[EzraBrooks]

I'm not familiar with the process for that on MoveIt, but I'll approve PRs if they're opened

[nbbrooks]

I believe mergify bot is configured to look at these tags and try to backport automatically. We will need to configure a backport-kilted label and workflow though.

[MatthijsBurgh]

@nbbrooks can you please also backport to kilted?