From 32fcd837dd96ec69e845264edc857b0bfdba3394 Mon Sep 17 00:00:00 2001
From: Armar6 <armar6@h2t.com>
Date: Mon, 30 Apr 2018 14:17:15 +0200
Subject: [PATCH] changed collision check for zero joint
---
.../components/units/RobotUnit/RobotUnit.cpp | 13 +++++++++++++
1 file changed, 13 insertions(+)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index f8fd2a54d..82eb2cbc8 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;
--
GitLab