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

Author: Felix Exner (fexner)

.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/calibration_example.png

@@ -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/calibration_example_corrected.png

@@ -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:=<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>`_.
+   usage
+   algorithm

ur_calibration/doc/calibration_uncorrected.png

@@ -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/doc/conf.py

@@ -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;
 }