diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e288705a1..801482e42 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 new file mode 100644 index 000000000..c91768eca --- /dev/null +++ b/ur_calibration/doc/index.rst @@ -0,0 +1,17 @@ + +ur_calibration +============== + +Package for extracting the factory calibration from a UR robot and changing it to be used by ``ur_description`` to gain a correct URDF model. + +Each UR robot is calibrated inside the factory giving exact forward and inverse kinematics. To also +make use of this in ROS, you first have to extract the calibration information from the robot. + +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. + +.. toctree:: + :maxdepth: 2 + + 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 + )) + + ));