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

moved calibration to own repository

Browse Source
This commit is contained in:
Felix Mauch
2019-05-27 16:30:21 +02:00
parent ceb00d8d6d
commit 312fe8b1b7
110 changed files with 250 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

186
ur_calibration/CMakeLists.txt Normal file
View File

@@ -0,0 +1,186 @@
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)
## 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 Eigen3
)
###########
## 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_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})
endif()
install(TARGETS calibration_correction RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

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

@@ -0,0 +1,172 @@
// 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>
/*!
* \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 Generates a string representation of this robot representation.
*
* This string can be directly included into the xacro file used for a calibrated robot.
*/
std::string toXacroProperties()
{
std::stringstream ss;
for (size_t i = 0; i < segments_.size(); ++i)
{
ss << "<xacro:property name=\"d" << i + 1 << "\" value=\"" << segments_[i].d_ << "\" />\n";
ss << "<xacro:property name=\"a" << i + 1 << "\" value=\"" << segments_[i].a_ << "\" />\n";
ss << "<xacro:property name=\"theta" << i + 1 << "\" value=\"" << segments_[i].theta_ << "\" />\n";
ss << "<xacro:property name=\"alpha" << i + 1 << "\" value=\"" << segments_[i].alpha_ << "\" />\n";
}
ss << "<xacro:property name=\"delta_theta_correction2\" value=\"" << delta_theta_correction2_ << "\" />\n";
ss << "<xacro:property name=\"delta_theta_correction3\" value=\"" << delta_theta_correction3_ << "\" />\n";
return ss.str();
}
};
/*!
* \brief Class that handles the calibration correction for Universal Robots
*
* Universal robots provide a factory calibration of their DH parameters to exactly estimate their
* TCP pose using forward kinematics. However, those DH parameters construct a kinematic chain that
* can be very long, as upper arm and lower arm segments can be drawn out of their physical position
* by multiple meters (even above 100m can occur).
*
* This class helps creating a kinematic chain, that is as close as possible to the physical model,
* by dragging the upper and lower arm segments back to their zero position.
*/
class Calibration
{
public:
Calibration(const DHRobot& robot);
virtual ~Calibration();
/*!
* \brief Corrects a UR kinematic chain in such a way that shoulder and elbow offsets are 0.
*/
void correctChain();
std::vector<Eigen::Matrix4d> getChain()
{
return chain_;
}
std::string toXacroProperties()
{
return robot_parameters_corrected_.toXacroProperties();
}
void writeToYaml(std::ofstream& ofstream) const;
std::vector<Eigen::Matrix4d> getSimplified() const;
Eigen::Matrix4d calcForwardKinematics(const Eigen::Matrix<double, 6, 1>& joint_values, const size_t link_nr = 6);
private:
void correctAxis(const size_t correction_index);
void buildChain();
DHRobot robot_parameters_;
DHRobot robot_parameters_corrected_;
std::vector<std::string> link_names_ = { "shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3" };
std::vector<Eigen::Matrix4d> chain_;
};
#endif // ifndef UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED

57
ur_calibration/package.xml Normal file
View File

@@ -0,0 +1,57 @@
<?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>
<test_depend>rosunit</test_depend>
</package>

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

