diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp index f8fd2a54d9d2f9c77a0eac036b0a0d7bcf469492..82eb2cbc8966d74b58ad3c73998e63b0e92d3354 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp @@ -2749,6 +2749,19 @@ namespace armarx // update joint values selfCollisionAvoidanceRobotSynchronization->sync(); + float absSum = 0; + VirtualRobot::RobotPtr tmpRobot = selfCollisionAvoidanceData.robot; + for (auto& node : tmpRobot->getRobotNodes()) + { + absSum += std::abs(node->getJointValue()); + } + // only check collision if joint values are not completely zero + if (absSum < 0.00001f) + { + ARMARX_INFO << deactivateSpam() << "All joint values are zero - skipping collision check for now"; + return; + } + // check distances between all collision models std::vector<VirtualRobot::SceneObjectSetPtr> conflictingColModels;