Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 10:00:48 +02:00
Code Releases Activity

Catch NaN error if angles are zero

Browse Source
This commit is contained in:
Simon
2015-11-16 12:52:15 +01:00
parent d24019b0d1
commit 737c8310d5
1 changed files with 5 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/ur_ros_wrapper.cpp
View File

@@ -602,7 +602,11 @@ private:
double ry = tool_vector_actual[4]; double ry = tool_vector_actual[4];
double rz = tool_vector_actual[5]; double rz = tool_vector_actual[5];
double angle = std::sqrt(std::pow(rx,2) + std::pow(ry,2) + std::pow(rz,2)); double angle = std::sqrt(std::pow(rx,2) + std::pow(ry,2) + std::pow(rz,2));
if (angle < 1e-16) {
quat.setValue(0, 0, 0, 1);
} else {
quat.setRotation(tf::Vector3(rx/angle, ry/angle, rz/angle), angle); quat.setRotation(tf::Vector3(rx/angle, ry/angle, rz/angle), angle);
}
//Create and broadcast transform //Create and broadcast transform
tf::Transform transform; tf::Transform transform;