added first version of calibration

CMakeLists.txt

@@ -13,17 +13,21 @@ find_package(catkin REQUIRED
     actionlib
     control_msgs
     controller_manager
+    eigen_conversions
     geometry_msgs
     hardware_interface
+    kdl_parser
     industrial_msgs
     roscpp
     sensor_msgs
     std_srvs
     tf
+    tf_conversions
     trajectory_msgs
     ur_msgs
 )
 find_package(Boost REQUIRED)
+find_package(Eigen3 REQUIRED)
 
 catkin_package(
   INCLUDE_DIRS
@@ -34,13 +38,18 @@ catkin_package(
     actionlib
     control_msgs
     controller_manager
+    eigen_conversions
     geometry_msgs
     hardware_interface
+    kdl_parser
     industrial_msgs
     roscpp
     sensor_msgs
     trajectory_msgs
     ur_msgs
+    std_srvs
+    tf
+    tf_conversions
   DEPENDS
     Boost
 )
@@ -67,6 +76,7 @@ include_directories(
   include
   ${catkin_INCLUDE_DIRS}
   ${Boost_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
 )
 
 add_library(ur_rtde_driver
@@ -110,12 +120,19 @@ add_executable(plain_driver
 target_link_libraries(plain_driver ${catkin_LIBRARIES} ur_rtde_driver)
 add_dependencies(plain_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
-install(TARGETS ur_rtde_driver ur_rtde_driver_node
+
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+add_executable(calibration_correction
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  src/calibration.cpp
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  src/calibration_correction.cpp
 )
 
+add_dependencies(calibration_correction ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(calibration_correction
+  ${catkin_LIBRARIES}
+  yaml-cpp
+)
 
 install(DIRECTORY config launch
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
@@ -136,3 +153,8 @@ install(DIRECTORY include/${PROJECT_NAME}/
   #catkin_add_gtest(ur_rtde_driver_test ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_TEST_SOURCES} tests/main.cpp)
   #target_link_libraries(ur_rtde_driver_test ur_rtde_driver ${catkin_LIBRARIES})
 #endif()
+install(TARGETS calibration_correction RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+install(TARGETS ur_rtde_driver ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
+install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+install(FILES Apache-2.0_ur_modern_driver.txt DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/LICENSES)
+install(FILES manifest.yaml rosdoc.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/doc)

include/ur_rtde_driver/calibration.h

@@ -0,0 +1,207 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-01-10
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED
+#define UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED
+
+#include <ros/ros.h>
+
+#include <kdl_parser/kdl_parser.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+
+/*!
+ * \brief An internal representation of a DH-parametrized link.
+ *
+ * Each segment consists of parameters a, d, alpha and theta.
+ */
+struct DHSegment
+{
+  double d_;
+  double a_;
+  double theta_;
+  double alpha_;
+
+  /*!
+   * \brief Creates an element with defined elements.
+   */
+  DHSegment(const double d, const double a, const double theta, const double alpha)
+    : d_(d), a_(a), theta_(theta), alpha_(alpha)
+  {
+  }
+
+  /*!
+   * \brief Creates a Segment with all elements equal to zero.
+   */
+  DHSegment() : d_(0), a_(0), theta_(0), alpha_(0)
+  {
+  }
+
+  /*!
+   * \brief Adds another segment element-wise (a+a, d+d, alpha+alpha, theta+theta)
+   *
+   * \param other Other segment to add
+   *
+   * \returns Segment consisting of element-wise addition of \p this and \p other
+   */
+  DHSegment operator+(const DHSegment& other)
+  {
+    return DHSegment(this->d_ + other.d_, this->a_ + other.a_, this->theta_ + other.theta_,
+                     this->alpha_ + other.alpha_);
+  }
+};
+
+/*!
+ * \brief Internal representation of a robot based on DH parameters.
+ *
+ * Note that this representation doesn't contain a real DH parameter representation, but is used for
+ * a corrected model of calibrated UR robots. Shoulder and elbow axes are artificially shortened in
+ * the final representation, requiring a correction parameter in \p theta_2 and \p theta_3.
+ */
+struct DHRobot
+{
+  std::vector<DHSegment> segments_;
+  double delta_theta_correction2_;
+  double delta_theta_correction3_;
+
+  /*!
+   * \brief Create a new robot representation giving a set of \ref DHSegment objects
+   */
+  DHRobot(const std::vector<DHSegment>& segments)
+  {
+  }
+
+  DHRobot() = default;
+
+  /*!
+   * \brief Adds another robot representation, by adding their segments element-wise. See \ref
+   * DHSegment::operator+ for details.
+   */
+  DHRobot operator+(const DHRobot& other)
+  {
+    assert(this->segments_.size() == other.segments_.size());
+    DHRobot ret;
+    for (size_t i = 0; i < this->segments_.size(); ++i)
+    {
+      ret.segments_.push_back(this->segments_[i] + other.segments_[i]);
+    }
+    return ret;
+  }
+
+  /*!
+   * \brief Generates a string representation of this robot representation.
+   *
+   * This string can be directly included into the xacro file used for a calibrated robot.
+   */
+  std::string toXacroProperties()
+  {
+    std::stringstream ss;
+    for (size_t i = 0; i < segments_.size(); ++i)
+    {
+      ss << "<xacro:property name=\"d" << i + 1 << "\" value=\"" << segments_[i].d_ << "\" />\n";
+      ss << "<xacro:property name=\"a" << i + 1 << "\" value=\"" << segments_[i].a_ << "\" />\n";
+      ss << "<xacro:property name=\"theta" << i + 1 << "\" value=\"" << segments_[i].theta_ << "\" />\n";
+      ss << "<xacro:property name=\"alpha" << i + 1 << "\" value=\"" << segments_[i].alpha_ << "\" />\n";
+    }
+    ss << "<xacro:property name=\"delta_theta_correction2\" value=\"" << delta_theta_correction2_ << "\" />\n";
+    ss << "<xacro:property name=\"delta_theta_correction3\" value=\"" << delta_theta_correction3_ << "\" />\n";
+    return ss.str();
+  }
+};
+
+
+/*!
+ * \brief Class that handles the calibration correction for Universal Robots
+ *
+ * Universal robots provide a factory calibration of their DH parameters to exactly estimate their
+ * TCP pose using forward kinematics. However, those DH parameters construct a kinematic chain that
+ * can be very long, as upper arm and lower arm segments can be drawn out of their physical position
+ * by multiple meters (even above 100m can occur).
+ *
+ * This class helps creating a kinematic chain, that is as close as possible to the physical model,
+ * by dragging the upper and lower arm segments back to their zero position.
+ */
+class Calibration
+{
+public:
+  Calibration();
+  virtual ~Calibration();
+
+  /*!
+   * \brief Corrects a UR kinematic chain in such a way that shoulder and elbow offsets are 0.
+   */
+  KDL::Chain correctChain();
+
+  /*!
+   * \brief Print information about every segment in the chain
+   *
+   */
+  static void debugChain(const KDL::Chain& chain);
+
+private:
+  /*!
+   * \brief Splits the given chain \p in into a subchain before \p split_index and after.
+   *
+   * \param[in,out] in Complete chain that will be split. Will be only the part before the \p
+   * split_index after the function returns.
+   * \param[out] second Will contain the subchain after \p split_index after the function returns.
+   * \param[in] split_index Index at which the chain should be split. The segment with this index
+   * will be the first segment in \p second.
+   *
+   * \returns true on success, false if \p split_index is larger than the chain size. In the latter
+   * case, \p in will not be touched.
+   */
+  bool splitChain(KDL::Chain& in, KDL::Chain& second, const size_t split_index);
+
+  KDL::Chain correctAxis(KDL::Chain& chain);
+
+  /*!
+   * \brief Converts a \p KDL::Chain representation into a \ref DHRobot representation
+   */
+  static DHRobot chainToDH(const KDL::Chain& chain);
+
+  /*!
+   * \brief Creates a \p KDL::Chain representation of the member \p robot_parameters_
+   */
+  KDL::Chain getChain();
+
+  /*!
+   * \brief Modifies the robot chain at segment \p correction_index
+   *
+   * Correction will set the \p d parameter at the segment to 0 and adapt the rest of the chain
+   * accordingly
+   *
+   * \param[in,out] robot_chain The kinemtatic chain to correct
+   * \param[in] correction_index Index at which the correction step should be performed.
+   *
+   * \returns true on success, false if the chain is smaller than the \p correction_index
+   */
+  bool correctChainAt(KDL::Chain& robot_chain, const size_t correction_index);
+
+  /*!
+   * \brief Given all parameters to correct the chain and a chain, create a corrected chain.
+   *
+   * \param[in] robot_chain Uncorrected chain from which the correction parameters were calculated.
+   * \param[in] new_length New segment length (\p a) of the corrected segment
+   * \param[in] new_theta New rotation offset (\p theta) of the corrected segment
+   * \param[in] distance_correction Distance by which the next segment has to be adapted due to the
+   * correction
+   *
+   * \returns The corrected chain, where the first segment's \p d parameter is 0
+   */
+  static KDL::Chain buildCorrectedChain(const KDL::Chain& robot_chain, const double new_length, const double new_theta, const double distance_correction);
+
+  DHRobot robot_parameters_;
+  std::vector<std::string> link_names_ = { "shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3" };
+};
+#endif  // ifndef UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED

package.xml

@@ -24,13 +24,16 @@
   <depend>controller_manager</depend>
   <depend>actionlib</depend>
   <depend>control_msgs</depend>
+  <depend>eigen_conversions</depend>
   <depend>geometry_msgs</depend>
+  <depend>kdl_parser</depend>
   <depend>industrial_msgs</depend>
   <depend>roscpp</depend>
   <depend>sensor_msgs</depend>
   <depend>trajectory_msgs</depend>
   <depend>ur_msgs</depend>
   <depend>tf</depend>
+  <depend>tf_conversions</depend>
   <depend>std_srvs</depend>
 
   <exec_depend>force_torque_sensor_controller</exec_depend>

src/calibration.cpp

@@ -0,0 +1,351 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-01-10
+ *
+ */
+//----------------------------------------------------------------------
+
+#include <ur_rtde_driver/calibration.h>
+
+#include <eigen_conversions/eigen_kdl.h>
+
+Calibration::Calibration()
+{
+  // This is the data of our ids-ur10-2 Later, dh parameters will be given using another
+  // constructor.
+
+  DHRobot my_robot;
+  my_robot.segments_.push_back(DHSegment(0.1273, 0, 0, M_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.163841, 0, 0.0, M_PI / 2));
+  my_robot.segments_.push_back(DHSegment(0.1157, 0, 0, -M_PI / 2));
+  my_robot.segments_.push_back(DHSegment(0.0922, 0, 0, 0));
+  // my_robot.segments_.push_back(DHSegment(0.1, 0.2, 0.3, 0.4));
+
+  // DHRobot my_robot_calibration;
+  //// clang-format off
+  ////                                                 d,    a,    theta, alpha
+  // my_robot_calibration.segments_.push_back(DHSegment(0    ,0    ,0     ,0    ));
+  // my_robot_calibration.segments_.push_back(DHSegment(1.5  ,0    ,0.0   ,0.0  ));
+  // my_robot_calibration.segments_.push_back(DHSegment(0.5  ,0.0  ,0.2   ,0.0  ));
+  // my_robot_calibration.segments_.push_back(DHSegment(-2   ,0    ,0     ,0    ));
+  // my_robot_calibration.segments_.push_back(DHSegment(0    ,0    ,0     ,0    ));
+  // my_robot_calibration.segments_.push_back(DHSegment(0    ,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
+
+  robot_parameters_ = my_robot + my_robot_calibration;
+}
+
+Calibration::~Calibration()
+{
+}
+
+KDL::Chain Calibration::correctChain()
+{
+  KDL::Chain robot_chain = getChain();
+
+  correctChainAt(robot_chain, 2);
+  correctChainAt(robot_chain, 4);
+
+  return robot_chain;
+}
+
+bool Calibration::correctChainAt(KDL::Chain& robot_chain, const size_t correction_index)
+{
+  bool success = true;
+  KDL::Chain correction_chain, corrected_subchain;
+
+  // Split chain into part before the correction joint and after. We then change the first link on
+  // the second chain. This splitting is necessary as KDL doesn't support forward kinematics
+  // relative to another link than the first.
+  success = splitChain(robot_chain, correction_chain, correction_index);
+
+  // Correct the chain after the correction joint
+  corrected_subchain = correctAxis(correction_chain);
+
+  // Add the corrected second part to the robot chain to get a full robot model again
+  robot_chain.addChain(corrected_subchain);
+
+  return success;
+}
+
+bool Calibration::splitChain(KDL::Chain& robot_chain, KDL::Chain& second_chain, const size_t split_index)
+{
+  if (split_index >= robot_chain.segments.size())
+  {
+    return false;
+  }
+
+  second_chain = KDL::Chain();
+  for (auto it = robot_chain.segments.begin() + split_index; it != robot_chain.segments.end();)
+  {
+    KDL::Segment new_segment = *it;
+    robot_chain.segments.erase(it);
+    second_chain.addSegment(new_segment);
+  }
+
+  return true;
+}
+
+KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain)
+{
+  // Each DH-Segment is split into two KDL segments. One representing the d and theta parameters and
+  // one with the a and alpha parameters. If we start from the first segment (which represents d and
+  // theta), there follows a passive segment (with a and alpha) and the next d/theta-segment after
+  // that.
+  //
+  // In principle, the d parameter of the first segment gets set to zero, first. With this change,
+  // 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
+  //   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.
+  //
+  //   -
+  KDL::Frame next_segment_frame;
+  KDL::Frame passive_frame;
+
+  KDL::ChainFkSolverPos_recursive fk_solver(robot_chain);
+  uint32_t num_jts = robot_chain.getNrOfJoints();
+  KDL::JntArray jointpositions = KDL::JntArray(num_jts);
+
+  fk_solver.JntToCart(jointpositions, passive_frame, 2);
+  ROS_INFO_STREAM(passive_frame.p.x() << ", " << passive_frame.p.y() << ", " << passive_frame.p.z());
+  fk_solver.JntToCart(jointpositions, next_segment_frame, 3);
+  ROS_INFO_STREAM(next_segment_frame.p.x() << ", " << next_segment_frame.p.y() << ", " << next_segment_frame.p.z());
+
+  Eigen::Vector3d eigen_passive;
+  tf::vectorKDLToEigen(passive_frame.p, eigen_passive);
+  Eigen::Vector3d eigen_next;
+  tf::vectorKDLToEigen(next_segment_frame.p, eigen_next);
+
+  // 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);
+
+  // ROS_INFO_STREAM("next_line:" << std::endl
+  //<< "Base:" << std::endl
+  //<< next_line.origin() << std::endl
+  //<< "Direction:" << std::endl
+  //<< next_line.direction());
+
+  // XY-Plane of first segment's start
+  Eigen::Hyperplane<double, 3> plane(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, 0, 0));
+
+  // 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);
+  double new_theta = std::atan(intersection.y() / intersection.x());
+  // Upper and lower arm segments on URs all have negative length due to dh params
+  double new_length = -1 * intersection.norm();
+  // ROS_INFO_STREAM("Wrist line intersecting at " << std::endl << intersection);
+  // ROS_INFO_STREAM("Angle is " << new_theta);
+  // ROS_INFO_STREAM("Length is " << new_length);
+  // ROS_INFO_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 distance_correction = intersection_param * sign_dir;
+
+  ROS_INFO_STREAM("Corrected chain at " << robot_chain.segments[0].getName());
+  return buildCorrectedChain(robot_chain, new_length, new_theta, distance_correction);
+}
+
+KDL::Chain Calibration::buildCorrectedChain(const KDL::Chain& robot_chain, const double new_length, const double new_theta,
+                                      const double distance_correction)
+{
+  KDL::Chain corrected_chain;
+
+  // Set d parameter of the first segment to 0 and theta to the calculated new angle
+  {
+    KDL::Frame frame = KDL::Frame::DH(0, 0, 0, new_theta);
+    KDL::Joint joint = KDL::Joint(KDL::Vector(0, 0, 0), KDL::Vector(0, 0, 1), KDL::Joint::RotAxis);
+    KDL::Segment segment = KDL::Segment(robot_chain.segments[0].getName(), joint, frame);
+    corrected_chain.addSegment(segment);
+  }
+
+  // Correct arm segment length and angle
+  {
+    // Get the original theta from the first link
+    double roll_a, pitch_a, yaw_a;
+    robot_chain.segments[0].getFrameToTip().M.GetRPY(roll_a, pitch_a, yaw_a);
+
+    double roll, pitch, yaw;
+    robot_chain.segments[1].getFrameToTip().M.GetRPY(roll, pitch, yaw);
+
+    // Since we changed the kinematic structure, we have to make sure to rotate around the correct
+    // axis, so we revert the theta change in this joint, again.
+    KDL::Rotation rotation = KDL::Rotation::EulerZYX(yaw_a - new_theta, 0, roll);
+    KDL::Vector position(new_length, 0, 0);
+    KDL::Frame frame(rotation, position);
+
+    // KDL::Frame frame = KDL::Frame::DH(new_length, roll, 0, yaw_a - new_theta);
+
+    KDL::Segment segment = KDL::Segment(robot_chain.segments[1].getName(), KDL::Joint(KDL::Joint::None), frame);
+    corrected_chain.addSegment(segment);
+  }
+
+  // Correct next joint
+  {
+    KDL::Frame new_frame = robot_chain.getSegment(2).getFrameToTip();
+    new_frame.p = robot_chain.getSegment(2).getFrameToTip().p;
+    ROS_INFO_STREAM("Correcting segment i+2 length from " << new_frame.p.z());
+
+
+    // the d-parameter can be modified by the intersection_parameter which is the distance traveled
+    // along the rotation axis
+    new_frame.p = KDL::Vector(0, 0, new_frame.p.z() - distance_correction);
+
+    ROS_INFO_STREAM("Corrected segment i+2 length to " << new_frame.p.z());
+    KDL::Joint new_joint = robot_chain.getSegment(2).getJoint();
+
+    KDL::Segment segment = KDL::Segment(robot_chain.getSegment(2).getName(), new_joint, new_frame);
+    corrected_chain.addSegment(segment);
+  }
+
+  for (size_t i = 3; i < robot_chain.segments.size(); ++i)
+  {
+    corrected_chain.addSegment(robot_chain.segments[i]);
+  }
+
+  return corrected_chain;
+}
+
+DHRobot Calibration::chainToDH(const KDL::Chain& chain)
+{
+  KDL::ChainFkSolverPos_recursive fk_solver(chain);
+  uint32_t num_jts = chain.getNrOfJoints();
+  KDL::JntArray jointpositions = KDL::JntArray(num_jts);
+  // Assign some values to the joint positions
+  for (unsigned int i = 0; i < num_jts; i++)
+  {
+    jointpositions(i) = 0;
+  }
+
+  DHRobot robot;
+
+  for (size_t i = 0; i < chain.segments.size(); i += 2)
+  {
+    ROS_INFO_STREAM("Extracting DH parameters for joint " << i);
+    for (size_t j = i; j < i + 2; ++j)
+    {
+      // ROS_INFO_STREAM(j << " < " << i << "+2");
+      KDL::Frame result;
+      result = chain.segments[j].getFrameToTip();
+      ROS_INFO_STREAM("Relative position: " << result.p.x() << ", " << result.p.y() << ", " << result.p.z());
+      double roll, pitch, yaw;
+      result.M.GetRPY(roll, pitch, yaw);
+      ROS_INFO_STREAM("Relative rotation: " << roll << ", " << pitch << ", " << yaw);
+      fk_solver.JntToCart(jointpositions, result, j);
+      KDL::Joint joint = chain.segments[j].getJoint();
+    }
+
+    DHSegment seg;
+    {
+      KDL::Frame result = chain.segments[i].getFrameToTip();
+      double roll, pitch, yaw;
+      result.M.GetRPY(roll, pitch, yaw);
+      seg.theta_ = yaw;
+      seg.d_ = result.p.z();
+    }
+    {
+      KDL::Frame result = chain.segments[i + 1].getFrameToTip();
+      double roll, pitch, yaw;
+      result.M.GetRPY(roll, pitch, yaw);
+      seg.alpha_ = roll;
+      seg.a_ = result.p.x();
+    }
+
+    robot.segments_.push_back(seg);
+  }
+
+  {
+    double roll, pitch, yaw;
+    chain.segments[3].getFrameToTip().M.GetRPY(roll, pitch, yaw);
+    robot.delta_theta_correction2_ = yaw;
+    chain.segments[5].getFrameToTip().M.GetRPY(roll, pitch, yaw);
+    robot.delta_theta_correction3_ = yaw;
+  }
+
+  return robot;
+}
+
+KDL::Chain Calibration::getChain()
+{
+  KDL::Chain robot_chain;
+  for (size_t i = 0; i < robot_parameters_.segments_.size(); ++i)
+  {
+    KDL::Frame frame = KDL::Frame::DH(0, 0, robot_parameters_.segments_[i].d_, robot_parameters_.segments_[i].theta_);
+    KDL::Joint joint = KDL::Joint(link_names_[i], KDL::Vector(0, 0, robot_parameters_.segments_[i].d_),
+                                  KDL::Vector(0, 0, 1), KDL::Joint::RotAxis);
+
+    KDL::Segment segment = KDL::Segment(link_names_[i], joint, frame);
+    robot_chain.addSegment(segment);
+    KDL::Frame frame2 = KDL::Frame::DH(robot_parameters_.segments_[i].a_, robot_parameters_.segments_[i].alpha_, 0, 0);
+    KDL::Segment segment2 =
+        KDL::Segment(link_names_[i] + "_passive", KDL::Joint(link_names_[i] + "_passive", KDL::Joint::None), frame2);
+    robot_chain.addSegment(segment2);
+  }
+  return robot_chain;
+}
+
+void Calibration::debugChain(const KDL::Chain& robot_chain)
+{
+  uint32_t num_jts = robot_chain.getNrOfJoints();
+
+  KDL::ChainFkSolverPos_recursive fk_solver(robot_chain);
+  KDL::JntArray jointpositions = KDL::JntArray(num_jts);
+  // Assign some values to the joint positions
+  for (unsigned int i = 0; i < num_jts; i++)
+  {
+    jointpositions(i) = 0;
+  }
+
+  for (size_t i = 0; i < robot_chain.segments.size(); ++i)
+  {
+    ROS_INFO_STREAM("Segment " << i << ": " << robot_chain.segments[i].getName());
+    KDL::Frame result;
+    result = robot_chain.segments[i].getFrameToTip();
+    ROS_INFO_STREAM("Relative position: " << result.p.x() << ", " << result.p.y() << ", " << result.p.z());
+    double roll, pitch, yaw;
+    result.M.GetRPY(roll, pitch, yaw);
+    ROS_INFO_STREAM("Relative rotation: " << roll << ", " << pitch << ", " << yaw);
+    fk_solver.JntToCart(jointpositions, result, i);
+    ROS_INFO_STREAM(std::setprecision(15) << "Absolute position: " << result.p.x() << ", " << result.p.y() << ", " << result.p.z());
+    KDL::Joint joint = robot_chain.segments[i].getJoint();
+    ROS_INFO_STREAM("Joint type: " << joint.getTypeName());
+    ROS_INFO_STREAM("Joint position: " << joint.pose(0).p.x() << ", " << joint.pose(0).p.y() << ", "
+                                       << joint.pose(0).p.z());
+    ROS_INFO_STREAM("Joint Axis: " << joint.JointAxis().x() << "," << joint.JointAxis().y() << ", "
+                                   << joint.JointAxis().z());
+  }
+
+  DHRobot robot_out = Calibration::chainToDH(robot_chain);
+  ROS_INFO_STREAM("Robot data:\n" << robot_out.toXacroProperties());
+}

src/calibration_correction.cpp

@@ -0,0 +1,25 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-01-10
+ *
+ */
+//----------------------------------------------------------------------
+
+
+#include <ur_rtde_driver/calibration.h>
+
+int main(int argc, char* argv[])
+{
+  Calibration calibration;
+  KDL::Chain corrected_chain = calibration.correctChain();
+  calibration.debugChain(corrected_chain);
+
+  return 0;
+}