Support including the names of other attached objects in touch_link

Fixes #3275
moveit · Jan 27, 2025 · 230e481 · 230e481
1 parent e7872eb
commit 230e481
Showing 1 changed file with 15 additions and 0 deletions.
diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp
@@ -160,6 +160,21 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
   {
     if (cd1->ptr.ab->getAttachedLink() == cd2->ptr.ab->getAttachedLink())
       always_allow_collision = true;
+    else
+    {
+      const std::set<std::string>& tl1 = cd1->ptr.ab->getTouchLinks();
+      const std::set<std::string>& tl2 = cd2->ptr.ab->getTouchLinks();
+      if (tl1.find(cd2->getID()) != tl1.end() || tl2.find(cd1->getID()) != tl2.end())
+      {
+        always_allow_collision = true;
+      }
+    }
+    if (always_allow_collision && cdata->req_->verbose)
+    {
+      RCLCPP_DEBUG(getLogger(),
+                   "Attached object '%s' is allowed to touch attached object '%s'. No contacts are computed.",
+                   cd2->getID().c_str(), cd1->getID().c_str());
+    }
   }
 
   // if collisions are always allowed, we are done