@@ -0,0 +1,230 @@
// 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>
Calibration::Calibration(const DHRobot& robot_parameters) : robot_parameters_(robot_parameters)
{
buildChain();
}
Calibration::~Calibration()
{
}
void Calibration::correctChain()
{
robot_parameters_corrected_ = robot_parameters_;
robot_parameters_corrected_.delta_theta_correction2_ = 0.0;
robot_parameters_corrected_.delta_theta_correction3_ = 0.0;
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::Matrix<double, 6, 1> jointvalues;
jointvalues << 0, 0, 0, 0, 0, 0;
// Eigen::Matrix4d fk_current = calcForwardKinematics(jointvalues, link_index);
// Eigen::Vector3d current_passive = fk_current.topRightCorner(3, 1);
// ROS_INFO_STREAM("current passive:\n" << current_passive);
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);
// ROS_INFO_STREAM("Eigen passive:\n" << eigen_passive);
Eigen::Vector3d eigen_next = (fk_next_passive * chain_[(link_index + 1) * 2]).topRightCorner(3, 1);
// ROS_INFO_STREAM("Eigen next:\n" << eigen_next);
// Construct a representation of the next segment's rotational axis
Eigen::ParametrizedLine<double, 3> next_line;
next_line = Eigen::ParametrizedLine<double, 3>::Through(eigen_passive, eigen_next);
// ROS_INFO_STREAM("next_line:" << std::endl
//<< "Base:" << std::endl
//<< next_line.origin() << std::endl
//<< "Direction:" << std::endl
//<< next_line.direction());
// XY-Plane of first segment's start
Eigen::Hyperplane<double, 3> plane(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_INFO_STREAM("Wrist line intersecting at " << std::endl << intersection);
// ROS_INFO_STREAM("Angle is " << new_theta);
// ROS_INFO_STREAM("Length is " << new_length);
// ROS_INFO_STREAM("Intersection param is " << intersection_param);
// as we move the passive segment towards the first segment, we have to move away the next segment
// again, to keep the same kinematic structure.
double sign_dir = next_line.direction().z() > 0 ? 1.0 : -1.0;
double distance_correction = intersection_param * sign_dir;
// Set d parameter of the first segment to 0 and theta to the calculated new angle
// Correct arm segment length and angle
// ROS_INFO_STREAM("Passive old:\n" << chain_[2 * link_index]);
chain_[2 * link_index](2, 3) = 0.0;
robot_parameters_corrected_.segments_[link_index].d_ = 0.0;
chain_[2 * link_index].topLeftCorner(3, 3) =
Eigen::AngleAxisd(new_theta, Eigen::Vector3d::UnitZ()).toRotationMatrix();
robot_parameters_corrected_.segments_[link_index].theta_ = new_theta;
// ROS_INFO_STREAM("Passive new:\n" << chain_[2 * link_index]);
// Correct arm segment length and angle
// ROS_INFO_STREAM("Next old:\n" << chain_[2 * link_index + 1]);
// ROS_INFO_STREAM("Theta correction: " << robot_parameters_.segments_[link_index].theta_ - new_theta);
// ROS_INFO_STREAM("Alpha correction: " << robot_parameters_.segments_[link_index].alpha_);
chain_[2 * link_index + 1](0, 3) = new_length;
robot_parameters_corrected_.segments_[link_index].a_ = 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();
if (link_index == 1)
{
robot_parameters_corrected_.delta_theta_correction2_ = robot_parameters_.segments_[link_index].theta_ - new_theta;
}
else if (link_index == 2)
{
robot_parameters_corrected_.delta_theta_correction3_ = robot_parameters_.segments_[link_index].theta_ - new_theta;
}
// ROS_INFO_STREAM("Next new:\n" << chain_[2 * link_index + 1]);
// Correct next joint
// ROS_INFO_STREAM("Second Next old:\n" << chain_[2 * link_index + 2]);
chain_[2 * link_index + 2](2, 3) -= distance_correction;
robot_parameters_corrected_.segments_[link_index + 1].d_ -= distance_correction;
// ROS_INFO_STREAM("Second Next new:\n" << chain_[2 * link_index + 2]);
}
Eigen::Matrix4d Calibration::calcForwardKinematics(const Eigen::Matrix<double, 6, 1>& joint_values,
const size_t link_nr)
{
// ROS_INFO_STREAM("Calculating forward kinematics at link " << 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;
}
// ROS_INFO_STREAM("forward_kinematics at " << link_nr << std::endl << output);
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);
// ROS_INFO_STREAM("Rotation " << i / 2 << " a: [" << rpy_a.transpose() << "]");
Eigen::Matrix3d rot_b = chain_[i + 1].topLeftCorner(3, 3);
Eigen::Vector3d rpy_b = rot_b.eulerAngles(0, 1, 2);
// ROS_INFO_STREAM("Rotation " << i / 2 << " b: [" << rpy_b.transpose() << "]");
// ROS_INFO_STREAM("Matrix " << i / 2 << ":\n" << simplified_chain.back());
Eigen::Matrix3d rot = simplified_chain.back().topLeftCorner(3, 3);
Eigen::Vector3d rpy = rot.eulerAngles(0, 1, 2);
Eigen::Quaterniond quat(rot);
// ROS_INFO_STREAM("Rotation (rpy) " << i / 2 << ": [" << rpy.transpose() << "]");
// ROS_INFO_STREAM("Rotation (quat, [xyz], w)" << i / 2 << ": [" << quat.vec().transpose() << "], " << quat.w());
}
simplified_chain.push_back(chain_.back());
return simplified_chain;
}
void Calibration::writeToYaml(std::ofstream& ofstream) 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;
}
ofstream << node;
}

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

