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.

ur_calibration/src/calibration.cpp

@@ -73,67 +73,82 @@ 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();