diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index c321e9d1e..c24a18a1d 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -151,6 +151,17 @@ 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()); }