@@ -0,0 +1,181 @@
// 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>
#include <sensor_msgs/JointState.h>
#include <tf/transform_listener.h>
sensor_msgs::JointStatePtr joint_state;
std::unique_ptr<Calibration> my_calibration;
std::unique_ptr<tf::TransformListener> tf_listener;
void jointStateCallback(sensor_msgs::JointStatePtr msg)
{
ROS_INFO("Callback");
tf::StampedTransform stamped_transform;
try
{
tf_listener->waitForTransform("base", "tool0_controller", msg->header.stamp, ros::Duration(0.5));
tf_listener->lookupTransform("base", "tool0_controller", msg->header.stamp, stamped_transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("TF lookup error error: %s", ex.what());
return;
}
geometry_msgs::TransformStamped tf_msg;
tf::transformStampedTFToMsg(stamped_transform, tf_msg);
Eigen::Matrix<double, 6, 1> jointvalues = Eigen::Matrix<double, 6, 1>(msg->position.data());
Eigen::Matrix4d mat = my_calibration->calcForwardKinematics(jointvalues);
Eigen::Vector3d error;
error << mat.topRightCorner(3, 1) - Eigen::Vector3d(stamped_transform.getOrigin());
ROS_INFO_STREAM("Error(abs): " << error.norm());
ROS_INFO_STREAM("Error per axis:\n" << error);
}
int main(int argc, char* argv[])
{
ros::init(argc, argv, "ur_calibration");
ros::NodeHandle nh;
DHRobot my_robot;
// This is an ideal UR10
// clang-format off
// d, a, theta, alpha
my_robot.segments_.push_back(DHSegment(0.1273 , 0 , 0 , M_PI_2));
my_robot.segments_.push_back(DHSegment(0.0 , -0.612 , 0.0 , 0.0));
my_robot.segments_.push_back(DHSegment(0.0 , -0.5723, 0 , 0.0));
my_robot.segments_.push_back(DHSegment(0.163941, 0 , 0 , M_PI_2));
my_robot.segments_.push_back(DHSegment(0.1157 , 0 , 0 , -M_PI_2));
my_robot.segments_.push_back(DHSegment(0.0922 , 0 , 0 , 0));
// clang-format on
std::map<std::string, DHRobot> calibrations;
// d, a, theta, alpha
{
DHRobot robot_calibration;
// clang-format off
robot_calibration.segments_.push_back(DHSegment( 0.00065609212979853 , 4.6311376834935676e-05 , -7.290070070824746e-05 , 0.000211987863869334 ));
robot_calibration.segments_.push_back(DHSegment( 1.4442162376284788 , -0.00012568315331862312 , -0.01713897289704999 , -0.0072553625957652995));
robot_calibration.segments_.push_back(DHSegment( 0.854147723854608 , 0.00186216581161458 , -0.03707159413492756 , -0.013483226769541364 ));
robot_calibration.segments_.push_back(DHSegment(-2.2989425877563705 , 9.918593870679266e-05 , 0.054279462160583214 , 0.0013495820227329425 ));
robot_calibration.segments_.push_back(DHSegment(-1.573498686836816e-05 , 4.215462720453189e-06 , 1.488984257025741e-07 , -0.001263136163679901 ));
robot_calibration.segments_.push_back(DHSegment( 1.9072435590711256e-05 , 0 , 1.551499479707493e-05 , 0 ));
// clang-format on
calibrations.insert(std::make_pair("ids-ur10-2", robot_calibration));
}
{
DHRobot robot_calibration;
// clang-format off
robot_calibration.segments_.push_back(DHSegment(0.000489427369583322891 , -0.000157659251888300598 , 7.12144160891142552e-06 , -0.000181484221894123721));
robot_calibration.segments_.push_back(DHSegment(2.15820897072427709 , 0.000797598154419598693 , -0.0597679909195674153 , -0.0169424433153391799 ));
robot_calibration.segments_.push_back(DHSegment(-0.200070560336579328 , 0.00141079051860837357 , 0.0276740817239430857 , -0.00936706021457411366 ));
robot_calibration.segments_.push_back(DHSegment(-1.95850825255269623 , 8.02528233902343753e-05 , 0.0321549453426039911 , 0.000705308984090935454 ));
robot_calibration.segments_.push_back(DHSegment(-2.27384815544295904e-05, 2.49305433182637798e-06 , -7.91131358362739777e-05, -0.000129247903554619015));
robot_calibration.segments_.push_back(DHSegment(1.40058148229704749e-05 , 0 , 2.43093497590859384e-05 , 0 ));
// clang-format on
calibrations.insert(std::make_pair("ids-ur10-3", robot_calibration));
}
// my_calibration.reset(new Calibration(my_robot));
/*
for (auto& calib : calibrations)
{
Eigen::Matrix<double, 6, 1> jointvalues;
double pos_min_error = 9999;
double pos_max_error = 0;
double pos_mean_error = 0;
double angle_min_error = 9999;
double angle_max_error = 0;
double angle_mean_error = 0;
const size_t num_runs = 100000;
for (size_t i = 0; i < num_runs; ++i)
{
Calibration calibration(my_robot + calib.second);
jointvalues = 2 * M_PI * Eigen::Matrix<double, 6, 1>::Random();
Eigen::Matrix4d fk_orig = calibration.calcForwardKinematics(jointvalues);
calibration.correctChain();
Eigen::Matrix4d fk_corrected = calibration.calcForwardKinematics(jointvalues);
Eigen::Vector3d pos_error;
pos_error << fk_orig.topRightCorner(3, 1) - fk_corrected.topRightCorner(3, 1);
if (pos_error.norm() < pos_min_error)
{
pos_min_error = pos_error.norm();
}
if (pos_error.norm() > pos_max_error)
{
pos_max_error = pos_error.norm();
}
pos_mean_error += pos_error.norm() / static_cast<double>(num_runs);
Eigen::Matrix3d rot_mat_orig = fk_orig.topLeftCorner(3, 3);
Eigen::Matrix3d rot_mat_corrected = fk_corrected.topLeftCorner(3, 3);
Eigen::Quaterniond rot_orig(rot_mat_orig);
Eigen::Quaterniond rot_corrected(rot_mat_corrected);
double angle_error = std::abs(rot_orig.angularDistance(rot_corrected));
if (angle_error < angle_min_error)
{
angle_min_error = angle_error;
}
if (angle_error > angle_max_error)
{
angle_max_error = angle_error;
}
angle_mean_error += angle_error / static_cast<double>(num_runs);
}
ROS_INFO_STREAM(calib.first << ": Position Error over " << num_runs << " joint angles (mean, min, max): ("
<< pos_mean_error << ", " << pos_min_error << ", " << pos_max_error << ")");
ROS_INFO_STREAM(calib.first << ": Angle Error over " << num_runs << " joint angles (mean, min, max): ("
<< angle_mean_error << ", " << angle_min_error << ", " << angle_max_error << ")");
// Calibration calibration(my_robot + calib.second);
// calibration.correctChain();
// ROS_INFO_STREAM("Corrected calibration: " << std::endl << calibration.toXacroProperties());
}
*/
my_calibration->correctChain();
std::vector<Eigen::Matrix4d> corrected_chain = my_calibration->getSimplified();
for (auto& it : corrected_chain)
{
Eigen::Matrix3d rot_a = it.topLeftCorner(3, 3);
Eigen::Vector3d rpy_a = rot_a.eulerAngles(0, 1, 2);
ROS_INFO_STREAM("Translation: [" << it.topRightCorner(3, 1).transpose() << "]");
ROS_INFO_STREAM("Rotation: [" << rpy_a.transpose() << "]");
}
std::ofstream file;
file.open("test.yaml");
my_calibration->writeToYaml(file);
file.close();
// ROS_INFO_STREAM("Corrected calibration: " << std::endl << my_calibration->toXacroProperties());
std::cout << my_calibration->toXacroProperties() << std::endl;
// tf_listener.reset(new tf::TransformListener);
// ros::Subscriber joint_state_sub = nh.subscribe("joint_states", 1, jointStateCallback);
// ros::spin();
return 0;
}

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

@@ -0,0 +1,155 @@
// 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>
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)
{
DHRobot my_robot;
const double pi = std::atan(1) * 4;
// const double pi = 1.570796327 * 2.0;
// const double pi = M_PI;
// 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.163841, 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);
}
{
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-10);
}
{
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-9);
}
}
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.163841, 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();
}