diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 5538e9f38..e119a9c7e 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -116,7 +116,7 @@ repos: stages: [commit] entry: ament_copyright language: system - args: ['--exclude', 'ur_robot_driver/doc/conf.py'] + args: ['--exclude', 'ur_robot_driver/doc/conf.py', 'ur_calibration/doc/conf.py'] # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 diff --git a/ur_calibration/doc/algorithm.rst b/ur_calibration/doc/algorithm.rst new file mode 100644 index 000000000..036439030 --- /dev/null +++ b/ur_calibration/doc/algorithm.rst @@ -0,0 +1,97 @@ +Calibration correction algorithm +================================ + +When extracting the robot's calibration there will be a set of Denavit–Hartenberg (DH) parameters +describing the robot. Due to the calibration process they can seem a bit unintuitive since the +``d``-parameter of the second and third joint can be quite large on those. + +For example, let's consider the following dh parameters taken from a real robot: + +.. code-block:: yaml + + # joint1 joint2 joint3 joint4 joint5 joint6 + dh_theta: [-2.4147806894359e-07 1.60233952386695 -1.68607190752171 0.0837331147700119 -1.01260355871158e-07 3.91986209186124e-08 ] + dh_a: [ 2.12234865571206e-05 0.0193171326277006 -0.569251663611088 -4.61409023720934e-05 -6.39280053471802e-05 0 ] + dh_d: [ 0.180539811714259 439.140974079901 -446.027059806332 7.0603368964236 0.119811341150314 0.115670917257426 ] + dh_alpha: [ 1.57014608044242 0.0013941666682559 0.00693818880325995 1.56998468543761 -1.57038520649543 0 ] + +One can see that the upper arm is placed 439 m out of the base link with the lower arm being 7 m to +the other side. + +We can build a robot description that splits each DH segment into two links: One for ``d`` and +``theta`` representing the rotational part of the joint and one for ``a`` and ``alpha`` +representing the "passive" part of the joint displacing the next link. +:numref:`calibration_example` shows (a part of) the description matching the parameters above. The +arm links are left out of the image as they are really far away. + +.. _calibration_example: +.. figure:: calibration_example.png + :alt: Example of a calibrated model + + This shows an example calibrated model when using the DH parameters directly as explained above. + The two arm links are virtually displaced very far from the physical robot while the TCP ends up + at the correct location again + + +For explaining the correction algorithm, we will use an artificial set of DH parameters for a +UR10e: + +.. code-block:: yaml + + # join1 joint2 joint3 joint4 joint5 joint6 + dh_theta: [0 0 0 0 0 0 ] + dh_a: [0 -0.6127 -0.57155 0 0 0 ] + dh_d: [0.1807 1 0.5 -1.32585 0.11985 0.11655] + dh_alpha: [1.570796327 0.2 0 1.570796327 -1.570796327 0 ] + +The resulting uncorrected model can be seen in :numref:`calibration_uncorrected`. The upper arm is +placed 1 m to the left of the shoulder, the upper arm is placed 0.5 m further out and there's an +added ``alpha`` rotation in joint2. + +Note: This is not a valid calibration, so when placing this "calibration" on a robot and using the +correction, we won't get correct tcp pose results. This only serves as a exaggerated example. + +.. _calibration_uncorrected: +.. figure:: calibration_uncorrected.png + :alt: Exaggerated calibrated model + + This shows an artificial calibration only to show the algorithm. This is no valid calibration! + +In :numref:`calibration_uncorrected` the separation between the two DH components can be seen quite +clearly. joint2's ``d`` and ``theta`` parameters are represented by ``upper_arm_d`` and its ``a`` +and ``alpha`` parameters result in ``upper_arm_a``. + +The "correction" step tries to bring the two arm segments back to their physical representation. +In principle, the d parameter to zero, first. With this change, +the kinematic structure gets destroyed, which has to be corrected: + +- With setting ``d`` to 0, both the start (``upper_arm_d``) and end (``upper_arm_a``) points of the + passive segment move along the joint's rotational axis. Instead, the end point of the passive + segment has to move along the rotational axis of the next segment. This requires adapting + ``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 next rotational axis with the XY-plane of the moved ``_d`` frame. This gets subtracted from + the next joint's ``d`` parameter. + +Note that the parameters from this model are not strict DH parameters anymore, as the two frames at +the tip of the upper and lower arm have to get an additional rotation to compensate the change of +the arm segment orientation, when the tip is moving along its rotation axis. + +The resulting "DH parameters" are then reassembled into six individual transforms that can become +the six frames of the robot's kinematic chain. This is exported in a yaml representation and gets +read by the description package. + +Also, no correction of the visual meshes is performed. Strictly speaking, the visual +model is not 100 % correct, but with a calibrated model and ideal meshes this cannot be achieved and +the inaccuracies introduced by this are considered negligible. + +The example as visualized in :numref:`calibration_example` looks as follows if a description with +the correct parameters is loaded: + +.. figure:: calibration_example_corrected.png + :alt: Example with corrected kinematics structure + + This shows the model from :numref:`calibration_example` with the calibration correction applied. + The robot is correctly assembled and the ``base->tool0`` transformation is exactly the same as + on the robot controller. diff --git a/ur_calibration/doc/calibration_example.png b/ur_calibration/doc/calibration_example.png new file mode 100644 index 000000000..6185a198c Binary files /dev/null and b/ur_calibration/doc/calibration_example.png differ diff --git a/ur_calibration/doc/calibration_example_corrected.png b/ur_calibration/doc/calibration_example_corrected.png new file mode 100644 index 000000000..a90c695d3 Binary files /dev/null and b/ur_calibration/doc/calibration_example_corrected.png differ diff --git a/ur_calibration/doc/calibration_uncorrected.png b/ur_calibration/doc/calibration_uncorrected.png new file mode 100644 index 000000000..73b2a46dc Binary files /dev/null and b/ur_calibration/doc/calibration_uncorrected.png differ diff --git a/ur_calibration/doc/conf.py b/ur_calibration/doc/conf.py new file mode 100644 index 000000000..c0b683e07 --- /dev/null +++ b/ur_calibration/doc/conf.py @@ -0,0 +1,85 @@ +project = "ur_calibration" +copyright = "2022, Universal Robots A/S" +author = "Felix Exner" + +# The short X.Y version +version = "" +# The full version, including alpha/beta/rc tags +release = "" + +# -- General configuration --------------------------------------------------- + +# If your documentation needs a minimal Sphinx version, state it here. +# +# needs_sphinx = '1.0' + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ["_templates"] + +# The suffix(es) of source filenames. +# You can specify multiple suffix as a list of string: +# +source_suffix = [".rst"] + +# The master toctree document. +master_doc = "index" + +numfig = True + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +# +# This is also used if you do content translation via gettext catalogs. +# Usually you set "language" from the command line for these cases. +language = None + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +# This pattern also affects html_static_path and html_extra_path. +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = None + + +# -- Options for HTML output ------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +# +html_theme = "alabaster" + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +# +# html_theme_options = {} + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ["_static"] + +# Custom sidebar templates, must be a dictionary that maps document names +# to template names. +# +# The default sidebars (for documents that don't match any pattern) are +# defined by theme itself. Builtin themes are using these templates by +# default: ``['localtoc.html', 'relations.html', 'sourcelink.html', +# 'searchbox.html']``. +# +# html_sidebars = {} + + +# -- Options for HTMLHelp output --------------------------------------------- + +# Output file base name for HTML help builder. +htmlhelp_basename = "ur_calibration_doc" + + +# -- Options for LaTeX output ------------------------------------------------ diff --git a/ur_calibration/doc/index.rst b/ur_calibration/doc/index.rst index d8056e35a..c91768eca 100644 --- a/ur_calibration/doc/index.rst +++ b/ur_calibration/doc/index.rst @@ -10,36 +10,8 @@ make use of this in ROS, you first have to extract the calibration information f Though this step is not necessary, to control the robot using this driver, it is highly recommended to do so, as end effector positions might be off in the magnitude of centimeters. -Nodes ------ +.. toctree:: + :maxdepth: 2 -calibration_correction -^^^^^^^^^^^^^^^^^^^^^^ - -This node extracts calibration information directly from a robot, calculates the URDF correction and -saves it into a .yaml file. - -In the launch folder of the ur_calibration package is a helper script: - -.. code-block:: bash - - $ ros2 launch ur_calibration calibration_correction.launch.py \ - robot_ip:= target_filename:="${HOME}/my_robot_calibration.yaml" - -For the parameter ``robot_ip`` insert the IP address on which the ROS pc can reach the robot. As -``target_filename`` provide an absolute path where the result will be saved to. - -With that, you can launch your specific robot with the correct calibration using - -.. code-block:: bash - - $ ros2 launch ur_robot_driver ur_control.launch.py \ - ur_type:=ur5e \ - robot_ip:=192.168.56.101 \ - kinematics_params_file:="${HOME}/my_robot_calibration.yaml" - -Adapt the robot model matching to your robot. - -Ideally, you would create a package for your custom workcell, as explained in `the custom workcell -tutorial -`_. + usage + algorithm diff --git a/ur_calibration/doc/usage.rst b/ur_calibration/doc/usage.rst new file mode 100644 index 000000000..ecf4b2384 --- /dev/null +++ b/ur_calibration/doc/usage.rst @@ -0,0 +1,28 @@ +Usage +===== + +To use the calibration correction this package provides a launchfile that extracts calibration +information directly from a robot, calculates the URDF correction and saves it into a .yaml file: + +.. code-block:: bash + + $ ros2 launch ur_calibration calibration_correction.launch.py \ + robot_ip:= target_filename:="${HOME}/my_robot_calibration.yaml" + +For the parameter ``robot_ip`` insert the IP address on which the ROS pc can reach the robot. As +``target_filename`` provide an absolute path where the result will be saved to. + +With that, you can launch your specific robot with the correct calibration using + +.. code-block:: bash + + $ ros2 launch ur_robot_driver ur_control.launch.py \ + ur_type:=ur5e \ + robot_ip:=192.168.56.101 \ + kinematics_params_file:="${HOME}/my_robot_calibration.yaml" + +Adapt the robot model matching to your robot. + +Ideally, you would create a package for your custom workcell, as explained in `the custom workcell +tutorial +`_. diff --git a/ur_calibration/src/calibration.cpp b/ur_calibration/src/calibration.cpp index 0c9108b35..9a680d856 100644 --- a/ur_calibration/src/calibration.cpp +++ b/ur_calibration/src/calibration.cpp @@ -73,72 +73,87 @@ 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); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "next_line:" << std::endl - << "Base:" << std::endl - << next_line.origin() << std::endl - << "Direction:" << std::endl - << next_line.direction()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "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(); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "Wrist line intersecting at " << std::endl << intersection); + double new_link_length = -1 * intersection_point.norm(); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "Next joint's rotation axis intersecting at " + << std::endl + << intersection_point); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "Angle is " << new_theta); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "Length is " << new_length); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "Length is " << new_link_length); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "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(); - // Correct next joint + // Correct next joint's d parameter chain_[2 * link_index + 2](2, 3) -= distance_correction; } diff --git a/ur_calibration/test/calibration_test.cpp b/ur_calibration/test/calibration_test.cpp index f6910fe0f..e5f0685b0 100644 --- a/ur_calibration/test/calibration_test.cpp +++ b/ur_calibration/test/calibration_test.cpp @@ -42,12 +42,16 @@ //---------------------------------------------------------------------- #include +#include +#include #include using ur_calibration::Calibration; using ur_calibration::DHRobot; using ur_calibration::DHSegment; +using CalibrationMat = Eigen::Matrix; + namespace { /* @@ -67,12 +71,36 @@ void doubleEqVec(const Eigen::Matrix vec1, const Eigen::Matrix } // namespace -class CalibrationTest : public ::testing::Test +DHRobot model_from_dh(std::array d, std::array a, std::array theta, + std::array alpha) +{ + 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; +} + +class CalibrationTest : public ::testing::TestWithParam> { public: void SetUp() { - Test::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() { @@ -82,24 +110,13 @@ class CalibrationTest : public ::testing::Test protected: const double pi = std::atan(1) * 4; const double pi_2 = 1.570796327; // This is what the simulated robot reports as pi/2 - DHRobot my_robot_; - DHRobot my_robot_calibration_; + std::map robot_models_; }; TEST_F(CalibrationTest, ur10_fw_kinematics_ideal) { - my_robot_.segments_.clear(); - // This is an ideal UR10 - // clang-format off - my_robot_.segments_.emplace_back(DHSegment(0.1273 , 0 , 0 , pi / 2)); - my_robot_.segments_.emplace_back(DHSegment(0 , -0.612 , 0 , 0)); - my_robot_.segments_.emplace_back(DHSegment(0 , -0.5723, 0 , 0.0)); - my_robot_.segments_.emplace_back(DHSegment(0.163941, 0 , 0 , pi / 2)); - my_robot_.segments_.emplace_back(DHSegment(0.1157 , 0 , 0 , -pi / 2)); - my_robot_.segments_.emplace_back(DHSegment(0.0922 , 0 , 0 , 0)); - // clang-format on - - Calibration calibration(my_robot_); + const auto& my_robot = robot_models_["ur10_ideal"]; + Calibration calibration(my_robot); Eigen::Matrix jointvalues; Eigen::Vector3d expected_position; @@ -107,38 +124,28 @@ TEST_F(CalibrationTest, ur10_fw_kinematics_ideal) { jointvalues << 0, 0, 0, 0, 0, 0; Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); - EXPECT_DOUBLE_EQ(fk(0, 3), my_robot_.segments_[1].a_ + my_robot_.segments_[2].a_); - EXPECT_DOUBLE_EQ(fk(1, 3), -1 * (my_robot_.segments_[3].d_ + my_robot_.segments_[5].d_)); - EXPECT_DOUBLE_EQ(fk(2, 3), my_robot_.segments_[0].d_ - my_robot_.segments_[4].d_); + EXPECT_DOUBLE_EQ(fk(0, 3), my_robot.segments_[1].a_ + my_robot.segments_[2].a_); + EXPECT_DOUBLE_EQ(fk(1, 3), -1 * (my_robot.segments_[3].d_ + my_robot.segments_[5].d_)); + EXPECT_DOUBLE_EQ(fk(2, 3), my_robot.segments_[0].d_ - my_robot.segments_[4].d_); } { jointvalues << M_PI_2, -M_PI_4, M_PI_2, -M_PI_4, 0, 0; Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); - 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); + 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-8); } } TEST_F(CalibrationTest, ur10_fw_kinematics_real) { // This test compares a corrected ideal model against positions taken from a simulated robot. - my_robot_.segments_.clear(); - // This is an ideal UR10 - // clang-format off - my_robot_.segments_.emplace_back(DHSegment(0.1273 , 0 , 0 , pi_2)); - my_robot_.segments_.emplace_back(DHSegment(0 , -0.612 , 0 , 0)); - my_robot_.segments_.emplace_back(DHSegment(0 , -0.5723, 0 , 0.0)); - my_robot_.segments_.emplace_back(DHSegment(0.163941, 0 , 0 , pi_2)); - my_robot_.segments_.emplace_back(DHSegment(0.1157 , 0 , 0 , -pi_2)); - my_robot_.segments_.emplace_back(DHSegment(0.0922 , 0 , 0 , 0)); - // clang-format on - - Calibration calibration(my_robot_); + const auto& my_robot = robot_models_["ur10"]; + Calibration calibration(my_robot); Eigen::Matrix jointvalues; Eigen::Vector3d expected_position; @@ -167,46 +174,21 @@ TEST_F(CalibrationTest, ur10_fw_kinematics_real) TearDown(); } -TEST_F(CalibrationTest, 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). - my_robot_.segments_.clear(); - - // This is an ideal UR10 - // clang-format off - // d, a, theta, alpha - my_robot_.segments_.emplace_back(DHSegment(0.1273 , 0 , 0 , pi / 2)); - my_robot_.segments_.emplace_back(DHSegment(0 , -0.612 , 0 , 0)); - my_robot_.segments_.emplace_back(DHSegment(0 , -0.5723, 0 , 0.0)); - my_robot_.segments_.emplace_back(DHSegment(0.163941, 0 , 0 , pi / 2)); - my_robot_.segments_.emplace_back(DHSegment(0.1157 , 0 , 0 , -pi / 2)); - my_robot_.segments_.emplace_back(DHSegment(0.0922 , 0 , 0 , 0)); - // clang-format on - my_robot_calibration_.segments_.clear(); - // clang-format off - // d, a, theta, alpha - my_robot_calibration_.segments_.emplace_back(DHSegment( 0.00065609212979853 , 4.6311376834935676e-05 , - -7.290070070824746e-05 , 0.000211987863869334 )); - my_robot_calibration_.segments_.emplace_back(DHSegment( 1.4442162376284788 , -0.00012568315331862312 , - -0.01713897289704999 , -0.0072553625957652995)); - my_robot_calibration_.segments_.emplace_back(DHSegment( 0.854147723854608 , 0.00186216581161458 , - -0.03707159413492756 , -0.013483226769541364 )); - my_robot_calibration_.segments_.emplace_back(DHSegment(-2.2989425877563705 , 9.918593870679266e-05 , - 0.054279462160583214 , 0.0013495820227329425 )); - my_robot_calibration_.segments_.emplace_back(DHSegment(-1.573498686836816e-05 , 4.215462720453189e-06 , - 1.488984257025741e-07 , -0.001263136163679901 )); - my_robot_calibration_.segments_.emplace_back(DHSegment( 1.9072435590711256e-05 , 0 , - 1.551499479707493e-05 , 0 )); - // clang-format on + auto [robot_model, robot_calibration] = GetParam(); + auto my_robot = robot_models_.at(robot_model); + Calibration calibration(my_robot); 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); @@ -222,3 +204,45 @@ TEST_F(CalibrationTest, calibration) EXPECT_NEAR(angle_error, 0.0, 1e-12); } } + +// 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( + std::make_tuple("ur10", + 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 + )), + + std::make_tuple("ur10e", + 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 + )), + std::make_tuple("ur10e", + 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 + )) + + ));