Fix calibration (UniversalRobots#704)

* Make calibration tests parametrizable

* 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 a comment on test suite parametrization

ur_calibration/src/calibration.cpp

@@ -56,68 +56,81 @@ 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);
 
-  ROS_DEBUG_STREAM("next_line:" << std::endl
-                                << "Base:" << std::endl
-                                << next_line.origin() << std::endl
-                                << "Direction:" << std::endl
-                                << next_line.direction());
+  ROS_DEBUG_STREAM("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();
-  ROS_DEBUG_STREAM("Wrist line intersecting at " << std::endl << intersection);
+  double new_link_length = -1 * intersection_point.norm();
+  ROS_DEBUG_STREAM("Next joint's rotation axis intersecting at " << std::endl << intersection_point);
   ROS_DEBUG_STREAM("Angle is " << new_theta);
-  ROS_DEBUG_STREAM("Length is " << new_length);
+  ROS_DEBUG_STREAM("Length is " << new_link_length);
   ROS_DEBUG_STREAM("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();
@@ -170,16 +183,7 @@ std::vector<Eigen::Matrix4d> Calibration::getSimplified() const
   simplified_chain.push_back(chain_[0]);
   for (size_t i = 1; i < chain_.size() - 1; i += 2)
   {
-    simplified_chain.push_back(chain_[i] * chain_[i + 1]);
-    Eigen::Matrix3d rot_a = chain_[i].topLeftCorner(3, 3);
-    Eigen::Vector3d rpy_a = rot_a.eulerAngles(0, 1, 2);
-
-    Eigen::Matrix3d rot_b = chain_[i + 1].topLeftCorner(3, 3);
-    Eigen::Vector3d rpy_b = rot_b.eulerAngles(0, 1, 2);
-
-    Eigen::Matrix3d rot = simplified_chain.back().topLeftCorner(3, 3);
-    Eigen::Vector3d rpy = rot.eulerAngles(0, 1, 2);
-    Eigen::Quaterniond quat(rot);
+    simplified_chain.emplace_back(chain_[i] * chain_[i + 1]);
   }
   simplified_chain.push_back(chain_.back());
   return simplified_chain;

ur_calibration/test/calibration_test.cpp

@@ -29,15 +29,18 @@
 #include <gtest/gtest.h>
 #include <ur_calibration/calibration.h>
 
-using namespace ur_calibration;
+using ur_calibration::Calibration;
+using ur_calibration::DHRobot;
+using ur_calibration::DHSegment;
 
-namespace
-{
+using CalibrationMat = Eigen::Matrix<double, 6, 4>;
+
+/*
 bool isApproximately(const double val1, const double val2, const double precision)
 {
   return std::abs(val1 - val2) < precision;
 }
-
+*/
 template <class Scalar_, int dim_>
 void doubleEqVec(const Eigen::Matrix<Scalar_, dim_, 1> vec1, const Eigen::Matrix<Scalar_, dim_, 1> vec2,
                  const double precision)
@@ -48,21 +51,52 @@ void doubleEqVec(const Eigen::Matrix<Scalar_, dim_, 1> vec1, const Eigen::Matrix
   }
 }
 
-TEST(UrRtdeDriver, ur10_fw_kinematics_ideal)
+DHRobot model_from_dh(std::array<double, 6> d, std::array<double, 6> a, std::array<double, 6> theta,
+                      std::array<double, 6> alpha)
 {
-  DHRobot my_robot;
-  const double pi = std::atan(1) * 4;
+  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;
+}
 
-  // This is an ideal UR10
-  // clang-format off
-  my_robot.segments_.push_back(DHSegment(0.1273  , 0      , 0    , pi / 2));
-  my_robot.segments_.push_back(DHSegment(0       , -0.612 , 0    , 0));
-  my_robot.segments_.push_back(DHSegment(0       , -0.5723, 0    , 0.0));
-  my_robot.segments_.push_back(DHSegment(0.163941, 0      , 0    , pi / 2));
-  my_robot.segments_.push_back(DHSegment(0.1157  , 0      , 0    , -pi / 2));
-  my_robot.segments_.push_back(DHSegment(0.0922  , 0      , 0    , 0));
-  // clang-format on
+class CalibrationTest : public ::testing::TestWithParam<DHRobot>
+{
+public:
+  void 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()
+  {
+    Test::TearDown();
+  }
 
+protected:
+  const double pi = std::atan(1) * 4;
+  const double pi_2 = 1.570796327;  // This is what the simulated robot reports as pi/2
+  std::map<std::string, DHRobot> robot_models_;
+};
+
+TEST_F(CalibrationTest, ur10_fw_kinematics_ideal)
+{
+  const auto& my_robot = robot_models_["ur10_ideal"];
   Calibration calibration(my_robot);
 
   Eigen::Matrix<double, 6, 1> jointvalues;
@@ -84,26 +118,14 @@ TEST(UrRtdeDriver, ur10_fw_kinematics_ideal)
         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<double, 3>(expected_position, fk.topRightCorner(3, 1), 1e-16);
+    doubleEqVec<double, 3>(expected_position, fk.topRightCorner(3, 1), 1e-8);
   }
 }
 
-TEST(UrRtdeDriver, ur10_fw_kinematics_real)
+TEST_F(CalibrationTest, ur10_fw_kinematics_real)
 {
   // This test compares a corrected ideal model against positions taken from a simulated robot.
-  DHRobot my_robot;
-  const double pi_2 = 1.570796327;  // This is what the simulated robot reports as pi/2
-
-  // This is an ideal UR10
-  // clang-format off
-  my_robot.segments_.push_back(DHSegment(0.1273  , 0      , 0    , pi_2));
-  my_robot.segments_.push_back(DHSegment(0       , -0.612 , 0    , 0));
-  my_robot.segments_.push_back(DHSegment(0       , -0.5723, 0    , 0.0));
-  my_robot.segments_.push_back(DHSegment(0.163941, 0      , 0    , pi_2));
-  my_robot.segments_.push_back(DHSegment(0.1157  , 0      , 0    , -pi_2));
-  my_robot.segments_.push_back(DHSegment(0.0922  , 0      , 0    , 0));
-  // clang-format on
-
+  const auto& my_robot = robot_models_["ur10"];
   Calibration calibration(my_robot);
 
   Eigen::Matrix<double, 6, 1> jointvalues;
@@ -129,44 +151,27 @@ TEST(UrRtdeDriver, ur10_fw_kinematics_real)
 
     doubleEqVec<double, 3>(expected_position, fk.topRightCorner(3, 1), 1e-15);
   }
+
+  TearDown();
 }
 
