Do not switch object and shape poses of attached objects with subframes

[ANogin]

Description

moveit/moveit#2816 (comment) said

If the object consists of a single shape, the object pose should be set to that shape's pose, and the primitive pose to identity. Otherwise, there is a regression for scenes that were built in a scene-graph-like manner by referring to previously placed objects.

It also looks nicer when you visualize the object pose in TF.

Namely, if an attached object has exactly one shape (primitive or mesh) and the pose of the attached object is "empty" (that is, the origin of the attached object coincides with the origin of the link it is attached to), then the code would "swap" the poses, moving the origin of the attached object to the origin of the shape/mesh.

Unfortunately this change had some IMHO very unfortunate side-effects. In my use case, the pose is not "empty", it is correctly set to identity, as the URDF was intentionally structured to have the origin of the robot link and attached object coincide, to make the whole thing simpler to set up. Whereas in my case the mesh origin is a completely "random" location (the URDF is created by exporting from Fusion using https://github.com/jaredgonzales/fusion360descriptor/ and Fusion has all meshes originate at global origin of the design, regardless of the local origin of the specific bodies). The issue is than two-fold:

• This changes the location of the attached object origin to some "random" place
• This fails to change the pose of the subframes, so the subframe locations end up equally wrong.
  This means that using the attached object or its subframe as a frame id or tip name in IK/motion planning results in things failing in completely unpredictable and seemingly incomprehensible ways (I just happened to have the scene returned by move_group's get_planning_scene logged and happened to see that the mesh origin was 0, which clued me to where to look in source code, otherwise I would have likely spent hours trying to understand why things work correctly for some attached objects, and completely bogus for others).

I see the following options:

1. If swapping, update the subframe poses so that they are unchanged.
2. Do not swap when subframes are present.
3. Never swap, and fix whatever incorrectly relies on the swapping behavior.
4. Add an extra boolean field yes_this_is_the_real_pose_even_if_its_an_identity_and_do_not_mess_with_it to the message type (obviously silly, but IMHO not more so than the current implementation).

IMHO option 3 is the right way, but I do not have bandwidth or understanding to implement it, and I do not know how much may be relying on the current behavior by now. This PR implements option 2 - it is the simplest fix for my use case and although I would expect to leave plenty of opportunities to trip people, it would hopefully avoid the worst side-effects.

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

[rhaschke]

I agree that swapping poses of the collision object and its single shape should not be done silently.
It is definitely a bug that subframes are not adapted appropriately. But the code would be much simpler, if swapping is simply omitted.
@felixvd can you explain which regression you are referring to in your comment moveit/moveit#2816 (comment)?
Dropping the pose switching does not harm CI in MoveIt1: moveit/moveit#3671

[codecov-commenter]

⚠️ Please install the to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 45.62%. Comparing base (e7872eb) to head (1a0ec71).

❗ Your organization needs to install the Codecov GitHub app to enable full functionality.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3145      +/-   ##
==========================================
- Coverage   46.00%   45.62%   -0.38%     
==========================================
  Files         483      482       -1     
  Lines       40632    40432     -200     
==========================================
- Hits        18689    18442     -247     
- Misses      21943    21990      +47     

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

[v4hn]

It is definitely a bug that subframes are not adapted appropriately.

👍

But the code would be much simpler, if swapping is simply omitted.
[Felix] can you explain which regression you are referring to in your comment moveit/moveit#2816 (comment)?

As mentioned somewhere in the threads on the migration to CollisionObject::pose, the previous pose of a CO was the pose of the only shape when there was exactly one and no other pose was known in a CollisionObject.
When objects were added w.r.t. other objects in the scene this pose was used as reference.
When we added the pose field, this semantic changed to use the new pose field instead, but a decade of code that possibly relied on the old interpretation would not break, but implicitly change behavior. After all the API only added a new field. We used the empty state to check whether the user knows about the field or left it uninitialized.

and the pose of the attached object is "empty" (that is, the origin of the attached object coincides with the origin of the link it is attached to)

That is not the definition of "empty" used in moveit. It is a bug in moveit2 that was introduced because quaternions in ROS2 default to identity instead of being zero-initialized.
I believe this condition should not exist in the moveit2 repository at all (as you suggest),
but it works as expected in moveit and I don't think it makes sense to change it there.