Merge branch 'calibration_new'

This introduces a separate tool for extracting calibration data from the robot
and generating a parameter file that can be used by the ur_description package
to generate a correct URDF model.

ur_calibration/CMakeLists.txt

@@ -0,0 +1,189 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ur_calibration)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  ur_rtde_driver
+)
+find_package(Eigen3 REQUIRED)
+find_package(yaml-cpp REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES ur_calibration
+  CATKIN_DEPENDS
+    roscpp
+    ur_rtde_driver
+  DEPENDS
+    yaml-cpp
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+add_executable(calibration_correction
+  src/calibration.cpp
+  src/calibration_consumer.cpp
+  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 ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+if (CATKIN_ENABLE_TESTING)
+  catkin_add_gtest(calibration_test
+    test/calibration_test.cpp
+    src/calibration.cpp)
+  target_link_libraries(calibration_test ${catkin_LIBRARIES} yaml-cpp)
+endif()
+
+install(TARGETS calibration_correction RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

ur_calibration/README.md

@@ -0,0 +1,39 @@
+# ur_calibration
+
+Package for extracting the factory calibration from a UR robot and change it such that it can be used by `ur_description` to gain a correct URDF
+
+## Nodes
+### calibration_correction
+This node extracts calibration information directly from a robot, calculates the URDF correction and
+saves it into a yaml file.
+
+With the parameters explained below calibration will be saved inside
+```bash
+<output_package_name>/<subfolder>/<robot_name>_calibration.yaml
+```
+
+#### Example usage
+```bash
+rosrun ur_calibration calibration_correction _robot_ip:=192.168.56.101 _robot_name:=ur10_ideal _output_package_name:=ur_calibration
+```
+
+#### Parameters
+ * **"~subfolder_name"** (default: "etc")
+
+     Given a package where the output should be saved, the calibration file will be saved in this
+     subfolder relative to the package root.
+
+ * **"~robot_ip"** (required)
+
+    IP address of the robot. The robot has to be reachable with this ip from the machine running
+    this node.
+
+ * **"~robot_name"** (required)
+
+    Arbitrary name that will be used for generating the calibration file's filename (see node
+    description).
+
+ * **"~output_package_name"** (required)
+
+    Package inside which the calibration data will be stored in. This package has to exist and has
+    to be writable. Otherwise execution will fail and calibration data won't be saved.

ur_calibration/include/ur_calibration/calibration.h

@@ -0,0 +1,186 @@
+// 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 <Eigen/Dense>
+#include <yaml-cpp/yaml.h>
+#include <fstream>
+
+namespace ur_calibration
+{
+/*!
+ * \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)
+  {
+    delta_theta_correction2_ = 0;
+    delta_theta_correction3_ = 0;
+  }
+
+  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 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(const DHRobot& robot);
+  virtual ~Calibration();
+
+  /*!
+   * \brief Corrects a UR kinematic chain in such a way that shoulder and elbow offsets are 0.
+   */
+  void correctChain();
+
+  /*!
+   * \brief Get the transformation matrix representation of the chain as constructed by the
+   * DH parameters.
+   *
+   * This will contain twice as many transformation matrices as joints, as for each set of DH
+   * parameters two matrices are generated. If you'd like to receive one matrix per joint instead,
+   * use the getSimplified() function instead.
+   *
+   * \returns A vector of 4x4 transformation matrices, two for each joint going from the base to the
+   * tcp.
+   */
+  std::vector<Eigen::Matrix4d> getChain()
+  {
+    return chain_;
+  }
+
+  /*!
+   * \brief Get the transformation matrix representation of the chain, where each joint is
+   * represented by one matrix.
+   *
+   * \returns Vector of 4x4 transformation matrices, one for each joint going from the base to the
+   * tcp.
+   */
+  std::vector<Eigen::Matrix4d> getSimplified() const;
+
+  /*!
+   * \brief Generates a yaml representation of all transformation matrices as returned by
+   * getSimplified()
+   *
+   * \returns A YAML tree representing all tranformation matrices.
+   */
+  YAML::Node toYaml() const;
+
+  /*!
+   * \brief Calculates the forwart kinematics given a joint configuration with respect to the base
+   * link.
+   *
+   * \param joint_values Joint values for which the forward kinematics should be calculated.
+   * \param link_nr If given, the cartesian position for this joint (starting at 1) is returned. By
+   * default the 6th joint is used.
+   *
+   * \returns Transformation matrix giving the full pose of the requested link in base coordinates.
+   */
+  Eigen::Matrix4d calcForwardKinematics(const Eigen::Matrix<double, 6, 1>& joint_values, const size_t link_nr = 6);
+
+private:
+  // Corrects a single axis
+  void correctAxis(const size_t correction_index);
+
+  // Builds the chain from robot_parameters_
+  void buildChain();
+
+  DHRobot robot_parameters_;
+  std::vector<std::string> link_names_ = { "shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3" };
+
+  std::vector<Eigen::Matrix4d> chain_;
+};
+}  // namespace ur_calibration
+#endif  // ifndef UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED

ur_calibration/include/ur_calibration/calibration_consumer.h

@@ -0,0 +1,60 @@
+// 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-05-28
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_CALIBRATION_CALIBRATION_CONSUMER_H_INCLUDED
+#define UR_CALIBRATION_CALIBRATION_CONSUMER_H_INCLUDED
+#include <ur_rtde_driver/comm/pipeline.h>
+
+#include <ur_rtde_driver/primary/robot_state/kinematics_info.h>
+
+#include <ur_calibration/calibration.h>
+
+namespace ur_calibration
+{
+class CalibrationConsumer
+  : public ur_driver::comm::IConsumer<ur_driver::comm::URPackage<ur_driver::primary_interface::PackageHeader>>
+{
+public:
+  CalibrationConsumer();
+  virtual ~CalibrationConsumer() = default;
+
+  virtual void setupConsumer()
+  {
+  }
+  virtual void teardownConsumer()
+  {
+  }
+  virtual void stopConsumer()
+  {
+  }
+  virtual void onTimeout()
+  {
+  }
+
+  virtual bool
+  consume(std::shared_ptr<ur_driver::comm::URPackage<ur_driver::primary_interface::PackageHeader>> product);
+
+  bool isCalibrated() const
+  {
+    return calibrated_;
+  }
+
+  YAML::Node getCalibrationParameters() const;
+
+private:
+  bool calibrated_;
+  YAML::Node calibration_parameters_;
+};
+}  // namespace ur_calibration
+#endif  // ifndef UR_CALIBRATION_CALIBRATION_CONSUMER_H_INCLUDED

ur_calibration/package.xml

@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ur_calibration</name>
+  <version>0.0.1</version>
+  <description>Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="mauch@fzi.de">Felix Mauch</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>Apache 2.0</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/ur_calibration</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>roscpp</depend>
+  <depend>ur_rtde_driver</depend>
+  <depend>yaml-cpp</depend>
+
+
+  <test_depend>rosunit</test_depend>
+</package>

ur_calibration/src/calibration.cpp

@@ -0,0 +1,196 @@
+// 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_calibration/calibration.h>
+
+namespace ur_calibration
+{
+Calibration::Calibration(const DHRobot& robot_parameters) : robot_parameters_(robot_parameters)
+{
+  buildChain();
+}
+
+Calibration::~Calibration()
+{
+}
+
+void Calibration::correctChain()
+{
+  correctAxis(1);
+  correctAxis(2);
+}
+
+void Calibration::correctAxis(const size_t link_index)
+{
+  // Each DH-Segment is split into two chain 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.
+
+  if (chain_[2 * link_index](2, 3) == 0.0)
+  {
+    // Nothing to do here.
+    return;
+  }
+
+  Eigen::Matrix4d fk_current = Eigen::Matrix4d::Identity();
+  Eigen::Vector3d current_passive = Eigen::Vector3d::Zero();
+
+  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 eigen_next = (fk_next_passive * chain_[(link_index + 1) * 2]).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);
+
+  ROS_DEBUG_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(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());
+  // 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);
+  ROS_DEBUG_STREAM("Angle is " << new_theta);
+  ROS_DEBUG_STREAM("Length is " << new_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 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();
+
+  // Correct arm segment length and angle
+  chain_[2 * link_index + 1](0, 3) = new_length;
+  chain_[2 * link_index + 1].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
+  chain_[2 * link_index + 2](2, 3) -= distance_correction;
+}
+
+Eigen::Matrix4d Calibration::calcForwardKinematics(const Eigen::Matrix<double, 6, 1>& joint_values,
+                                                   const size_t link_nr)
+{
+  // Currently ignore input and calculate for zero vector input
+  Eigen::Matrix4d output = Eigen::Matrix4d::Identity();
+
+  std::vector<Eigen::Matrix4d> simplified_chain = getSimplified();
+  for (size_t i = 0; i < link_nr; ++i)
+  {
+    Eigen::Matrix4d rotation = Eigen::Matrix4d::Identity();
+    rotation.topLeftCorner(3, 3) = Eigen::AngleAxisd(joint_values(i), Eigen::Vector3d::UnitZ()).toRotationMatrix();
+    output *= simplified_chain[i] * rotation;
+  }
+
+  return output;
+}
+
+void Calibration::buildChain()
+{
+  chain_.clear();
+  for (size_t i = 0; i < robot_parameters_.segments_.size(); ++i)
+  {
+    Eigen::Matrix4d seg1_mat = Eigen::Matrix4d::Identity();
+    seg1_mat.topLeftCorner(3, 3) =
+        Eigen::AngleAxisd(robot_parameters_.segments_[i].theta_, Eigen::Vector3d::UnitZ()).toRotationMatrix();
+    seg1_mat(2, 3) = robot_parameters_.segments_[i].d_;
+
+    chain_.push_back(seg1_mat);
+
+    Eigen::Matrix4d seg2_mat = Eigen::Matrix4d::Identity();
+    seg2_mat.topLeftCorner(3, 3) =
+        Eigen::AngleAxisd(robot_parameters_.segments_[i].alpha_, Eigen::Vector3d::UnitX()).toRotationMatrix();
+    seg2_mat(0, 3) = robot_parameters_.segments_[i].a_;
+
+    chain_.push_back(seg2_mat);
+  }
+}
+
+std::vector<Eigen::Matrix4d> Calibration::getSimplified() const
+{
+  std::vector<Eigen::Matrix4d> simplified_chain;
+  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.push_back(chain_.back());
+  return simplified_chain;
+}
+
+YAML::Node Calibration::toYaml() const
+{
+  YAML::Node node;
+
+  std::vector<Eigen::Matrix4d> chain = getSimplified();
+
+  for (std::size_t i = 0; i < link_names_.size(); ++i)
+  {
+    YAML::Node link;
+    link["x"] = chain[i](0, 3);
+    link["y"] = chain[i](1, 3);
+    link["z"] = chain[i](2, 3);
+    Eigen::Matrix3d rot = chain[i].topLeftCorner(3, 3);
+    Eigen::Vector3d rpy = rot.eulerAngles(0, 1, 2);
+    link["roll"] = rpy[0];
+    link["pitch"] = rpy[1];
+    link["yaw"] = rpy[2];
+    node["kinematics"][link_names_[i]] = link;
+  }
+
+  return node;
+}
+}  // namespace ur_calibration

ur_calibration/src/calibration_consumer.cpp

@@ -0,0 +1,53 @@
+// 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-05-28
+ *
+ */
+//----------------------------------------------------------------------
+
+#include <ur_calibration/calibration_consumer.h>
+
+namespace ur_calibration
+{
+CalibrationConsumer::CalibrationConsumer() : calibrated_(false)
+{
+}
+
+bool CalibrationConsumer::consume(
+    std::shared_ptr<ur_driver::comm::URPackage<ur_driver::primary_interface::PackageHeader>> product)
+{
+  auto kin_info = std::dynamic_pointer_cast<ur_driver::primary_interface::KinematicsInfo>(product);
+  if (kin_info != nullptr)
+  {
+    LOG_INFO("%s", product->toString().c_str());
+    DHRobot my_robot;
+    for (size_t i = 0; i < kin_info->dh_a_.size(); ++i)
+    {
+      my_robot.segments_.push_back(
+          DHSegment(kin_info->dh_d_[i], kin_info->dh_a_[i], kin_info->dh_theta_[i], kin_info->dh_alpha_[i]));
+    }
+    Calibration calibration(my_robot);
+    calibration.correctChain();
+
+    calibration_parameters_ = calibration.toYaml();
+    calibrated_ = true;
+  }
+  return true;
+}
+
+YAML::Node CalibrationConsumer::getCalibrationParameters() const
+{
+  if (!calibrated_)
+  {
+    throw(std::runtime_error("Cannot get calibration, as no calibration data received yet"));
+  }
+  return calibration_parameters_;
+}
+}  // namespace ur_calibration

ur_calibration/src/calibration_correction.cpp

@@ -0,0 +1,184 @@
+// 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_calibration/calibration_consumer.h>
+
+#include <ur_rtde_driver/comm/parser.h>
+#include <ur_rtde_driver/comm/pipeline.h>
+#include <ur_rtde_driver/comm/producer.h>
+#include <ur_rtde_driver/comm/stream.h>
+#include <ur_rtde_driver/primary/package_header.h>
+#include <ur_rtde_driver/primary/primary_parser.h>
+
+#include <sensor_msgs/JointState.h>
+#include <tf/transform_listener.h>
+#include <ros/package.h>
+
+#include <boost/filesystem.hpp>
+
+namespace fs = boost::filesystem;
+
+using namespace ur_driver;
+using namespace primary_interface;
+using namespace ur_calibration;
+
+static const int UR_PRIMARY_PORT = 30001;
+
+class ParamaterMissingException : public ros::Exception
+{
+public:
+  ParamaterMissingException(const std::string& name)
+    : Exception("Cannot find required parameter " + name + " on the parameter server.")
+  {
+  }
+};
+
+class CalibrationCorrection
+{
+public:
+  CalibrationCorrection(const ros::NodeHandle& nh) : nh_(nh)
+  {
+    subfolder_ = nh_.param<std::string>("subfolder_name", "etc");
+    std::string output_package_name;
+    try
+    {
+      // The robot's IP address
+      robot_ip_ = getRequiredParameter<std::string>("robot_ip");
+
+      // Name with which the robot will be referenced. Will be used for the filename the calibration
+      // data is stored in. This can be any arbitrary name. If left empty, the robot's serial number
+      // will be used.
+      robot_name_ = getRequiredParameter<std::string>("robot_name");
+
+      // The resulting parameter file will be stored inside
+      // <output_package_name>/<subfolder>/<robot_name>_calibration.yaml
+      output_package_name = getRequiredParameter<std::string>("output_package_name");
+    }
+    catch (const ParamaterMissingException& e)
+    {
+      ROS_FATAL_STREAM(e.what());
+      exit(1);
+    }
+    package_path_ = ros::package::getPath(output_package_name);
+    if (package_path_ == "")
+    {
+      ROS_FATAL_STREAM("Could not find package " << output_package_name << ". Cannot save output file there.");
+      exit(1);
+    }
+  }
+
+  virtual ~CalibrationCorrection() = default;
+
+  void run()
+  {
+    comm::URStream<PackageHeader> stream(robot_ip_, UR_PRIMARY_PORT);
+    primary_interface::PrimaryParser parser;
+    comm::URProducer<PackageHeader> prod(stream, parser);
+    CalibrationConsumer consumer;
+
+    comm::INotifier notifier;
+
+    comm::Pipeline<PackageHeader> pipeline(prod, consumer, "Pipeline", notifier);
+    pipeline.run();
+    while (!consumer.isCalibrated())
+    {
+      ros::Duration(0.1).sleep();
+    }
+    calibration_data_.reset(new YAML::Node);
+    *calibration_data_ = consumer.getCalibrationParameters();
+  }
+
+  bool writeCalibrationData()
+  {
+    if (calibration_data_ == nullptr)
+    {
+      ROS_ERROR_STREAM("Calibration data not yet set.");
+      return false;
+    }
+
+    ROS_INFO_STREAM("Writing calibration data");
+    fs::path dst_path = fs::path(package_path_) / fs::path(subfolder_);
+    if (!fs::exists(dst_path))
+    {
+      try
+      {
+        fs::create_directory(dst_path);
+      }
+      catch (const boost::filesystem::filesystem_error& e)
+      {
+        ROS_ERROR_STREAM("Could not create " << dst_path << ". Reason: " << e.what());
+        return false;
+      }
+    }
+    fs::path output_filename = dst_path / fs::path(robot_name_ + "_calibration.yaml");
+    if (fs::exists(output_filename))
+    {
+      ROS_WARN_STREAM("Output file " << output_filename << " already exists. Overwriting.");
+    }
+    std::ofstream file(output_filename.string());
+    if (file.is_open())
+    {
+      file << *calibration_data_;
+    }
+    else
+    {
+      ROS_ERROR_STREAM("Failed writing the file. Do you have the correct rights?");
+      return false;
+    }
+    ROS_INFO_STREAM("Wrote output to " << output_filename);
+
+    return true;
+  }
+
+private:
+  template <typename T>
+  T getRequiredParameter(const std::string& param_name) const
+  {
+    T ret_val;
+    if (nh_.hasParam(param_name))
+    {
+      nh_.getParam(param_name, ret_val);
+    }
+    else
+    {
+      throw ParamaterMissingException(nh_.resolveName(param_name));
+    }
+
+    return ret_val;
+  }
+
+  ros::NodeHandle nh_;
+  std::string robot_ip_;
+  std::string robot_name_;
+  std::string package_path_;
+  std::string subfolder_;
+
+  std::unique_ptr<YAML::Node> calibration_data_;
+};
+
+int main(int argc, char* argv[])
+{
+  ros::init(argc, argv, "ur_calibration");
+  ros::NodeHandle nh("~");
+
+  CalibrationCorrection my_calibration_correction(nh);
+  my_calibration_correction.run();
+  if (!my_calibration_correction.writeCalibrationData())
+  {
+    ROS_ERROR_STREAM("Failed writing calibration data. See errors above for details.");
+    return -1;
+  }
+  ROS_INFO("Calibration correction done");
+  return 0;
+}

ur_calibration/test/calibration_test.cpp

@@ -0,0 +1,178 @@
+// 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-03-11
+ *
+ */
+//----------------------------------------------------------------------
+
+#include <gtest/gtest.h>
+#include <ur_calibration/calibration.h>
+
+using namespace ur_calibration;
+
+namespace
+{
+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)
+{
+  for (size_t i = 0; i < dim_; ++i)
+  {
+    EXPECT_NEAR(vec1[i], vec2[i], precision);
+  }
+}
+
+TEST(UrRtdeDriver, ur10_fw_kinematics_ideal)
+{
+  DHRobot my_robot;
+  const double pi = std::atan(1) * 4;
+
+  // 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
+
+  Calibration calibration(my_robot);
+
+  Eigen::Matrix<double, 6, 1> jointvalues;
+  Eigen::Vector3d expected_position;
+  Eigen::Quaterniond expected_orientation;
+  {
+    jointvalues << 0, 0, 0, 0, 0, 0;
+    Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues);
+    EXPECT_DOUBLE_EQ(fk(0, 3), my_robot.segments_[1].a_ + my_robot.segments_[2].a_);
+    EXPECT_DOUBLE_EQ(fk(1, 3), -1 * (my_robot.segments_[3].d_ + my_robot.segments_[5].d_));
+    EXPECT_DOUBLE_EQ(fk(2, 3), my_robot.segments_[0].d_ - my_robot.segments_[4].d_);
+  }
+
+  {
+    jointvalues << M_PI_2, -M_PI_4, M_PI_2, -M_PI_4, 0, 0;
+    Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues);
+
+    expected_position << my_robot.segments_[3].d_ + my_robot.segments_[5].d_,
+        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);
+  }
+}
+
+TEST(UrRtdeDriver, 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
+
+  Calibration calibration(my_robot);
+
+  Eigen::Matrix<double, 6, 1> jointvalues;
+  Eigen::Vector3d expected_position;
+  Eigen::Quaterniond expected_orientation;
+  {
+    jointvalues << -1.6007002035724084976209269370884, -1.7271001974688928726209269370884,
+        -2.2029998938189905288709269370884, -0.80799991289247685699592693708837, 1.59510004520416259765625,
+        -0.03099996248354131012092693708837;
+    expected_position << -0.179925914147547, -0.606869755162764, 0.230789102067257;
+
+    Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues);
+
+    doubleEqVec<double, 3>(expected_position, fk.topRightCorner(3, 1), 1e-15);
+  }
+  {
+    jointvalues << 1.32645022869110107421875, 2.426007747650146484375, 5.951572895050048828125,
+        1.27409040927886962890625, -0.54105216661562138824592693708837, 0.122173048555850982666015625;
+    expected_position << 0.39922988003280424074148413637886, 0.59688365069340565405298093537567,
+        -0.6677819040276375961440180617501;
+
+    Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues);
+
+    doubleEqVec<double, 3>(expected_position, fk.topRightCorner(3, 1), 1e-15);
+  }
+}
+
+TEST(UrRtdeDriver, 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;
+
+  // 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
+
+  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);
+    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);
+    Eigen::Quaterniond rot_orig(rot_mat_orig);
+    calibration.correctChain();
+    Eigen::Matrix4d fk_corrected = calibration.calcForwardKinematics(jointvalues);
+    Eigen::Matrix3d rot_mat_corrected = fk_corrected.topLeftCorner(3, 3);
+    Eigen::Quaterniond rot_corrected(rot_mat_corrected);
+    double angle_error = std::abs(rot_orig.angularDistance(rot_corrected));
+    EXPECT_NEAR(fk_orig(0, 3), fk_corrected(0, 3), 1e-12);
+    EXPECT_NEAR(fk_orig(1, 3), fk_corrected(1, 3), 1e-12);
+    EXPECT_NEAR(fk_orig(2, 3), fk_corrected(2, 3), 1e-12);
+    EXPECT_NEAR(angle_error, 0.0, 1e-12);
+  }
+}
+}  // namespace
+
+int main(int argc, char* argv[])
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  return RUN_ALL_TESTS();
+}

