Fix issue moveit#258

Levi-Armstrong · Nov 25, 2015 · fba10bb · fba10bb
1 parent 3c4c7a6
commit fba10bb
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/collision_detection_fcl/src/collision_common.cpp b/collision_detection_fcl/src/collision_common.cpp
@@ -364,6 +364,10 @@ bool distanceCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void*
   const CollisionGeometryData* cd1 = static_cast<const CollisionGeometryData*>(o1->collisionGeometry()->getUserData());
   const CollisionGeometryData* cd2 = static_cast<const CollisionGeometryData*>(o2->collisionGeometry()->getUserData());
 
+  // do not perform distance calculation for geoms part of the same object / link / attached body
+  if (cd1->sameObject(*cd2))
+    return false;
+
   // If active components are specified
   if (cdata->active_components_only_)
   {