diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index e153f092b538589a8ca72ae97ae16a596c4e97d2..072aeafd23e67e9d00a3662c4385104d43d3c6ee 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -715,7 +715,7 @@ namespace armarx
         publishNewSensorDataTime = TimeUtil::GetTime();
         publisherTask = new PublisherTaskT([&] {publish({});}, pubtimestep, false, getName() + "_PublisherTask");
         ARMARX_INFO << "starting publisher with timestep " << pubtimestep;
-        publisherTask->setDelayWarningTolerance(15);
+        publisherTask->setDelayWarningTolerance(100);
         publisherTask->start();
         ARMARX_INFO << "finishUnitInitialization...done! " << (MicroNow() - beg).count() << " us";
     }
@@ -2324,20 +2324,29 @@ namespace armarx
 
             selfCollisionAvoidanceData.modelPairs.push_back(std::make_pair(first, second));
 
-            if (colModels.find(first) == colModels.end() && first != "FLOOR")
+            auto inflateNodeSet = [&](VirtualRobot::RobotNodeSetPtr nodeset)
             {
-                for (size_t i = 0; i < selfCollisionAvoidanceData.robot->getRobotNodeSet(first)->getSize(); ++i)
+                for (int i = 0; i < static_cast<int>(nodeset->getSize()); ++i)
                 {
-                    selfCollisionAvoidanceData.robot->getRobotNodeSet(first)->getNode(i)->getCollisionModel()->inflateModel(selfCollisionAvoidanceData.minDistance / 2.f);
+                    if (nodeset->getNode(i)->getCollisionModel())
+                    {
+                        nodeset->getNode(i)->getCollisionModel()->inflateModel(selfCollisionAvoidanceData.minDistance / 2.f);
+                    }
+                    else
+                    {
+                        ARMARX_WARNING << "Self Collision Avoidance: No collision model found for '" << nodeset->getNode(i)->getName() << "'";
+                    }
                 }
+            };
+
+            if (colModels.find(first) == colModels.end() && first != "FLOOR")
+            {
+                inflateNodeSet(selfCollisionAvoidanceData.robot->getRobotNodeSet(first));
                 colModels[first] = selfCollisionAvoidanceData.robot->getRobotNodeSet(first);
             }
             if (colModels.find(second) == colModels.end() && second != "FLOOR")
             {
-                for (size_t i = 0; i < selfCollisionAvoidanceData.robot->getRobotNodeSet(second)->getSize(); ++i)
-                {
-                    selfCollisionAvoidanceData.robot->getRobotNodeSet(second)->getNode(i)->getCollisionModel()->inflateModel(selfCollisionAvoidanceData.minDistance / 2.f);
-                }
+                inflateNodeSet(selfCollisionAvoidanceData.robot->getRobotNodeSet(second));
                 colModels[second] = selfCollisionAvoidanceData.robot->getRobotNodeSet(second);
             }
         }
@@ -2361,6 +2370,7 @@ namespace armarx
         int colChecksPerSecond = getProperty<int>("SelfCollisionChecksPerSecond").getValue();
         int intervalMs = (1.0 / (float) colChecksPerSecond) * 1000;
         selfCollisionAvoidanceTask = new PeriodicTask<RobotUnit>(this, &RobotUnit::updateSelfCollisionAvoidance, intervalMs, false, "SelfCollisionAvoidance");
+        selfCollisionAvoidanceTask->setDelayWarningTolerance(200);
         selfCollisionAvoidanceTask->start();
 
         // logging
@@ -2472,10 +2482,10 @@ namespace armarx
         else
         {
             // no collision
-            ARMARX_VERBOSE << deactivateSpam(1) << "Self Distance Check: OK." <<
-                           "\nmin. allowed distance: " << selfCollisionAvoidanceData.minDistance << " mm" <<
-                           "\n" << distancesString <<
-                           "\nComputation time: " << (duration.count() * 1000) << " ms (max. allowed duration (as specified): " << (1000.0 / getProperty<int>("SelfCollisionChecksPerSecond").getValue()) << " ms)";
+            ARMARX_DEBUG << deactivateSpam(5) << "Self Distance Check: OK." <<
+                         "\nmin. allowed distance: " << selfCollisionAvoidanceData.minDistance << " mm" <<
+                         "\n" << distancesString <<
+                         "\nComputation time: " << (duration.count() * 1000) << " ms (max. allowed duration (as specified): " << (1000.0 / getProperty<int>("SelfCollisionChecksPerSecond").getValue()) << " ms)";
         }
 
     }