diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index 3a5389b35..4da436032 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -153,17 +153,6 @@ std::pair ModifyPlanningScene::apply(const Interf if (callback_) callback_(scene, properties()); - - // check for collisions - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - req.contacts = true; - req.max_contacts = 1; - scene->checkCollision(req, res); - if (res.collision) { - const auto contact = res.contacts.begin()->second.front(); - traj.markAsFailure(contact.body_name_1 + " colliding with " + contact.body_name_2); - } } catch (const std::exception& e) { traj.markAsFailure(e.what()); }