ur_rtde_driver/CMakeLists.txt

@@ -19,7 +19,6 @@ find_package(catkin REQUIRED
     roscpp
     sensor_msgs
     std_srvs
-    tf
     trajectory_msgs
     ur_controllers
     ur_msgs
@@ -43,6 +42,7 @@ catkin_package(
     trajectory_msgs
     ur_controllers
     ur_msgs
+    std_srvs
   DEPENDS
     Boost
 )
@@ -127,14 +127,3 @@ install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
   FILES_MATCHING PATTERN "*.h"
 )
-
-
-#if (CATKIN_ENABLE_TESTING)
-  #set(${PROJECT_NAME}_TEST_SOURCES
-    #tests/ur/master_board.cpp
-    #tests/ur/robot_mode.cpp
-    #tests/ur/rt_state.cpp)
-
-  #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()

ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h

@@ -26,6 +26,7 @@
 #include <cstddef>
 #include <cstring>
 #include <string>
+#include <memory>
 #include "ur_rtde_driver/log.h"
 #include "ur_rtde_driver/types.h"
 

ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h

@@ -154,6 +154,7 @@ public:
 
   virtual ~Pipeline()
   {
+    LOG_INFO("Destructing pipeline");
     stop();
   }
 

ur_rtde_driver/include/ur_rtde_driver/primary/primary_parser.h

