From 8b3f8950dc3f15ea131cae97cb056d09c629336c Mon Sep 17 00:00:00 2001 From: Aleksey Nogin Date: Fri, 31 Jan 2025 04:24:13 -0800 Subject: [PATCH] Support including the names of other attached objects in `touch_link` (#3276) * Support including the names of other attached objects in `touch_link` Fixes #3275 * Add a comment; add {} for readability --------- Co-authored-by: Sebastian Jahr (cherry picked from commit 75fb28bc3e18e62f33e688c03f44f20614268aa0) --- .../src/collision_common.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index 89ee984e09..ca76202d6a 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -155,11 +155,30 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi } } } + // bodies attached to the same link should not collide + // If one of the attached objects lists the other in touch links set, then collisions are also allowed if (cd1->type == BodyTypes::ROBOT_ATTACHED && cd2->type == BodyTypes::ROBOT_ATTACHED) { if (cd1->ptr.ab->getAttachedLink() == cd2->ptr.ab->getAttachedLink()) + { always_allow_collision = true; + } + else + { + const std::set& tl1 = cd1->ptr.ab->getTouchLinks(); + const std::set& 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