Fix calibration (#1017)

* Make calibration tests parametrizable

* Add two more real-world configurations to tests

Note: the second one fails with our current implementation

* Fix calibration correction

There was an error from using std::atan instead of std::atan2.
In most cases that seemed to work fine, but with certain calibrations the
calculated angle could end up in another quadrant, so atan was returning
the wrong angle.

We renamed a lot of variables and changed some of the docstrings in order
to hopefully make things more understandable in the future.

* Add robot model to calibration tests

* Add documentation to calibration algorithm

* Add a comment on test suite parametrization

(cherry picked from commit 557b57e)

# Conflicts:
#	ur_calibration/doc/index.rst

.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

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.

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 ------------------------------------------------

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

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:=<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
+<https://github.com/UniversalRobots/Universal_Robots_ROS2_Tutorials/blob/main/my_robot_cell/doc/start_ur_driver.rst#extract-the-calibration>`_.

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<double, 3> next_line;
-  next_line = Eigen::ParametrizedLine<double, 3>::Through(eigen_passive, eigen_next);
+  Eigen::ParametrizedLine<double, 3> next_rotation_axis;
+  next_rotation_axis = Eigen::ParametrizedLine<double, 3>::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<double, 3> 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<double, 3> 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;
 }