From ad83b7f14555baa6335712ab404eacef2bd6cd93 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 18 Dec 2018 15:45:49 +0100 Subject: [PATCH] configurable IK unit tests --- moveit_kinematics/package.xml | 2 + moveit_kinematics/test/CMakeLists.txt | 3 +- moveit_kinematics/test/fanuc.test | 15 +++ .../test/test_kinematics_plugin.cpp | 110 +++++++++++++++++- 4 files changed, 125 insertions(+), 5 deletions(-) diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index 9ae9d15edfa..895cade7445 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -37,6 +37,8 @@ tf2_kdl orocos_kdl + xmlrpcpp + diff --git a/moveit_kinematics/test/CMakeLists.txt b/moveit_kinematics/test/CMakeLists.txt index 1171586e3b8..37e79ba23f9 100644 --- a/moveit_kinematics/test/CMakeLists.txt +++ b/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() diff --git a/moveit_kinematics/test/fanuc.test b/moveit_kinematics/test/fanuc.test index 84a859e8191..e5058c7de0e 100644 --- a/moveit_kinematics/test/fanuc.test +++ b/moveit_kinematics/test/fanuc.test @@ -14,6 +14,21 @@ [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] + +- 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] + diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp index 2d3d1241e5c..f337f3c612a 100644 --- a/moveit_kinematics/test/test_kinematics_plugin.cpp +++ b/moveit_kinematics/test/test_kinematics_plugin.cpp @@ -41,6 +41,7 @@ #include #include #include +#include // MoveIt! #include @@ -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(v); + else if (v.getType() == XmlRpc::XmlRpcValue::TypeInt) + return static_cast(v); + else + return 0.0; +} +static void parseVector(XmlRpc::XmlRpcValue& vec, std::vector& 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 seed, sol; + const std::vector& 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 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& 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 reached_poses; + kinematics_solver_->getPositionFK(tip_frames, sol, reached_poses); + expectNear({goal}, reached_poses); + + Eigen::Map solution(sol.data(), sol.size()); + Eigen::Map 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 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 seed, fk_values, solution;