Links not included in JMG are considered for collision detection

[dseredyn]

Description

Joint model group is ignored on collision detection. All links are checked for collisions with environment.

Environment

• ROS Distro: Melodic
• OS Version: Ubuntu 18.04
• Binary build: 1.0.10-1bionic.20220413.182619

Steps to reproduce

Add group in the srdf file, check for collision betwee link excluded from the group and environment (e.g. octomap).

Expected behaviour

The collision between link excluded from the group with the environment should be ignored.

Actual behaviour

The collisions are checked for all links. Group information is ignored.

Possible cause

In method CollisionData::enableGroup the following method is used to get link names of a group: moveit::core::JointModelGroup::getUpdatedLinkModelsSet and the docs tor this method says: "Get the names of the links that are to be updated when the state of this group changes. This includes links that are in the kinematic model but outside this group, if those links are descendants of joints in this group that have their values updated. The order is the correct order for updating the corresponding states" so all links are checked in collision detection and this causes wrong behaviour.

[welcome[bot]]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[rhaschke]

This is the desired behavior for motion planning: If you sample joints in the JMG, you need to check for all links that might have moved due to these joint motions. This includes all descendants. If you want to check for a smaller set, you can explicitly set active_components_only, can't you?

[dseredyn]

active_components_only can be explicitly set only in DistanceRequest. It is not present in generic CollisionRequest, and FCL CollisionData which contains active_components_only_ is created in moveit internals and it is not accessible. I don't understand what is the purpose of "group" arg in planning_scene::PlanningScene::isStateValid ? The docs says: isStateColliding (const std::string &group="", bool verbose=false) Check if the current state is in collision (with the environment or self collision). If a group name is specified, collision checking is done for that group only. Since the function is non-const, the current state transforms are updated before the collision check.

[rhaschke]

active_components_only can be explicitly set only in DistanceRequest.

Looks like you need to resort to this collision checking method then.

I don't understand what is the purpose of "group" arg in planning_scene::PlanningScene::isStateValid

As you figured out, group is used to initialize the set of active components as described in your initial comment.

[simonschmeisser]

Please feel invited to add hints either in doxygen comments or MoveIt tutorials that explain that group always includes all descendant links. You are certainly not the first one stumbling over this. I'm looking forward to your PR and closing this as the code works as intended.