@@ -123,7 +123,7 @@ public:
 
       default:
       {
-        LOG_WARN("Invalid robot package type recieved: %u", static_cast<uint8_t>(type));
+        LOG_DEBUG("Invalid robot package type recieved: %u", static_cast<uint8_t>(type));
         bp.consume();
         return true;
       }

ur_rtde_driver/include/ur_rtde_driver/primary/robot_state.h

@@ -28,8 +28,7 @@
 #ifndef UR_RTDE_DRIVER_ROBOT_STATE_H_INCLUDED
 #define UR_RTDE_DRIVER_ROBOT_STATE_H_INCLUDED
 
-#include <mutex>
+#include <sstream>
-#include <condition_variable>
 
 #include "ur_rtde_driver/primary/primary_package.h"
 #include "ur_rtde_driver/primary/package_header.h"

ur_rtde_driver/package.xml

@@ -31,7 +31,6 @@
   <depend>trajectory_msgs</depend>
   <depend>ur_controllers</depend>
   <depend>ur_msgs</depend>
-  <depend>tf</depend>
   <depend>std_srvs</depend>
 
   <exec_depend>force_torque_sensor_controller</exec_depend>

ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp

@@ -15,6 +15,8 @@
 #include "ur_rtde_driver/log.h"
 #include "ur_rtde_driver/primary/robot_state/kinematics_info.h"
 
+#include <iomanip>
+
 namespace ur_driver
 {
 namespace primary_interface
@@ -43,28 +45,28 @@ std::string KinematicsInfo::toString() const
   os << "dh_theta: [";
   for (size_t i = 0; i < dh_theta_.size(); ++i)
   {
-    os << dh_theta_[i] << " ";
+    os << std::setprecision(15) << dh_theta_[i] << " ";
   }
   os << "]" << std::endl;
 
   os << "dh_a: [";
   for (size_t i = 0; i < dh_a_.size(); ++i)
   {
-    os << dh_a_[i] << " ";
+    os << std::setprecision(15) << dh_a_[i] << " ";
   }
   os << "]" << std::endl;
 
   os << "dh_d: [";
   for (size_t i = 0; i < dh_d_.size(); ++i)
   {
-    os << dh_d_[i] << " ";
+    os << std::setprecision(15) << dh_d_[i] << " ";
   }
   os << "]" << std::endl;
 
   os << "dh_alpha: [";
   for (size_t i = 0; i < dh_alpha_.size(); ++i)
   {
-    os << dh_alpha_[i] << " ";
+    os << std::setprecision(15) << dh_alpha_[i] << " ";
   }
   os << "]" << std::endl;