Only check end effector collisions for target pose in ComputeIK

[henningkayser]

Fixes false positive collisions of links/objects unrelated to the (rigidly extended) end effector. This includes collisions between objects or secondary groups and root links caused by the added padding. Common examples for this are collision objects positioned on a table link or another planning group having a self collision.

[codecov-io]

Codecov Report

Merging #117 into master will decrease coverage by 0.02%. The diff coverage is 0.00%.

@@            Coverage Diff             @@
##           master     #117      +/-   ##
==========================================
- Coverage   26.87%   26.84%   -0.03%     
==========================================
  Files          90       90              
  Lines        5752     5758       +6     
==========================================
  Hits         1546     1546              
- Misses       4206     4212       +6     

Impacted Files	Coverage Δ
core/src/stages/compute_ik.cpp	20.00% <0.00%> (-0.62%)	:arrow_down:

---

Continue to review full report at Codecov.

Legend - Click here to learn more Δ = absolute <relative> (impact), ø = not affected, ? = missing data Powered by Codecov. Last update 4fa7066...66b4381. Read the comment docs.

[henningkayser]

Could you please elaborate which issue you want to tackle with this PR? At first sight, it doesn't make sense to me to restrict collision checking to the end-effector. If there are collisions with the arm, the IK solution is not feasible, is it?

Also, there is a much better approach to restrict collision planning to a subset of links: https://github.com/ros-planning/moveit/blob/844a0212865d66541a3d6dc6ac6abef23b0da526/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h#L275-L276

This is just the pre-check for seeing if the target pose itself is in collision (not the IK solution). The actual collision check inside the IK validation callback is not affected and would still detect collisions in the arm, therefore all IK solutions are feasible. The solved issue is simply that ComputeIK detects collisions in static links and secondary groups (i.e. other groups than the ones it should run IK for) only because of added padding. IMO ComputeIK should only "care" for collisions in the groups it was initiated with.

The method you linked is a distance request. I wasn't aware that you could run this without computing the distances.

[rhaschke]

This is just the pre-check for seeing if the target pose itself is in collision (not the IK solution).

I missed this fact. Sorry.

The solved issue is simply that ComputeIK detects collisions in static links and secondary groups (i.e. other groups than the ones it should run IK for) only because of added padding.

Are those unrelated links considered without padding in all remaining checks? In this case, you are right.

But, your implementation is still sub optimal: Instead of first checking all possible collision pairs and subsequently filtering out unrelated ones, you should restrict collision checking to the relevant pairs in first place. Maybe active_components_only cannot be specified (yet) for collision checking (as an old TODO comment above the function suggests). In this case you should (further) modify the ACM to exclude unrelated link pairs.

[mamoll]

@henningkayser, @rhaschke's suggestion of setting up the ACM correctly before the collision check seems reasonable to me. Can you make this change?

[henningkayser]

@rhaschke @mamoll I updated this PR to only modify the ACM instead of checking the colliding links afterwards. Collisions between rigidly connected root links and scene objects are now allowed since these don't affect the IK solution as already discussed above. Also, the modifications are applied to a copied ACM so that IK is run using only the original entries. Correct me if I'm wrong, but didn't the collision check in setFromIK() (original implementation) still allow the disabled collisions for all group links since it's using the same sandbox_scene with the modified ACM? I didn't run a test to validate this, but looking at the code this seems to be the case and this PR would fix this.

[henningkayser]

I just saw that this still doesn't cover all cases. If the colliding links are outside of the kinematic chain then the pre-check will still collide with other collision objects. My suggestion now is to really only check for collisions in the eef links in the pre-check and disable all others.

[henningkayser]

@mamoll @rhaschke please re-review. I don't think there is really a better way to fix this here but this fully covers all edge cases, including disabling collisions between static links outside of kinematic chain and scene objects while still checking for self-collisions.

[henningkayser]

@rhaschke ping

[gautz]

@rhaschke @henningkayser My end-effector is randomly colliding with some remote environment link. This is not happening with a MoveTo stage. I am guessing the error comes from the issue explained here. Any plan to merge this? Or was it solve already elsewhere?