From b9fe4f38d59315b0fa28c7ef2ff42cd221229d3f Mon Sep 17 00:00:00 2001
From: Raphael Grimm <raphael.grimm@kit.edu>
Date: Fri, 27 Jul 2018 14:14:47 +0200
Subject: [PATCH] Avoid collision checking if all joint values are 0
---
.../RobotUnitModuleSelfCollisionChecker.cpp | 17 +++++++++++++++++
1 file changed, 17 insertions(+)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index 96b313836..63886c9b2 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -285,6 +285,23 @@ namespace armarx
//update robot
_module<ControlThreadDataBuffer>().updateVirtualRobot(selfCollisionAvoidanceRobot, selfCollisionAvoidanceRobotNodes);
+ //check for all nodes 0
+ {
+ bool allJoints0 = true;
+ for(const VirtualRobot::RobotNodePtr node : selfCollisionAvoidanceRobotNodes)
+ {
+ if(0 != node->getJointValue())
+ {
+ allJoints0 = false;
+ break;
+ }
+ }
+ if(allJoints0)
+ {
+ continue;
+ }
+ }
+
bool collision = false;
for (std::size_t idx = 0; idx < nodePairsToCheck.size(); ++idx)
{
--
GitLab