From 792cdc3a08c07eff922e6f63ae052290285c38c8 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Mon, 11 Mar 2019 22:15:49 +0100 Subject: [PATCH] added unit tests for calibration --- CMakeLists.txt | 17 +++-- include/ur_rtde_driver/calibration.h | 12 +-- package.xml | 2 + src/calibration.cpp | 46 ++--------- src/calibration_correction.cpp | 21 +++++- test/calibration_test.cpp | 109 +++++++++++++++++++++++++++ 6 files changed, 154 insertions(+), 53 deletions(-) create mode 100644 test/calibration_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e083bf6..ebd26ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -143,12 +143,19 @@ install(DIRECTORY include/${PROJECT_NAME}/ FILES_MATCHING PATTERN "*.h" ) +############# +## Testing ## +############# -#if (CATKIN_ENABLE_TESTING) - #set(${PROJECT_NAME}_TEST_SOURCES - #tests/ur/master_board.cpp - #tests/ur/robot_mode.cpp - #tests/ur/rt_state.cpp) +if (CATKIN_ENABLE_TESTING) + catkin_add_gtest(calibration_test + test/calibration_test.cpp + src/calibration.cpp) + target_link_libraries(calibration_test ${catkin_LIBRARIES}) +endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) #catkin_add_gtest(ur_rtde_driver_test ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_TEST_SOURCES} tests/main.cpp) #target_link_libraries(ur_rtde_driver_test ur_rtde_driver ${catkin_LIBRARIES}) diff --git a/include/ur_rtde_driver/calibration.h b/include/ur_rtde_driver/calibration.h index ef99cd4..7068515 100644 --- a/include/ur_rtde_driver/calibration.h +++ b/include/ur_rtde_driver/calibration.h @@ -134,7 +134,7 @@ struct DHRobot class Calibration { public: - Calibration(); + Calibration(const DHRobot& robot); virtual ~Calibration(); /*! @@ -148,6 +148,11 @@ public: */ static void debugChain(const KDL::Chain& chain); + /*! + * \brief Creates a \p KDL::Chain representation of the member \p robot_parameters_ + */ + KDL::Chain getChain(); + private: /*! * \brief Splits the given chain \p in into a subchain before \p split_index and after. @@ -170,11 +175,6 @@ private: */ static DHRobot chainToDH(const KDL::Chain& chain); - /*! - * \brief Creates a \p KDL::Chain representation of the member \p robot_parameters_ - */ - KDL::Chain getChain(); - /*! * \brief Modifies the robot chain at segment \p correction_index * diff --git a/package.xml b/package.xml index a3255a2..f1e6b57 100644 --- a/package.xml +++ b/package.xml @@ -36,6 +36,8 @@ tf_conversions std_srvs + gtest + force_torque_sensor_controller joint_state_controller joint_trajectory_controller diff --git a/src/calibration.cpp b/src/calibration.cpp index f215bf3..d690484 100644 --- a/src/calibration.cpp +++ b/src/calibration.cpp @@ -16,43 +16,8 @@ #include -Calibration::Calibration() +Calibration::Calibration(const DHRobot& robot_parameters) : robot_parameters_(robot_parameters) { - // This is the data of our ids-ur10-2 Later, dh parameters will be given using another - // constructor. - - DHRobot my_robot; - my_robot.segments_.push_back(DHSegment(0.1273, 0, 0, M_PI / 2)); - my_robot.segments_.push_back(DHSegment(0, -0.612, 0, 0)); - my_robot.segments_.push_back(DHSegment(0, -0.5723, 0, 0.0)); - my_robot.segments_.push_back(DHSegment(0.163841, 0, 0.0, M_PI / 2)); - my_robot.segments_.push_back(DHSegment(0.1157, 0, 0, -M_PI / 2)); - my_robot.segments_.push_back(DHSegment(0.0922, 0, 0, 0)); - // my_robot.segments_.push_back(DHSegment(0.1, 0.2, 0.3, 0.4)); - - // DHRobot my_robot_calibration; - //// clang-format off - //// d, a, theta, alpha - // my_robot_calibration.segments_.push_back(DHSegment(0 ,0 ,0 ,0 )); - // my_robot_calibration.segments_.push_back(DHSegment(1.5 ,0 ,0.0 ,0.0 )); - // my_robot_calibration.segments_.push_back(DHSegment(0.5 ,0.0 ,0.2 ,0.0 )); - // my_robot_calibration.segments_.push_back(DHSegment(-2 ,0 ,0 ,0 )); - // my_robot_calibration.segments_.push_back(DHSegment(0 ,0 ,0 ,0 )); - // my_robot_calibration.segments_.push_back(DHSegment(0 ,0 ,0 ,0 )); - //// clang-format on - - DHRobot my_robot_calibration; - // clang-format off - // d, a, theta, alpha - my_robot_calibration.segments_.push_back(DHSegment(0.00065609212979853 ,4.6311376834935676e-05 ,-7.290070070824746e-05 ,0.000211987863869334 )); - my_robot_calibration.segments_.push_back(DHSegment(1.4442162376284788 ,-0.00012568315331862312 ,-0.01713897289704999 ,-0.0072553625957652995)); - my_robot_calibration.segments_.push_back(DHSegment(0.854147723854608 ,0.00186216581161458 ,-0.03707159413492756 ,-0.013483226769541364 )); - my_robot_calibration.segments_.push_back(DHSegment(-2.2989425877563705 ,9.918593870679266e-05 ,0.054279462160583214 ,0.0013495820227329425 )); - my_robot_calibration.segments_.push_back(DHSegment(-1.573498686836816e-05 ,4.215462720453189e-06 ,1.488984257025741e-07 ,-0.001263136163679901 )); - my_robot_calibration.segments_.push_back(DHSegment(1.9072435590711256e-05 ,0 ,1.551499479707493e-05 ,0 )); - // clang-format on - - robot_parameters_ = my_robot + my_robot_calibration; } Calibration::~Calibration() @@ -177,8 +142,8 @@ KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain) return buildCorrectedChain(robot_chain, new_length, new_theta, distance_correction); } -KDL::Chain Calibration::buildCorrectedChain(const KDL::Chain& robot_chain, const double new_length, const double new_theta, - const double distance_correction) +KDL::Chain Calibration::buildCorrectedChain(const KDL::Chain& robot_chain, const double new_length, + const double new_theta, const double distance_correction) { KDL::Chain corrected_chain; @@ -217,7 +182,6 @@ KDL::Chain Calibration::buildCorrectedChain(const KDL::Chain& robot_chain, const new_frame.p = robot_chain.getSegment(2).getFrameToTip().p; ROS_INFO_STREAM("Correcting segment i+2 length from " << new_frame.p.z()); - // the d-parameter can be modified by the intersection_parameter which is the distance traveled // along the rotation axis new_frame.p = KDL::Vector(0, 0, new_frame.p.z() - distance_correction); @@ -310,6 +274,7 @@ KDL::Chain Calibration::getChain() KDL::Frame frame2 = KDL::Frame::DH(robot_parameters_.segments_[i].a_, robot_parameters_.segments_[i].alpha_, 0, 0); KDL::Segment segment2 = KDL::Segment(link_names_[i] + "_passive", KDL::Joint(link_names_[i] + "_passive", KDL::Joint::None), frame2); + robot_chain.addSegment(segment2); } return robot_chain; @@ -337,7 +302,8 @@ void Calibration::debugChain(const KDL::Chain& robot_chain) result.M.GetRPY(roll, pitch, yaw); ROS_INFO_STREAM("Relative rotation: " << roll << ", " << pitch << ", " << yaw); fk_solver.JntToCart(jointpositions, result, i); - ROS_INFO_STREAM(std::setprecision(15) << "Absolute position: " << result.p.x() << ", " << result.p.y() << ", " << result.p.z()); + ROS_INFO_STREAM(std::setprecision(15) + << "Absolute position: " << result.p.x() << ", " << result.p.y() << ", " << result.p.z()); KDL::Joint joint = robot_chain.segments[i].getJoint(); ROS_INFO_STREAM("Joint type: " << joint.getTypeName()); ROS_INFO_STREAM("Joint position: " << joint.pose(0).p.x() << ", " << joint.pose(0).p.y() << ", " diff --git a/src/calibration_correction.cpp b/src/calibration_correction.cpp index 5acbf65..3026a8d 100644 --- a/src/calibration_correction.cpp +++ b/src/calibration_correction.cpp @@ -12,12 +12,29 @@ */ //---------------------------------------------------------------------- - #include int main(int argc, char* argv[]) { - Calibration calibration; + DHRobot my_robot; + my_robot.segments_.push_back(DHSegment(0.1273, 0, 0, M_PI / 2)); + my_robot.segments_.push_back(DHSegment(0, -0.612, 0, 0)); + my_robot.segments_.push_back(DHSegment(0, -0.5723, 0, 0.0)); + my_robot.segments_.push_back(DHSegment(0.163841, 0, 0.0, M_PI / 2)); + my_robot.segments_.push_back(DHSegment(0.1157, 0, 0, -M_PI / 2)); + my_robot.segments_.push_back(DHSegment(0.0922, 0, 0, 0)); + DHRobot my_robot_calibration; + // clang-format off + // d, a, theta, alpha + my_robot_calibration.segments_.push_back(DHSegment(0.00065609212979853 ,4.6311376834935676e-05 ,-7.290070070824746e-05 ,0.000211987863869334 )); + my_robot_calibration.segments_.push_back(DHSegment(1.4442162376284788 ,-0.00012568315331862312 ,-0.01713897289704999 ,-0.0072553625957652995)); + my_robot_calibration.segments_.push_back(DHSegment(0.854147723854608 ,0.00186216581161458 ,-0.03707159413492756 ,-0.013483226769541364 )); + my_robot_calibration.segments_.push_back(DHSegment(-2.2989425877563705 ,9.918593870679266e-05 ,0.054279462160583214 ,0.0013495820227329425 )); + my_robot_calibration.segments_.push_back(DHSegment(-1.573498686836816e-05 ,4.215462720453189e-06 ,1.488984257025741e-07 ,-0.001263136163679901 )); + my_robot_calibration.segments_.push_back(DHSegment(1.9072435590711256e-05 ,0 ,1.551499479707493e-05 ,0 )); + // clang-format on + + Calibration calibration(my_robot + my_robot_calibration); KDL::Chain corrected_chain = calibration.correctChain(); calibration.debugChain(corrected_chain); diff --git a/test/calibration_test.cpp b/test/calibration_test.cpp new file mode 100644 index 0000000..f706e8c --- /dev/null +++ b/test/calibration_test.cpp @@ -0,0 +1,109 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@fzi.de + * \date 2019-03-11 + * + */ +//---------------------------------------------------------------------- + +#include +#include + +TEST(UrRtdeDriver, ur10_ideal) +{ + DHRobot my_robot; + const double pi = std::atan(1)*4; + + // This is an ideal UR10 + // clang-format off + // d, a, theta, alpha + my_robot.segments_.push_back(DHSegment(0.1273 , 0 , 0 , pi / 2)); + my_robot.segments_.push_back(DHSegment(0 , -0.612 , 0 , 0)); + my_robot.segments_.push_back(DHSegment(0 , -0.5723, 0 , 0.0)); + my_robot.segments_.push_back(DHSegment(0.163841, 0 , 0 , pi / 2)); + my_robot.segments_.push_back(DHSegment(0.1157 , 0 , 0 , -pi / 2)); + my_robot.segments_.push_back(DHSegment(0.0922 , 0 , 0 , 0)); + // clang-format on + + Calibration calibration(my_robot); + + KDL::Chain robot_chain = calibration.getChain(); + uint32_t num_jts = robot_chain.getNrOfJoints(); + + KDL::ChainFkSolverPos_recursive fk_solver(robot_chain); + KDL::JntArray jointpositions = KDL::JntArray(num_jts); + KDL::Frame result; + fk_solver.JntToCart(jointpositions, result); + + // Check whether our internal KDL representation gives correct values + EXPECT_DOUBLE_EQ(result.p.x(), my_robot.segments_[1].a_ + my_robot.segments_[2].a_); + EXPECT_DOUBLE_EQ(result.p.y(), -1 * (my_robot.segments_[3].d_ + my_robot.segments_[5].d_)); + EXPECT_DOUBLE_EQ(result.p.z(), my_robot.segments_[0].d_ - my_robot.segments_[4].d_); +} + +TEST(UrRtdeDriver, calibration) +{ + DHRobot my_robot; + const double pi = std::atan(1)*4; + + // This is an ideal UR10 + // clang-format off + // d, a, theta, alpha + my_robot.segments_.push_back(DHSegment(0.1273 , 0 , 0 , pi / 2)); + my_robot.segments_.push_back(DHSegment(0 , -0.612 , 0 , 0)); + my_robot.segments_.push_back(DHSegment(0 , -0.5723, 0 , 0.0)); + my_robot.segments_.push_back(DHSegment(0.163841, 0 , 0 , pi / 2)); + my_robot.segments_.push_back(DHSegment(0.1157 , 0 , 0 , -pi / 2)); + my_robot.segments_.push_back(DHSegment(0.0922 , 0 , 0 , 0)); + // clang-format on + DHRobot my_robot_calibration; + // clang-format off + // d, a, theta, alpha + my_robot_calibration.segments_.push_back(DHSegment(0.00065609212979853 ,4.6311376834935676e-05 ,-7.290070070824746e-05 ,0.000211987863869334 )); + my_robot_calibration.segments_.push_back(DHSegment(1.4442162376284788 ,-0.00012568315331862312 ,-0.01713897289704999 ,-0.0072553625957652995)); + my_robot_calibration.segments_.push_back(DHSegment(0.854147723854608 ,0.00186216581161458 ,-0.03707159413492756 ,-0.013483226769541364 )); + my_robot_calibration.segments_.push_back(DHSegment(-2.2989425877563705 ,9.918593870679266e-05 ,0.054279462160583214 ,0.0013495820227329425 )); + my_robot_calibration.segments_.push_back(DHSegment(-1.573498686836816e-05 ,4.215462720453189e-06 ,1.488984257025741e-07 ,-0.001263136163679901 )); + my_robot_calibration.segments_.push_back(DHSegment(1.9072435590711256e-05 ,0 ,1.551499479707493e-05 ,0 )); + // clang-format on + + Calibration calibration(my_robot + my_robot_calibration); + + KDL::Chain robot_chain = calibration.getChain(); + uint32_t num_jts = robot_chain.getNrOfJoints(); + + KDL::ChainFkSolverPos_recursive fk_solver(robot_chain); + KDL::JntArray jointpositions = KDL::JntArray(num_jts); + KDL::Frame result_original; + fk_solver.JntToCart(jointpositions, result_original); + + KDL::Chain robot_chain_corrected = calibration.correctChain(); + KDL::ChainFkSolverPos_recursive fk_solver_corrected(robot_chain_corrected); + KDL::Frame result_corrected; + fk_solver.JntToCart(jointpositions, result_corrected); + // + // Check whether our internal KDL representation gives correct values + std::cout << result_original.p.x() << std::endl; + std::cout << result_corrected.p.x() << std::endl; + std::cout << result_original.p.y() << std::endl; + std::cout << result_corrected.p.y() << std::endl; + std::cout << result_original.p.z() << std::endl; + std::cout << result_corrected.p.z() << std::endl; + EXPECT_DOUBLE_EQ(result_original.p.x(), result_corrected.p.x()); + EXPECT_DOUBLE_EQ(result_original.p.y(), result_corrected.p.y()); + EXPECT_DOUBLE_EQ(result_original.p.z(), result_corrected.p.z()); +} + +int main(int argc, char* argv[]) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} +