From 8b00500e56493cb5b0f5db4a224f5ad708aac162 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?S=C3=A9bastien=20Barth=C3=A9l=C3=A9my?=
 <sbarthelemy@aldebaran-robotics.com>
Date: Wed, 1 Oct 2014 20:38:58 +0200
Subject: [PATCH] add urdf::Rotation unit test

demonstrating numerical issues with the quaternion/rpy conversion (#49)
---
 CMakeLists.txt                      |  2 +-
 urdf_parser/CMakeLists.txt          |  2 +
 urdf_parser/test/urdf_unit_test.cpp | 65 +++++++++++++++++++++++++++++
 3 files changed, 68 insertions(+), 1 deletion(-)
 create mode 100644 urdf_parser/test/urdf_unit_test.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2ed60b87..d31b645d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -46,7 +46,7 @@ find_package(console_bridge REQUIRED)
 include_directories(SYSTEM ${console_bridge_INCLUDE_DIRS})
 link_directories(${console_bridge_LIBRARY_DIRS})
 
-find_package(Boost REQUIRED system thread)
+find_package(Boost REQUIRED system thread unit_test_framework)
 include_directories(${Boost_INCLUDE_DIR})
 link_directories(${Boost_LIBRARY_DIRS})
 
diff --git a/urdf_parser/CMakeLists.txt b/urdf_parser/CMakeLists.txt
index 7ce2429d..0e91d7b0 100644
--- a/urdf_parser/CMakeLists.txt
+++ b/urdf_parser/CMakeLists.txt
@@ -27,6 +27,8 @@ target_link_libraries(urdf_to_graphiz urdfdom_model)
 add_executable(urdf_mem_test test/memtest.cpp)
 target_link_libraries(urdf_mem_test urdfdom_model)
 
+add_executable(urdf_unit_test test/urdf_unit_test.cpp)
+target_link_libraries(urdf_unit_test urdfdom_model ${Boost_LIBRARIES})
 
 INSTALL(TARGETS urdfdom_model DESTINATION ${CMAKE_INSTALL_LIBDIR})
 INSTALL(TARGETS urdfdom_world DESTINATION ${CMAKE_INSTALL_LIBDIR})
diff --git a/urdf_parser/test/urdf_unit_test.cpp b/urdf_parser/test/urdf_unit_test.cpp
new file mode 100644
index 00000000..f826b4c7
--- /dev/null
+++ b/urdf_parser/test/urdf_unit_test.cpp
@@ -0,0 +1,65 @@
+#include "urdf_parser/urdf_parser.h"
+#include <iostream>
+#include <iomanip>
+#include <cmath>
+
+// the name of our test module
+#define BOOST_TEST_MODULE URDF_UNIT_TEST
+// needed for automatic generation of the main()
+#define BOOST_TEST_DYN_LINK
+#include <boost/test/unit_test.hpp>
+
+
+bool quat_are_near(urdf::Rotation left, urdf::Rotation right)
+{
+  static const double epsilon = 1e-3; // quite loose epsilon
+  double l[4], r[4];
+  left.getQuaternion(l[0], l[1], l[2], l[3]);
+  right.getQuaternion(r[0], r[1], r[2], r[3]);
+  return (std::abs(l[0] - r[0]) < epsilon &&
+          std::abs(l[1] - r[1]) < epsilon &&
+          std::abs(l[2] - r[2]) < epsilon &&
+          std::abs(l[3] - r[3]) < epsilon) ||
+         (std::abs(l[0] + r[0]) < epsilon &&
+          std::abs(l[1] + r[1]) < epsilon &&
+          std::abs(l[2] + r[2]) < epsilon &&
+          std::abs(l[3] + r[3]) < epsilon);
+}
+
+std::ostream &operator<<(std::ostream &os, const urdf::Rotation& rot)
+{
+  double roll, pitch, yaw;
+  double x, y, z, w;
+  rot.getRPY(roll, pitch, yaw);
+  rot.getQuaternion(x, y, z, w);
+  os << std::setprecision(9)
+     << "x: " << x << " y: " << y << " z: " << z << " w: " <<  w
+     << "  roll: "  << roll << " pitch: " << pitch << " yaw: "<< yaw;
+  return os;
+}
+
+
+void check_get_set_rpy_is_idempotent(double x, double y, double z, double w)
+{
+  urdf::Rotation rot0;
+  rot0.setFromQuaternion(x, y, z, w);
+  double roll, pitch, yaw;
+  rot0.getRPY(roll, pitch, yaw);
+  urdf::Rotation rot1;
+  rot1.setFromRPY(roll, pitch, yaw);
+  if (true) {
+    std::cout << "\n"
+              << "before  " << rot0 << "\n"
+              << "after   " << rot1 << "\n"
+              << "ok      " << quat_are_near(rot0, rot1) << "\n";
+  }
+  BOOST_CHECK(quat_are_near(rot0, rot1));
+}
+
+BOOST_AUTO_TEST_CASE(test_rotation_get_set_rpy_idempotent)
+{
+  double x0 = 0.5, y0 = -0.5, z0 = 0.5,  w0 = 0.5;
+  check_get_set_rpy_is_idempotent(x0, y0, z0, w0);
+  double delta = 2.2e-8;
+  check_get_set_rpy_is_idempotent(x0, y0, z0+delta, w0-delta);
+}