diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index a4d9867488a2a2f8e9cb176158ba18fc01a8c538..2423d3b13204734a7ff3921ea995d8ceb0bf4df6 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -21,12 +21,16 @@
  */
 
 #include "RobotUnitModuleSelfCollisionChecker.h"
+#include <algorithm>
+#include <cstddef>
+#include <string>
 
 #include <SimoxUtility/algorithm/string/string_tools.h>
 #include <VirtualRobot/CollisionDetection/CollisionChecker.h>
 #include <VirtualRobot/Obstacle.h>
 #include <VirtualRobot/RobotNodeSet.h>
 
+#include "ArmarXCore/core/logging/Logging.h"
 #include <ArmarXCore/core/util/OnScopeExit.h>
 
 #include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
@@ -219,6 +223,58 @@ namespace armarx::RobotUnitModule
                 node->getCollisionModel()->inflateModel(minSelfDistance / 2.f);
             }
         }
+
+        // Remove / filter collision pairs according to robot model (XML: Physics/IgnoreCollision)
+        {
+            ARMARX_VERBOSE << "Removing ignored collision pairs";
+            // introduce vector to remove elements "in-place" via remove-erase-if idiom (not possible for sets)
+            std::vector<std::pair<std::string, std::string>> validNamePairsToCheck(namePairsToCheck.begin(), namePairsToCheck.end());
+
+            const auto isCollisionIgnored = [this](const std::string& a, const std::string& b) -> bool {
+
+                if(a == FLOOR_OBJ_STR or b == FLOOR_OBJ_STR)
+                {
+                    return false;
+                }
+
+                const auto nodeA = selfCollisionAvoidanceRobot->getRobotNode(a);
+                const auto nodeB = selfCollisionAvoidanceRobot->getRobotNode(b);
+
+                if(nodeA == nullptr or nodeB == nullptr)
+                {
+                    return false;
+                }
+
+                const std::vector<std::string> nodesIgnoredByA = nodeA->getIgnoredCollisionModels();
+                const std::vector<std::string> nodesIgnoredByB = nodeB->getIgnoredCollisionModels();
+
+                if(std::find(nodesIgnoredByA.begin(), nodesIgnoredByA.end(), b) != nodesIgnoredByA.end())
+                {
+                    ARMARX_VERBOSE << "Ignoring collision between nodes: " << a << " -- " << b;
+                    return true;
+                }
+
+                if(std::find(nodesIgnoredByB.begin(), nodesIgnoredByB.end(), a) != nodesIgnoredByB.end())
+                {
+                    ARMARX_VERBOSE << "Ignoring collision between nodes: " << b << " -- " << a;
+                    return true;
+                }
+
+                return false;
+
+            };
+
+            validNamePairsToCheck.erase(std::remove_if(validNamePairsToCheck.begin(), validNamePairsToCheck.end(), [&isCollisionIgnored](const auto& p) -> bool {
+                const auto& [a, b] = p;
+                return isCollisionIgnored(a, b);
+            }), validNamePairsToCheck.end());
+
+            ARMARX_VERBOSE << "Removed " << (namePairsToCheck.size() - validNamePairsToCheck.size()) << " collision pairs.";
+
+            // copy over name pairs which should not be ignored
+            namePairsToCheck = std::set(validNamePairsToCheck.begin(), validNamePairsToCheck.end());
+        }
+
         //collect pairs
         for (const auto& pair : namePairsToCheck)
         {
@@ -227,14 +283,18 @@ namespace armarx::RobotUnitModule
                     ? floor->getSceneObject(0)
                     : selfCollisionAvoidanceRobot->getRobotNode(pair.first);
 
+            ARMARX_CHECK_NOT_NULL(first) << pair.first;
+
             VirtualRobot::SceneObjectPtr second =
                 (pair.second == FLOOR_OBJ_STR)
                     ? floor->getSceneObject(0)
                     : selfCollisionAvoidanceRobot->getRobotNode(pair.second);
 
+            ARMARX_CHECK_NOT_NULL(second) << pair.second;
+
             nodePairsToCheck.emplace_back(first, second);
         }
-        ARMARX_CHECK_EQUAL(nodePairsToCheck.size(), nodePairsToCheck.size());
+        ARMARX_CHECK_EQUAL(nodePairsToCheck.size(), namePairsToCheck.size());
     }
 
     void
@@ -332,6 +392,7 @@ namespace armarx::RobotUnitModule
                     bool allJoints0 = true;
                     for (const auto& node : selfCollisionAvoidanceRobotNodes)
                     {
+                        ARMARX_CHECK_NOT_NULL(node);
                         if (0 != node->getJointValue())
                         {
                             allJoints0 = false;
@@ -348,6 +409,10 @@ namespace armarx::RobotUnitModule
                 for (std::size_t idx = 0; idx < nodePairsToCheck.size(); ++idx)
                 {
                     const auto& pair = nodePairsToCheck.at(idx);
+
+                    ARMARX_CHECK_NOT_NULL(pair.first);
+                    ARMARX_CHECK_NOT_NULL(pair.second);
+
                     if (selfCollisionAvoidanceRobot->getCollisionChecker()->checkCollision(
                             pair.first, pair.second))
                     {