From 737c8310d52af6f2ddc1ec5c7303f3505f7d56c4 Mon Sep 17 00:00:00 2001
From: Simon <sjansen@smart-robotics.com>
Date: Mon, 16 Nov 2015 12:52:15 +0100
Subject: [PATCH] Catch NaN error if angles are zero

---
 src/ur_ros_wrapper.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp
index 09a9526..24d805e 100644
--- a/src/ur_ros_wrapper.cpp
+++ b/src/ur_ros_wrapper.cpp
@@ -602,7 +602,11 @@ private:
             double ry = tool_vector_actual[4];
             double rz = tool_vector_actual[5];
             double angle = std::sqrt(std::pow(rx,2) + std::pow(ry,2) + std::pow(rz,2));
-            quat.setRotation(tf::Vector3(rx/angle, ry/angle, rz/angle), angle);
+            if (angle < 1e-16) {
+                quat.setValue(0, 0, 0, 1);
+            } else {
+                quat.setRotation(tf::Vector3(rx/angle, ry/angle, rz/angle), angle);
+            }
 
             //Create and broadcast transform
             tf::Transform transform;