Allow RobotState::setFromIK to work with subframes (backport #3077)

[mergify[bot]]

Description

Fixes #3072

• Adds passing regression / sanity tests which verify that a RobotState can setFromIK with a collision object
• Adds failing tests demonstrating that RobotState cannot setFromIK with a subframe
• Fixes failing tests by modifying RobotState::setFromIK to consider subframes
• Adds some helper functions to the pilz tests for attaching objects / subframes and checking joint values

I tried adding the tests directly to the robot state tests, however, I was getting issues with no kinematic solver being associated with the planning group of the OneRobot fixture.

Pilz already had a test which was checking setFromIK, and appears to be setup with a kinematic solver, so I've added the tests there.

Checklist

• [x] 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
• [x] Create tests, which fail without this PR reference
• [x] Include a screenshot if changing a GUI (no GUI changes)
• [ ] 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 #3077 done by Mergify.

[mergify[bot]]

Cherry-pick of ab34495d2d901fb9f053b706bb610c574b5bc0d6 has failed:

On branch mergify/bp/humble/pr-3077
Your branch is up to date with 'origin/humble'.

You are currently cherry-picking commit ab34495d2.
  (fix conflicts and run "git cherry-pick --continue")
  (use "git cherry-pick --skip" to skip this patch)
  (use "git cherry-pick --abort" to cancel the cherry-pick operation)

Changes to be committed:
    modified:   moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp

Unmerged paths:
  (use "git add <file>..." to mark resolution)
    both modified:   moveit_core/robot_state/src/robot_state.cpp

To fix up this pull request, you can check it out locally. See documentation: https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/reviewing-changes-in-pull-requests/checking-out-pull-requests-locally

[sea-bass]

@rr-tom-noble sadly this one has conflicts too -- would you like to take a look? Thanks!

[rr-tom-noble]

@sea-bass Given the tests, I'm confident that attaching objects & subframes through the RobotState now work with setFromIK.

Our in-practice use case a bit more complicated and involves using planning scene services to load and attach objects. It could be related to our mix of and binary and source, but my gut feel is that getFrameInfo() is working, and it's more related to the difference in how the objects are attached.

Given that issue arises from a somewhat different use-case, I think I'm happy with this PR as is, though I'm happy to leave it up to your discretion. I'll be looking into the next issue shortly.

[TSNoble]

@sea-bass I believe the failure is unrelated. I've seen it sporadically on a few other PRs