diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 5890137e4..db3d53116 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -1,3 +1,8 @@ +2.1.5 (2024-06-17) +------------------ +* Fix calibration (`#704 `_) +* Contributors: Felix Exner (fexner) + 2.1.4 (2024-04-08) ------------------ diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index 43ecbb1bb..46a595518 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -1,7 +1,7 @@ ur_calibration - 2.1.4 + 2.1.5 Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF Felix Exner diff --git a/ur_calibration/src/calibration.cpp b/ur_calibration/src/calibration.cpp index f54e7e80a..41930c99a 100644 --- a/ur_calibration/src/calibration.cpp +++ b/ur_calibration/src/calibration.cpp @@ -56,68 +56,81 @@ void Calibration::correctAxis(const size_t link_index) // the kinematic structure gets destroyed, which has to be corrected: // - With setting d to 0, both the start and end points of the passive segment move along the // rotational axis of the start segment. Instead, the end point of the passive segment has to - // move along the rotational axis of the next segment. This creates a change in a and and theta, if + // move along the rotational axis of the next segment. This creates a change in a and theta, if // the two rotational axes are not parallel. // // - The length of moving along the next segment's rotational axis is calculated by intersecting // the rotational axis with the XY-plane of the first segment. + // + auto& d_theta_segment = chain_[2 * link_index]; + auto& a_alpha_segment = chain_[2 * link_index + 1]; + + auto& d = d_theta_segment(2, 3); + auto& a = a_alpha_segment(0, 3); - if (chain_[2 * link_index](2, 3) == 0.0) + if (d == 0.0) { // Nothing to do here. return; } - Eigen::Matrix4d fk_current = Eigen::Matrix4d::Identity(); - Eigen::Vector3d current_passive = Eigen::Vector3d::Zero(); + // Start of the next joint's d_theta segment relative to the joint before the current one + Eigen::Matrix4d next_joint_root = Eigen::Matrix4d::Identity(); + next_joint_root *= d_theta_segment * a_alpha_segment; - Eigen::Matrix4d fk_next_passive = Eigen::Matrix4d::Identity(); - fk_next_passive *= chain_[link_index * 2] * chain_[link_index * 2 + 1]; - Eigen::Vector3d eigen_passive = fk_next_passive.topRightCorner(3, 1); + Eigen::Vector3d next_root_position = next_joint_root.topRightCorner(3, 1); - Eigen::Vector3d eigen_next = (fk_next_passive * chain_[(link_index + 1) * 2]).topRightCorner(3, 1); + const auto& next_d_theta_segment = chain_[(link_index + 1) * 2]; + Eigen::Vector3d next_d_theta_end = (next_joint_root * next_d_theta_segment).topRightCorner(3, 1); // Construct a representation of the next segment's rotational axis - Eigen::ParametrizedLine next_line; - next_line = Eigen::ParametrizedLine::Through(eigen_passive, eigen_next); + Eigen::ParametrizedLine next_rotation_axis; + next_rotation_axis = Eigen::ParametrizedLine::Through(next_root_position, next_d_theta_end); - ROS_DEBUG_STREAM("next_line:" << std::endl - << "Base:" << std::endl - << next_line.origin() << std::endl - << "Direction:" << std::endl - << next_line.direction()); + ROS_DEBUG_STREAM("next rotation axis:" << std::endl + << "Base:" << std::endl + << next_rotation_axis.origin() << std::endl + << "Direction:" << std::endl + << next_rotation_axis.direction()); // XY-Plane of first segment's start - Eigen::Hyperplane plane(fk_current.topLeftCorner(3, 3) * Eigen::Vector3d(0, 0, 1), current_passive); - - // Intersect the rotation axis with the XY-Plane. We use both notations, the length and - // intersection point, as we will need both. The intersection_param is the length moving along the - // plane (change in the next segment's d param), while the intersection point will be used for - // calculating the new angle theta. - double intersection_param = next_line.intersectionParameter(plane); - Eigen::Vector3d intersection = next_line.intersectionPoint(plane) - current_passive; - double new_theta = std::atan(intersection.y() / intersection.x()); + Eigen::Hyperplane plane(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d::Zero()); + + // Intersect the rotation axis of the next joint with the XY-Plane. + // * The intersection_param is the length moving along the rotation axis until intersecting the plane. + // * The intersection point will be used for calculating the new angle theta. + double intersection_param = next_rotation_axis.intersectionParameter(plane); + Eigen::Vector3d intersection_point = next_rotation_axis.intersectionPoint(plane); + + // A non-zero a parameter will result in an intersection point at (a, 0) even without any + // additional rotations. This effect has to be subtracted from the resulting theta value. + double subtraction_angle = 0.0; + if (std::abs(a) > 0) + { + // This is pi + subtraction_angle = atan(1) * 4; + } + double new_theta = std::atan2(intersection_point.y(), intersection_point.x()) - subtraction_angle; // Upper and lower arm segments on URs all have negative length due to dh params - double new_length = -1 * intersection.norm(); - ROS_DEBUG_STREAM("Wrist line intersecting at " << std::endl << intersection); + double new_link_length = -1 * intersection_point.norm(); + ROS_DEBUG_STREAM("Next joint's rotation axis intersecting at " << std::endl << intersection_point); ROS_DEBUG_STREAM("Angle is " << new_theta); - ROS_DEBUG_STREAM("Length is " << new_length); + ROS_DEBUG_STREAM("Length is " << new_link_length); ROS_DEBUG_STREAM("Intersection param is " << intersection_param); // as we move the passive segment towards the first segment, we have to move away the next segment // again, to keep the same kinematic structure. - double sign_dir = next_line.direction().z() > 0 ? 1.0 : -1.0; + double sign_dir = next_rotation_axis.direction().z() > 0 ? 1.0 : -1.0; double distance_correction = intersection_param * sign_dir; // Set d parameter of the first segment to 0 and theta to the calculated new angle // Correct arm segment length and angle - chain_[2 * link_index](2, 3) = 0.0; - chain_[2 * link_index].topLeftCorner(3, 3) = - Eigen::AngleAxisd(new_theta, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + d = 0.0; + d_theta_segment.topLeftCorner(3, 3) = Eigen::AngleAxisd(new_theta, Eigen::Vector3d::UnitZ()).toRotationMatrix(); // Correct arm segment length and angle - chain_[2 * link_index + 1](0, 3) = new_length; - chain_[2 * link_index + 1].topLeftCorner(3, 3) = + a = new_link_length; + a_alpha_segment.topLeftCorner(3, 3) = Eigen::AngleAxisd(robot_parameters_.segments_[link_index].theta_ - new_theta, Eigen::Vector3d::UnitZ()) .toRotationMatrix() * Eigen::AngleAxisd(robot_parameters_.segments_[link_index].alpha_, Eigen::Vector3d::UnitX()).toRotationMatrix(); @@ -170,16 +183,7 @@ std::vector Calibration::getSimplified() const 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); - - Eigen::Matrix3d rot_b = chain_[i + 1].topLeftCorner(3, 3); - Eigen::Vector3d rpy_b = rot_b.eulerAngles(0, 1, 2); - - Eigen::Matrix3d rot = simplified_chain.back().topLeftCorner(3, 3); - Eigen::Vector3d rpy = rot.eulerAngles(0, 1, 2); - Eigen::Quaterniond quat(rot); + simplified_chain.emplace_back(chain_[i] * chain_[i + 1]); } simplified_chain.push_back(chain_.back()); return simplified_chain; diff --git a/ur_calibration/test/calibration_test.cpp b/ur_calibration/test/calibration_test.cpp index 8c0260593..2c185fa91 100644 --- a/ur_calibration/test/calibration_test.cpp +++ b/ur_calibration/test/calibration_test.cpp @@ -29,15 +29,18 @@ #include #include -using namespace ur_calibration; +using ur_calibration::Calibration; +using ur_calibration::DHRobot; +using ur_calibration::DHSegment; -namespace -{ +using CalibrationMat = Eigen::Matrix; + +/* bool isApproximately(const double val1, const double val2, const double precision) { return std::abs(val1 - val2) < precision; } - +*/ template void doubleEqVec(const Eigen::Matrix vec1, const Eigen::Matrix vec2, const double precision) @@ -48,21 +51,52 @@ void doubleEqVec(const Eigen::Matrix vec1, const Eigen::Matrix } } -TEST(UrRtdeDriver, ur10_fw_kinematics_ideal) +DHRobot model_from_dh(std::array d, std::array a, std::array theta, + std::array alpha) { - DHRobot my_robot; - const double pi = std::atan(1) * 4; + DHRobot robot; + for (size_t i = 0; i < 6; ++i) + { + robot.segments_.emplace_back(DHSegment(d[i], a[i], theta[i], alpha[i])); + } + return robot; +} - // This is an ideal UR10 - // clang-format off - 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.163941, 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 +class CalibrationTest : public ::testing::TestWithParam +{ +public: + void SetUp() + { + robot_models_.insert({ "ur10_ideal", model_from_dh({ 0.1273, 0, 0, 0.163941, 0.1157, 0.0922 }, // d + { 0, -0.612, -0.5723, 0, 0, 0 }, // a + { 0, 0, 0, 0, 0, 0 }, // theta + { pi / 2, 0, 0, pi / 2, -pi / 2, 0 } // alpha + ) }); + robot_models_.insert({ "ur10", model_from_dh({ 0.1273, 0, 0, 0.163941, 0.1157, 0.0922 }, // d + { 0, -0.612, -0.5723, 0, 0, 0 }, // a + { 0, 0, 0, 0, 0, 0 }, // theta + { pi_2, 0, 0, pi_2, -pi_2, 0 } // alpha + ) }); + robot_models_.insert({ "ur10e", model_from_dh({ 0.1807, 0, 0, 0.17415, 0.11985, 0.11655 }, // d + { 0, -0.6127, -0.57155, 0, 0, 0 }, // a + { 0, 0, 0, 0, 0, 0 }, // theta + { pi_2, 0, 0, pi_2, -pi_2, 0 } // alpha + ) }); + } + void TearDown() + { + Test::TearDown(); + } +protected: + const double pi = std::atan(1) * 4; + const double pi_2 = 1.570796327; // This is what the simulated robot reports as pi/2 + std::map robot_models_; +}; + +TEST_F(CalibrationTest, ur10_fw_kinematics_ideal) +{ + const auto& my_robot = robot_models_["ur10_ideal"]; Calibration calibration(my_robot); Eigen::Matrix jointvalues; @@ -84,26 +118,14 @@ TEST(UrRtdeDriver, ur10_fw_kinematics_ideal) 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); + doubleEqVec(expected_position, fk.topRightCorner(3, 1), 1e-8); } } -TEST(UrRtdeDriver, ur10_fw_kinematics_real) +TEST_F(CalibrationTest, ur10_fw_kinematics_real) { // This test compares a corrected ideal model against positions taken from a simulated robot. - DHRobot my_robot; - const double pi_2 = 1.570796327; // This is what the simulated robot reports as pi/2 - - // This is an ideal UR10 - // clang-format off - 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.163941, 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 - + const auto& my_robot = robot_models_["ur10"]; Calibration calibration(my_robot); Eigen::Matrix jointvalues; @@ -129,44 +151,27 @@ TEST(UrRtdeDriver, ur10_fw_kinematics_real) doubleEqVec(expected_position, fk.topRightCorner(3, 1), 1e-15); } + + TearDown(); } -TEST(UrRtdeDriver, calibration) +TEST_P(CalibrationTest, 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). - DHRobot my_robot; - const double pi = std::atan(1) * 4; + auto my_robot = robot_models_["ur10"]; + Calibration calibration(my_robot); - // 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.163941, 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 + auto robot_calibration = GetParam(); Eigen::Matrix jointvalues; jointvalues << 0, 0, 0, 0, 0, 0; for (size_t i = 0; i < 1000; ++i) { - Calibration calibration(my_robot + my_robot_calibration); + Calibration calibration(my_robot + robot_calibration); jointvalues = 2 * pi * Eigen::Matrix::Random(); Eigen::Matrix4d fk_orig = calibration.calcForwardKinematics(jointvalues); Eigen::Matrix3d rot_mat_orig = fk_orig.topLeftCorner(3, 3); @@ -182,7 +187,43 @@ TEST(UrRtdeDriver, calibration) EXPECT_NEAR(angle_error, 0.0, 1e-12); } } -} // namespace + +// Tests are parametrized by both, the robot model (e.g. "ur10e") and the DH diff as they are stored on the robot +// controller's calibration file. +// The test will then assemble the DH parameters using the ones from the base model and the calibration. +INSTANTIATE_TEST_SUITE_P( + CalibrationTests, CalibrationTest, + ::testing::Values( + model_from_dh({ 0.00065609212979853, 1.4442162376284788, 0.854147723854608, -2.2989425877563705, + -1.573498686836816e-05, 1.9072435590711256e-05 }, // d + { 4.6311376834935676e-05, -0.00012568315331862312, 0.00186216581161458, 9.918593870679266e-05, + 4.215462720453189e-06, 0 }, // a + { -7.290070070824746e-05, -0.01713897289704999, -0.03707159413492756, 0.054279462160583214, + 1.488984257025741e-07, 1.551499479707493e-05 }, // theta + { 0.000211987863869334, -0.0072553625957652995, -0.013483226769541364, 0.0013495820227329425, + -0.001263136163679901, 0 } // alpha + ), + + model_from_dh({ -0.000144894975118076141, 303.469135666158195, -309.88394307789747, 6.41459904397394975, + -4.48232900190081995e-05, -0.00087071402790364627 }, // d + { 0.000108651068627930392, 0.240324175250532346, 0.00180628180213493472, 2.63076149165684402e-05, + 3.96638632500012715e-06, 0 }, // a + { 1.59613525316931737e-07, -0.917209621528830232, 7.12936131346499469, 0.0710299029424392298, + -1.64258976526054923e-06, 9.17286101034808787e-08 }, // theta + { -0.000444781952841921679, -0.00160215112531214153, 0.00631917793331091861, + -0.00165055247340828437, 0.000763682515545038854, 0 } // alpha + ), + model_from_dh({ -0.000160188285741325043, 439.140974079900957, -446.027059806332147, 6.88618689642360149, + -3.86588496858186748e-05, -0.00087908274257374186 }, // d + { 2.12234865571205891e-05, 0.632017132627700651, 0.00229833638891230319, -4.61409023720933908e-05, + -6.39280053471801522e-05, 0 }, // a + { -2.41478068943590252e-07, 1.60233952386694556, -1.68607190752171299, 0.0837331147700118711, + -1.01260355871157781e-07, 3.91986209186123702e-08 }, // theta + { -0.000650246557584166496, 0.00139416666825590402, 0.00693818880325995126, + -0.000811641562390219562, 0.000411120504570705592, 0 } // alpha + ) + + )); int main(int argc, char* argv[]) { diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index 33f54ae94..2c64f4702 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.5 (2024-06-17) +------------------ + 2.1.4 (2024-04-08) ------------------ diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml index f12d165e2..97b1c80e8 100644 --- a/ur_dashboard_msgs/package.xml +++ b/ur_dashboard_msgs/package.xml @@ -1,7 +1,7 @@ ur_dashboard_msgs - 2.1.4 + 2.1.5 Messages around the UR Dashboard server. Felix Exner diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 6c4ca4a3d..da00db8d4 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,3 +1,6 @@ +2.1.5 (2024-06-17) +------------------ + 2.1.4 (2024-04-08) ------------------ * Added support for UR30 (`#688 `_) diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 7f7cc4501..5f5fe2eb3 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -2,7 +2,7 @@ ur_robot_driver - 2.1.4 + 2.1.5 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. Thomas Timm Andersen Simon Rasmussen