diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 32f75b9e5..9e7939c03 100644 --- a/core/src/stages/compute_ik.cpp +++ b/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 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;