configurable IK unit tests

moveit_kinematics/package.xml

@@ -37,6 +37,8 @@
   <run_depend>tf2_kdl</run_depend>
   <run_depend>orocos_kdl</run_depend>
 
+  <test_depend>xmlrpcpp</test_depend>
+
   <export>
     <moveit_core plugin="${prefix}/kdl_kinematics_plugin_description.xml"/>
     <moveit_core plugin="${prefix}/lma_kinematics_plugin_description.xml"/>

moveit_kinematics/test/CMakeLists.txt

@@ -1,8 +1,9 @@
 if(CATKIN_ENABLE_TESTING)
   find_package(moveit_resources REQUIRED)
   find_package(rostest REQUIRED)
+  find_package(xmlrpcpp REQUIRED)
 
   add_rostest_gtest(test_kinematics_plugin all.test test_kinematics_plugin.cpp)
-  target_link_libraries(test_kinematics_plugin ${catkin_LIBRARIES})
+  target_link_libraries(test_kinematics_plugin ${catkin_LIBRARIES} ${xmlrpcpp_LIBRARIES})
   target_compile_options(test_kinematics_plugin PRIVATE -Wno-deprecated-declarations)
 endif()

moveit_kinematics/test/fanuc.test

@@ -14,6 +14,21 @@
 		<param name="num_ik_multiple_tests" value="20"/>
 		<param name="num_nearest_ik_tests" value="0"/>
 		<rosparam param="joint_names">[joint_1, joint_2, joint_3, joint_4, joint_5, joint_6]</rosparam>
+		<rosparam param="unit_tests">
+- pos.x: 0.1
+  joints: [0, 0.172255, 0.18616, 0, -0.013905, 0]
+- pos.y: 0.1
+  joints: [0.12591131286587795, 1.3485404839133919, 2.5252600125707478, 3.277831501700081, 1.1800087922500875, -3.1937639355821816]
+- pos.z: 0.1
+  joints: [0, 0.068450807289788598, 0.23178044027618283, 0, -0.16332963298639425, 0]
+
+- rot.x: 0.1
+  joints: [-0.012628482860616965, 1.3494014096738791, 2.5348846732901564, 3.0201372000379805, 1.1880516746752559, -3.096040098704433]
+- rot.y: 0.1
+  joints: [0, -0.0042227733863642609, -0.019776669463346962, 0, 0.11555389607698259, 0]
+- rot.z: 0.1
+  joints: [0, 0, 0, -0.05, 0, -0.05]
+</rosparam>
 
 		<test test-name="fanuc_kdl" pkg="moveit_kinematics" type="test_kinematics_plugin" time-limit="180">
 			<param name="ik_plugin_name" value="kdl_kinematics_plugin/KDLKinematicsPlugin"/>

moveit_kinematics/test/test_kinematics_plugin.cpp

@@ -41,6 +41,7 @@
 #include <pluginlib/class_loader.h>
 #include <ros/ros.h>
 #include <tf2_eigen/tf2_eigen.h>
+#include <xmlrpcpp/XmlRpcValue.h>
 
 // MoveIt!
 #include <moveit/kinematics_base/kinematics_base.h>
@@ -199,10 +200,11 @@ class KinematicsTest : public ::testing::Test
       EXPECT_NEAR(first[i].position.x, second[i].position.x, near);
       EXPECT_NEAR(first[i].position.y, second[i].position.y, near);
       EXPECT_NEAR(first[i].position.z, second[i].position.z, near);
-      EXPECT_NEAR(first[i].orientation.x, second[i].orientation.x, near);
-      EXPECT_NEAR(first[i].orientation.y, second[i].orientation.y, near);
-      EXPECT_NEAR(first[i].orientation.z, second[i].orientation.z, near);
-      EXPECT_NEAR(first[i].orientation.w, second[i].orientation.w, near);
+      Eigen::Isometry3d eig_first, eig_second;
+      tf2::fromMsg(first[i], eig_first);
+      tf2::fromMsg(second[i], eig_second);
+      Eigen::AngleAxisd diff_rotation(eig_first.linear() * eig_second.linear().transpose());
+      EXPECT_NEAR(diff_rotation.angle(), 0.0, near);
     }
   }
 
@@ -333,6 +335,106 @@ TEST_F(KinematicsTest, randomWalkIK)
 #endif
 }
 
