Get relative frame from robot state

[riv-mjohnson]

Description

• Fixes RobotState::setFromIK sporadically transforms poses wrong #3521
• Adds overloads to RobotState::getFrameTransform which take destination and source frames, and only calls updateLinkTransforms once.
• Uses this new overload in RobotState::setFromIK, to calculate the static transform between pose_frame and solver_tip_frame, avoiding the possibility of updateLinkTransforms invalidating the calculation.

Can this be backported to Humble etc. when merged.

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

[codecov]

Codecov Report

❌ Patch coverage is 66.66667% with 6 lines in your changes missing coverage. Please review.
✅ Project coverage is 46.22%. Comparing base (950322e) to head (1958998).
⚠️ Report is 2 commits behind head on main.

Files with missing lines	Patch %	Lines
moveit_core/robot_state/src/robot_state.cpp	66.67%	6 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3522      +/-   ##
==========================================
- Coverage   46.23%   46.22%   -0.00%     
==========================================
  Files         720      720              
  Lines       62717    62732      +15     
  Branches     7595     7598       +3     
==========================================
+ Hits        28988    28993       +5     
- Misses      33562    33572      +10     
  Partials      167      167              

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

[riv-mjohnson]

This doesn't seem to solve the problem. Since we already have pose_parent, is there a way to calculate the transform pose_parent_from_pose_frame directly, so we can go pose_frame_from_solver_tip_frame = pose_frame_from_pose_parent.inverse() * tip_parent_from_solver_tip_frame, which only uses rigid transforms?

Unhelpfully, the current code (before this PR) calls them pose_parent_to_frame and tip_parent_to_tip, but I don't think that's what is actually being calculated. I think getFrameTransform(pose_frame) returns ik_base_frame_from_pose_frame. Is this right?

[github-actions]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.