Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 10:00:48 +02:00
Code Releases Activity

Merge branch 'calibration_new'

Browse Source 
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.
This commit is contained in:
Felix Mauch
2019-06-03 09:41:59 +02:00
parent 649f499499 db8bf458ee
commit b15a416d06
16 changed files with 1154 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

189
ur_calibration/CMakeLists.txt Normal file
View File

@@ -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})

39
ur_calibration/README.md Normal file
View File

@@ -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.

186
ur_calibration/include/ur_calibration/calibration.h Normal file
View File

@@ -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

60
ur_calibration/include/ur_calibration/calibration_consumer.h Normal file
View File

@@ -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

58
ur_calibration/package.xml Normal file
View File

@@ -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>

196
ur_calibration/src/calibration.cpp Normal file
View File

@@ -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

53
ur_calibration/src/calibration_consumer.cpp Normal file
View File

@@ -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

184
ur_calibration/src/calibration_correction.cpp Normal file
View File

@@ -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;
}

178
ur_calibration/test/calibration_test.cpp Normal file
View File

@@ -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();
}

13
ur_rtde_driver/CMakeLists.txt
View File

@@ -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()

1
ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h
View File

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

1
ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h
View File

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

2
ur_rtde_driver/include/ur_rtde_driver/primary/primary_parser.h
View File

@@ -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;
}

3
ur_rtde_driver/include/ur_rtde_driver/primary/robot_state.h
View File

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

1
ur_rtde_driver/package.xml
View File

@@ -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>

10
ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp
View File

@@ -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;