diff --git a/ur_calibration/CMakeLists.txt b/ur_calibration/CMakeLists.txt new file mode 100644 index 0000000..6b8c026 --- /dev/null +++ b/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}) diff --git a/ur_calibration/README.md b/ur_calibration/README.md new file mode 100644 index 0000000..bbae9ff --- /dev/null +++ b/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 +//_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. diff --git a/ur_calibration/include/ur_calibration/calibration.h b/ur_calibration/include/ur_calibration/calibration.h new file mode 100644 index 0000000..3283749 --- /dev/null +++ b/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 +#include +#include +#include + +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 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& 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 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 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& 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 link_names_ = { "shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3" }; + + std::vector chain_; +}; +} // namespace ur_calibration +#endif // ifndef UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED diff --git a/ur_calibration/include/ur_calibration/calibration_consumer.h b/ur_calibration/include/ur_calibration/calibration_consumer.h new file mode 100644 index 0000000..e871cf8 --- /dev/null +++ b/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 + +#include + +#include + +namespace ur_calibration +{ +class CalibrationConsumer + : public ur_driver::comm::IConsumer> +{ +public: + CalibrationConsumer(); + virtual ~CalibrationConsumer() = default; + + virtual void setupConsumer() + { + } + virtual void teardownConsumer() + { + } + virtual void stopConsumer() + { + } + virtual void onTimeout() + { + } + + virtual bool + consume(std::shared_ptr> 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 diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml new file mode 100644 index 0000000..e15e130 --- /dev/null +++ b/ur_calibration/package.xml @@ -0,0 +1,58 @@ + + + ur_calibration + 0.0.1 + 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 + + + + + Felix Mauch + + + + + + Apache 2.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + ur_rtde_driver + yaml-cpp + + + rosunit + diff --git a/ur_calibration/src/calibration.cpp b/ur_calibration/src/calibration.cpp new file mode 100644 index 0000000..1233c91 --- /dev/null +++ b/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 + +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 next_line; + next_line = Eigen::ParametrizedLine::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 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& joint_values, + const size_t link_nr) +{ + // Currently ignore input and calculate for zero vector input + Eigen::Matrix4d output = Eigen::Matrix4d::Identity(); + + std::vector 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 Calibration::getSimplified() const +{ + std::vector 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 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 diff --git a/ur_calibration/src/calibration_consumer.cpp b/ur_calibration/src/calibration_consumer.cpp new file mode 100644 index 0000000..12ef2a7 --- /dev/null +++ b/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 + +namespace ur_calibration +{ +CalibrationConsumer::CalibrationConsumer() : calibrated_(false) +{ +} + +bool CalibrationConsumer::consume( + std::shared_ptr> product) +{ + auto kin_info = std::dynamic_pointer_cast(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 diff --git a/ur_calibration/src/calibration_correction.cpp b/ur_calibration/src/calibration_correction.cpp new file mode 100644 index 0000000..ff4ad37 --- /dev/null +++ b/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 + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +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("subfolder_name", "etc"); + std::string output_package_name; + try + { + // The robot's IP address + robot_ip_ = getRequiredParameter("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("robot_name"); + + // The resulting parameter file will be stored inside + // //_calibration.yaml + output_package_name = getRequiredParameter("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 stream(robot_ip_, UR_PRIMARY_PORT); + primary_interface::PrimaryParser parser; + comm::URProducer prod(stream, parser); + CalibrationConsumer consumer; + + comm::INotifier notifier; + + comm::Pipeline 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 + 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 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; +} diff --git a/ur_calibration/test/calibration_test.cpp b/ur_calibration/test/calibration_test.cpp new file mode 100644 index 0000000..8b97adb --- /dev/null +++ b/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 +#include + +using namespace ur_calibration; + +namespace +{ +bool isApproximately(const double val1, const double val2, const double precision) +{ + return std::abs(val1 - val2) < precision; +} + +template +void doubleEqVec(const Eigen::Matrix vec1, const Eigen::Matrix 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 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(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 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(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(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 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::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(); +} diff --git a/ur_rtde_driver/CMakeLists.txt b/ur_rtde_driver/CMakeLists.txt index 1de2464..ce85153 100644 --- a/ur_rtde_driver/CMakeLists.txt +++ b/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() diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h b/ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h index d10989c..7bf825d 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h +++ b/ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h @@ -26,6 +26,7 @@ #include #include #include +#include #include "ur_rtde_driver/log.h" #include "ur_rtde_driver/types.h" diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h b/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h index b995d07..c53840f 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h +++ b/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h @@ -154,6 +154,7 @@ public: virtual ~Pipeline() { + LOG_INFO("Destructing pipeline"); stop(); } diff --git a/ur_rtde_driver/include/ur_rtde_driver/primary/primary_parser.h b/ur_rtde_driver/include/ur_rtde_driver/primary/primary_parser.h index 3510342..6805643 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/primary/primary_parser.h +++ b/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(type)); + LOG_DEBUG("Invalid robot package type recieved: %u", static_cast(type)); bp.consume(); return true; } diff --git a/ur_rtde_driver/include/ur_rtde_driver/primary/robot_state.h b/ur_rtde_driver/include/ur_rtde_driver/primary/robot_state.h index ab23846..5cd812e 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/primary/robot_state.h +++ b/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 -#include +#include #include "ur_rtde_driver/primary/primary_package.h" #include "ur_rtde_driver/primary/package_header.h" diff --git a/ur_rtde_driver/package.xml b/ur_rtde_driver/package.xml index 69b99c6..8b3c8c1 100644 --- a/ur_rtde_driver/package.xml +++ b/ur_rtde_driver/package.xml @@ -31,7 +31,6 @@ trajectory_msgs ur_controllers ur_msgs - tf std_srvs force_torque_sensor_controller diff --git a/ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp b/ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp index 3a22bcb..459641c 100644 --- a/ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp +++ b/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 + 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;