moveit2 Allow attached objects in move_group's /compute

Description

In the existing move_group implementation, the pose frame in PositionIKRequest's pose_stamped and pose_stamped_vector were converted to robot's root frame using TF2, which meant that the calls to /compute_ik service would fail with FRAME_TRANSFORM_FAILURE when one tried to use an attached object frame (or subframe) there. This changes the code to use RobotState::getFrameTransform to convert those poses to the robot's root frame instead, enabling the use of attached object frames/subframes.

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
[ ] 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

Dec 01 '24 04:12 ANogin

Could this capability delegate purely to RobotState::setFromIK() ? As of https://github.com/moveit/moveit2/pull/3077, this method should take collision objects and subframes into account.

Dec 03 '24 18:12 TSNoble

Could this capability delegate purely to RobotState::setFromIK() ? As of #3077, this method should take collision objects and subframes into account.

@TSNoble no, RobotState::setFromIK() only takes collision objects and subframes into account for the tip, but for the pose it just gets a plain pose that is interpreted against the robot root link. The frame id for the pose is never even passed into RobotState::setFromIK(). An alternative would be to further overload RobotState::setFromIK() to also take PoseStamped, not just Pose, but it's already so overloaded, that it's probably too messy.

https://github.com/moveit/moveit2/blob/e7872ebf92146bb80930ec98cf6c181e8384e99c/moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp#L924-L1026

Dec 03 '24 19:12 ANogin

Also, does RobotState::getFrameTransform() actually support getting frames not in the robot model, like available as external frames in the TF tree?

This may also be another major difference between the existing and proposed implementations, which would also break for any non-robot model reference frames.

Dec 08 '24 15:12 sea-bass

Codecov Report

:x: Patch coverage is 0% with 12 lines in your changes missing coverage. Please review. :white_check_mark: Project coverage is 43.94%. Comparing base (e7872eb) to head (6624b00). :warning: Report is 91 commits behind head on main.

Files with missing lines	Patch %	Lines
...ult_capabilities/kinematics_service_capability.cpp	0.00%	12 Missing :warning:

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3144      +/-   ##
==========================================
- Coverage   46.00%   43.94%   -2.06%     
==========================================
  Files         483      697     +214     
  Lines       40632    61491   +20859     
  Branches        0     7458    +7458     
==========================================
+ Hits        18689    27014    +8325     
- Misses      21943    34309   +12366     
- Partials        0      168     +168

:umbrella: View full report in Codecov by Sentry.
:loudspeaker: Have feedback on the report? Share it here.

:rocket: New features to boost your workflow:

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