[OMPL] also check constraints in StateValidityCallback (backport #3586)

[mergify]

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

---

This is an automatic backport of pull request #3586 done by [Mergify](https://mergify.com).

[codecov]

Codecov Report

✅ All modified and coverable lines are covered by tests.
⚠️ Please upload report for BASE (kilted@e401bc6). Learn more about missing BASE report.

Additional details and impacted files

@@           Coverage Diff            @@
##             kilted   #3649   +/-   ##
========================================
  Coverage          ?   0.00%           
========================================
  Files             ?     207           
  Lines             ?   23096           
  Branches          ?    3145           
========================================
  Hits              ?       0           
  Misses            ?   23096           
  Partials          ?       0           

☔ 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.