-TEST(UrRtdeDriver, 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).
 
-  DHRobot my_robot;
-  const double pi = std::atan(1) * 4;
+  auto my_robot = robot_models_["ur10"];
+  Calibration calibration(my_robot);
 
-  // This is an ideal UR10
-  // clang-format off
-  //                                     d,        a,       theta, alpha
-  my_robot.segments_.push_back(DHSegment(0.1273  , 0      , 0    , pi / 2));
-  my_robot.segments_.push_back(DHSegment(0       , -0.612 , 0    , 0));
-  my_robot.segments_.push_back(DHSegment(0       , -0.5723, 0    , 0.0));
-  my_robot.segments_.push_back(DHSegment(0.163941, 0      , 0    , pi / 2));
-  my_robot.segments_.push_back(DHSegment(0.1157  , 0      , 0    , -pi / 2));
-  my_robot.segments_.push_back(DHSegment(0.0922  , 0      , 0    , 0));
-  // clang-format on
-  DHRobot my_robot_calibration;
-  // clang-format off
-  //                                                 d,        a,       theta, alpha
-  my_robot_calibration.segments_.push_back(DHSegment( 0.00065609212979853    ,  4.6311376834935676e-05 , -7.290070070824746e-05  ,  0.000211987863869334  ));
-  my_robot_calibration.segments_.push_back(DHSegment( 1.4442162376284788     , -0.00012568315331862312 , -0.01713897289704999    , -0.0072553625957652995));
-  my_robot_calibration.segments_.push_back(DHSegment( 0.854147723854608      ,  0.00186216581161458    , -0.03707159413492756    , -0.013483226769541364 ));
-  my_robot_calibration.segments_.push_back(DHSegment(-2.2989425877563705     ,  9.918593870679266e-05  , 0.054279462160583214   ,  0.0013495820227329425 ));
-  my_robot_calibration.segments_.push_back(DHSegment(-1.573498686836816e-05  ,  4.215462720453189e-06  , 1.488984257025741e-07  , -0.001263136163679901 ));
-  my_robot_calibration.segments_.push_back(DHSegment( 1.9072435590711256e-05 ,  0                      , 1.551499479707493e-05  ,  0 ));
-  // clang-format on
+  auto robot_calibration = GetParam();
 
   Eigen::Matrix<double, 6, 1> 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<double, 6, 1>::Random();
     Eigen::Matrix4d fk_orig = calibration.calcForwardKinematics(jointvalues);
     Eigen::Matrix3d rot_mat_orig = fk_orig.topLeftCorner(3, 3);
@@ -182,7 +187,43 @@ TEST(UrRtdeDriver, calibration)
     EXPECT_NEAR(angle_error, 0.0, 1e-12);
   }
 }
-}  // namespace
+
+// 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(
+        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
+                      ),
+
+        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
+                      ),
+        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
+                      )
+
+            ));
 
 int main(int argc, char* argv[])
 {