Only check end effector collisions for target pose in ComputeIK

Author: henningkayser

core/src/stages/compute_ik.cpp

@@ -104,7 +104,7 @@ bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, Eigen:
 	robot_state.updateCollisionBodyTransforms();
 
 	// disable collision checking for parent links (except links fixed to root)
-	auto& acm = scene->getAllowedCollisionMatrixNonConst();
+	collision_detection::AllowedCollisionMatrix acm(scene->getAllowedCollisionMatrix());
 	std::vector<const std::string*> pending_links;  // parent link names that might be rigidly connected to root
 	while (parent) {
 		pending_links.push_back(&parent->getName());
@@ -119,6 +119,15 @@ bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, Eigen:
 		}
 	}
 
+	// Remaining pending_links are fixed to root, disable padded scene collisions for this check
+	if (!pending_links.empty()) {
+		for (const std::string& object_id : scene->getWorld()->getObjectIds()) {
+			for (const std::string* name : pending_links) {
+				acm.setEntry(object_id, *name, true);
+			}
+		}
+	}
+
 	// check collision with the world using the padded version
 	collision_detection::CollisionRequest req;
 	collision_detection::CollisionResult result;