+static double parseDouble(XmlRpc::XmlRpcValue& v)
+{
+  if (v.getType() == XmlRpc::XmlRpcValue::TypeDouble)
+    return static_cast<double>(v);
+  else if (v.getType() == XmlRpc::XmlRpcValue::TypeInt)
+    return static_cast<int>(v);
+  else
+    return 0.0;
+}
+static void parseVector(XmlRpc::XmlRpcValue& vec, std::vector<double>& values)
+{
+  values.reserve(vec.size());
+  for (size_t i = 0; i < vec.size(); ++i)
+    values.push_back(parseDouble(vec[i]));
+}
+
+TEST_F(KinematicsTest, unitIK)
+{
+  XmlRpc::XmlRpcValue tests;
+  if (!getParam("unit_tests", tests))
+    return;
+
+  std::vector<double> seed, sol;
+  const std::vector<std::string>& tip_frames = kinematics_solver_->getTipFrames();
+  robot_state::RobotState robot_state(robot_model_);
+  // compute initial pose
+  robot_state.setToDefaultValues();
+  robot_state.copyJointGroupPositions(jmg_, seed);
+  std::vector<geometry_msgs::Pose> poses;
+  ASSERT_TRUE(kinematics_solver_->getPositionFK(tip_frames, seed, poses));
+  Eigen::Isometry3d initial, goal;
+  tf2::fromMsg(poses[0], initial);
+
+  auto f = [&](const geometry_msgs::Pose& goal, std::vector<double>& truth)
+  {
+    // compute IK
+    moveit_msgs::MoveItErrorCodes error_code;
+    kinematics_solver_->getPositionIK(goal, seed, sol, error_code);
+    EXPECT_EQ(error_code.val, error_code.SUCCESS);
+
+    // validate reached poses
+    std::vector<geometry_msgs::Pose> reached_poses;
+    kinematics_solver_->getPositionFK(tip_frames, sol, reached_poses);
+    expectNear({goal}, reached_poses);
+
+    Eigen::Map<Eigen::ArrayXd> solution(sol.data(), sol.size());
+    Eigen::Map<Eigen::ArrayXd> ground_truth(truth.data(), truth.size());
+    EXPECT_TRUE(solution.isApprox(ground_truth, IK_NEAR)) << solution.transpose() << std::endl << ground_truth.transpose() << std::endl;
+  };
+
+  ASSERT_EQ(tests.getType(), XmlRpc::XmlRpcValue::TypeArray);
+  std::vector<double> ground_truth;
+
+  /* process tests definitions on parameter server of the form
+     - pos.x: +0.1
+       joints: [0, 0, 0, 0, 0, 0]
+     - pos.y: -0.1
+       joints: [0, 0, 0, 0, 0, 0]
+  */
+  for (size_t i = 0; i < tests.size(); ++i)
+  {
+    goal = initial;  // reset goal to initial
+    ground_truth.clear();
+
+    ASSERT_EQ(tests[i].getType(), XmlRpc::XmlRpcValue::TypeStruct);
+    std::string desc;
+    for (auto& member : tests[i])
+    {
+      for (unsigned char axis = 0; axis < 3; ++axis)
+      {
+        char axis_char = 'x' + axis;
+        // position offset
+        if (member.first == (std::string("pos.") + axis_char))
+        {
+          goal.translation()[axis] += parseDouble(member.second);
+          break;
+        }
+        // rotation offset
+        else if (member.first == (std::string("rot.") + axis_char))
+        {
+          goal *= Eigen::AngleAxisd(parseDouble(member.second), Eigen::Vector3d::Unit(axis));
+          break;
+        }
+      }
+      if (member.first == "joints")
+      {
+        ASSERT_EQ(member.second.getType(), XmlRpc::XmlRpcValue::TypeArray);
+        parseVector(member.second, ground_truth);
+      }
+      else
+        desc += member.first + " ";
+    }
+    ASSERT_EQ(ground_truth.size(), seed.size()) << "Expecting joint array in element \"joints\"";
+    {
+      SCOPED_TRACE(desc);
+      f(tf2::toMsg(goal), ground_truth);
+    }
+  }
+}
+
 TEST_F(KinematicsTest, searchIK)
 {
   std::vector<double> seed, fk_values, solution;