From 4113f3ab2a5d142b9348704086d5bfb4353626ce Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Mon, 6 May 2019 08:47:10 +0200 Subject: [PATCH] Result in 6-jointed chain --- CMakeLists.txt | 1 - include/ur_rtde_driver/calibration.h | 5 +++ src/calibration.cpp | 51 +++++++++++++++++++++- test/calibration_test.cpp | 64 +++++++++++++++++++++------- 4 files changed, 103 insertions(+), 18 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ebd26ab..7d75787 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -127,7 +127,6 @@ add_executable(calibration_correction ) add_dependencies(calibration_correction ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - ## Specify libraries to link a library or executable target against target_link_libraries(calibration_correction ${catkin_LIBRARIES} diff --git a/include/ur_rtde_driver/calibration.h b/include/ur_rtde_driver/calibration.h index 9f228f6..55e00d5 100644 --- a/include/ur_rtde_driver/calibration.h +++ b/include/ur_rtde_driver/calibration.h @@ -17,6 +17,8 @@ #include #include +#include +#include /*! * \brief An internal representation of a DH-parametrized link. @@ -147,6 +149,9 @@ public: } std::string toXacroProperties() {return robot_parameters_corrected_.toXacroProperties();} + void writeToYaml(std::ofstream& ofstream) const; + + std::vector getSimplified() const; Eigen::Matrix4d calcForwardKinematics(const Eigen::Matrix& joint_values, const size_t link_nr = 6); diff --git a/src/calibration.cpp b/src/calibration.cpp index d6d1735..1d44819 100644 --- a/src/calibration.cpp +++ b/src/calibration.cpp @@ -147,11 +147,13 @@ Eigen::Matrix4d Calibration::calcForwardKinematics(const Eigen::Matrix simplified_chain = getSimplified(); for (size_t i = 0; i < link_nr; ++i) { Eigen::Matrix4d rotation = Eigen::Matrix4d::Identity(); rotation.topLeftCorner(3, 3) = Eigen::AngleAxisd(joint_values(i), Eigen::Vector3d::UnitZ()).toRotationMatrix(); - output *= chain_[2 * i] * rotation * chain_[2 * i + 1]; + output *= simplified_chain[i] * rotation; } // ROS_INFO_STREAM("forward_kinematics at " << link_nr << std::endl << output); @@ -179,3 +181,50 @@ void Calibration::buildChain() chain_.push_back(seg2_mat); } } + +std::vector Calibration::getSimplified() const +{ + std::vector simplified_chain; + simplified_chain.push_back(chain_[0]); + for (size_t i = 1; i < chain_.size() - 1; i += 2) + { + simplified_chain.push_back(chain_[i] * chain_[i + 1]); + Eigen::Matrix3d rot_a = chain_[i].topLeftCorner(3, 3); + Eigen::Vector3d rpy_a = rot_a.eulerAngles(0, 1, 2); + // ROS_INFO_STREAM("Rotation " << i / 2 << " a: [" << rpy_a.transpose() << "]"); + Eigen::Matrix3d rot_b = chain_[i + 1].topLeftCorner(3, 3); + Eigen::Vector3d rpy_b = rot_b.eulerAngles(0, 1, 2); + // ROS_INFO_STREAM("Rotation " << i / 2 << " b: [" << rpy_b.transpose() << "]"); + // ROS_INFO_STREAM("Matrix " << i / 2 << ":\n" << simplified_chain.back()); + Eigen::Matrix3d rot = simplified_chain.back().topLeftCorner(3, 3); + Eigen::Vector3d rpy = rot.eulerAngles(0, 1, 2); + Eigen::Quaterniond quat(rot); + // ROS_INFO_STREAM("Rotation (rpy) " << i / 2 << ": [" << rpy.transpose() << "]"); + // ROS_INFO_STREAM("Rotation (quat, [xyz], w)" << i / 2 << ": [" << quat.vec().transpose() << "], " << quat.w()); + } + simplified_chain.push_back(chain_.back()); + return simplified_chain; +} + +void Calibration::writeToYaml(std::ofstream& ofstream) const +{ + YAML::Node node; + + std::vector chain = getSimplified(); + + for (std::size_t i = 0; i < link_names_.size(); ++i) + { + YAML::Node link; + link["x"] = chain[i](0, 3); + link["y"] = chain[i](1, 3); + link["z"] = chain[i](2, 3); + Eigen::Matrix3d rot = chain[i].topLeftCorner(3, 3); + Eigen::Vector3d rpy = rot.eulerAngles(0, 1, 2); + link["roll"] = rpy[0]; + link["pitch"] = rpy[1]; + link["yaw"] = rpy[2]; + node["kinematics"][link_names_[i]] = link; + } + + ofstream << node; +} diff --git a/test/calibration_test.cpp b/test/calibration_test.cpp index 2bdaf51..130a1c7 100644 --- a/test/calibration_test.cpp +++ b/test/calibration_test.cpp @@ -22,10 +22,22 @@ bool isApproximately(const double val1, const double val2, const double precisio return std::abs(val1 - val2) < precision; } -TEST(UrRtdeDriver, ur10_ideal) +template +void doubleEqVec(const Eigen::Matrix vec1, const Eigen::Matrix vec2, + const double precision) +{ + for (size_t i = 0; i < dim_; ++i) + { + EXPECT_NEAR(vec1[i], vec2[i], precision); + } +} + +TEST(UrRtdeDriver, ur10_fw_kinematics) { DHRobot my_robot; const double pi = std::atan(1) * 4; + // const double pi = 1.570796327 * 2.0; + // const double pi = M_PI; // This is an ideal UR10 // clang-format off @@ -40,6 +52,8 @@ TEST(UrRtdeDriver, ur10_ideal) Calibration calibration(my_robot); Eigen::Matrix jointvalues; + Eigen::Vector3d expected_position; + Eigen::Quaterniond expected_orientation; { jointvalues << 0, 0, 0, 0, 0, 0; Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); @@ -51,22 +65,40 @@ TEST(UrRtdeDriver, ur10_ideal) { jointvalues << M_PI_2, -M_PI_4, M_PI_2, -M_PI_4, 0, 0; Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); - EXPECT_DOUBLE_EQ(fk(0, 3), my_robot.segments_[3].d_ + my_robot.segments_[5].d_); - EXPECT_DOUBLE_EQ(fk(1, 3), my_robot.segments_[1].a_ / std::sqrt(2) + my_robot.segments_[2].a_ / std::sqrt(2)); - // Because of the sqrt calculations a DOUBLE_EQ does not work here. - EXPECT_PRED3(isApproximately, fk(2, 3), - my_robot.segments_[0].d_ - my_robot.segments_[1].a_ / std::sqrt(2) + - my_robot.segments_[2].a_ / std::sqrt(2) - my_robot.segments_[4].d_, - 1e-16); + + expected_position << my_robot.segments_[3].d_ + my_robot.segments_[5].d_, + my_robot.segments_[1].a_ / std::sqrt(2) + my_robot.segments_[2].a_ / std::sqrt(2), + my_robot.segments_[0].d_ - my_robot.segments_[1].a_ / std::sqrt(2) + my_robot.segments_[2].a_ / std::sqrt(2) - + my_robot.segments_[4].d_; + doubleEqVec(expected_position, fk.topRightCorner(3, 1), 1e-16); + } + { + jointvalues << -1.6007002035724084976209269370884, -1.7271001974688928726209269370884, + -2.2029998938189905288709269370884, -0.80799991289247685699592693708837, 1.59510004520416259765625, + -0.03099996248354131012092693708837; + expected_position << -0.179925914147547, -0.606869755162764, 0.230789102067257; + + Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); + + doubleEqVec(expected_position, fk.topRightCorner(3, 1), 1e-10); + } + { + jointvalues << 1.32645022869110107421875, 2.426007747650146484375, 5.951572895050048828125, + 1.27409040927886962890625, -0.54105216661562138824592693708837, 0.122173048555850982666015625; + expected_position << 0.39922988003280424074148413637886, 0.59688365069340565405298093537567, + -0.6677819040276375961440180617501; + + Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); + + doubleEqVec(expected_position, fk.topRightCorner(3, 1), 1e-9); } } TEST(UrRtdeDriver, calibration) { - /* This test compares the forward kinematics of the model constructed from uncorrected - * parameters with the one from the corrected parameters. They are tested against random - * joint values and should be equal (in a numeric sense). - */ + // This test compares the forward kinematics of the model constructed from uncorrected + // parameters with the one from the corrected parameters. They are tested against random + // joint values and should be equal (in a numeric sense). DHRobot my_robot; const double pi = std::atan(1) * 4; @@ -107,10 +139,10 @@ TEST(UrRtdeDriver, calibration) Eigen::Matrix3d rot_mat_corrected = fk_corrected.topLeftCorner(3, 3); Eigen::Quaterniond rot_corrected(rot_mat_corrected); double angle_error = std::abs(rot_orig.angularDistance(rot_corrected)); - EXPECT_PRED3(isApproximately, fk_orig(0, 3), fk_corrected(0, 3), 1e-12); - EXPECT_PRED3(isApproximately, fk_orig(1, 3), fk_corrected(1, 3), 1e-12); - EXPECT_PRED3(isApproximately, fk_orig(2, 3), fk_corrected(2, 3), 1e-12); - EXPECT_PRED3(isApproximately, angle_error, 0.0, 1e-12); + EXPECT_NEAR(fk_orig(0, 3), fk_corrected(0, 3), 1e-12); + EXPECT_NEAR(fk_orig(1, 3), fk_corrected(1, 3), 1e-12); + EXPECT_NEAR(fk_orig(2, 3), fk_corrected(2, 3), 1e-12); + EXPECT_NEAR(angle_error, 0.0, 1e-12); } } } // namespace