Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-12 11:00:47 +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

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