Fix getLinkModelNamesWithCollisionGeometry to include the base link

[MarqRazz]

Description

I was working on an MTC task a while ago where I was manually modifying the planning scene. In my program I was calling getLinkModelNamesWithCollisionGeometry() on the JointModelGroup to get the links of the manipulator that had collision geometry and noticed that the base link of the group is never included. This caused planning problems like this while I was debugging...

To test this change out I modified the moveit2_tutorial Robot Model and Robot State where I asked it to print the results of getLinkModelNamesWithCollisionGeometry and with this new change it now includes panda_link0 🥇

RCLCPP_INFO(LOGGER, "Joint model group `panda_arm` collision links:");
std::vector<std::string> collision_link_model_names = joint_model_group->getLinkModelNamesWithCollisionGeometry();
for (const auto& name : collision_link_model_names)
{
    RCLCPP_INFO(LOGGER, "Link with collision: %s", name.c_str());
}

Output:
Joint model group `panda_arm` collision links:
  Link with collision: panda_link0
  Link with collision: panda_link1
  Link with collision: panda_link2
  Link with collision: panda_link3
  Link with collision: panda_link4
  Link with collision: panda_link5
  Link with collision: panda_link6
  Link with collision: panda_link7

Very small nitpick but I have also noticed that in Rviz the base link is not highlighted for the goal pose which shows that it is not include in the group when using the motion planning widget.

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

All modified and coverable lines are covered by tests ✅

Comparison is base (4c024bc) 51.15% compared to head (bb6b86f) 51.15%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2660      +/-   ##
==========================================
+ Coverage   51.15%   51.15%   +0.01%     
==========================================
  Files         393      393              
  Lines       32749    32753       +4     
==========================================
+ Hits        16750    16753       +3     
- Misses      15999    16000       +1     

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

[MarqRazz]

Closes #2325

[sjahr]

Makes sense to me, thanks for fixing it!

[simonschmeisser]

I'm not sure this is the correct approach. There can be more than one discrete subtree in a group, this would only consider the first one.

We discussed a similar issue I observed in moveit/moveit#3570

I think the previous version of the code is correct for is_chain_ == false (a list of <link> or <joint> tags in SRDF) but not for the case of <chain> where the root link is included but not parent joint.

[sjahr]

Oh you're right, thanks for pointing that out! Do you think we should revert this change for now or just add a check for is_chain_?

[simonschmeisser]

Hmm, this is getting surprisingly complicated ... is_chain_ is not passed to the constructor but guessed about near the end of the constructor. They might be different things altogether?

According to the code in RobotModel a chain as specified in SRDF could also go from finger_1_tip to finger_2_tip with the root being somewhere in the middle (eg the wrist).

Maybe a list of links should also get passed in to the constructor.

I'm also not yet sure how this relates to collision checking and what the problem actually was.

[simonschmeisser]

Could you please add your SRDF?

[sjahr]

Could you please add your SRDF?

Unfortunately, I don't have access to it

[MarqRazz]

Here is the SRDF for the Panda arm.

Prior to this PR if you were to request the link models with collision geometry
std::vector<std::string> collision_link_model_names = joint_model_group->getLinkModelNamesWithCollisionGeometry();
it would not include the base frame panda_link0 which you can see is part of the panda_arm joint model group. I was only trying to fix this function call to return a valid list. In my original use case with the robot on a rail (which was also build with <chain>) I was manually enabling and disabling collisions given the result from getLinkModelNamesWithCollisionGeometry which is why I noticed the issue.

[simonschmeisser]

Good to know that the collision checking failure was caused by workarounds. I tested a bit with a similar setup today (MoveIt1) but couldn't notice anything broken.

I think this fix adds regressions if:

• group is specified via , then the parent should obviously be excluded
• group is specified via , then the convention as documented is to include only the child links
• group is chain but ^ shaped, then some link will be duplicated in the list

And does not fix

• more than one "vertical" chain is specified, then only one will be corrected

Most likely the list of links should be passed into the constructor and the special casing for be done in RobotModel