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

Made calibration extraction a working node.

Browse Source
This commit is contained in:
Felix Mauch
2019-05-28 10:03:23 +02:00
parent 22f392222e
commit 157b5a696c
7 changed files with 153 additions and 150 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
ur_calibration/CMakeLists.txt
View File

@@ -127,6 +127,7 @@ include_directories(
add_executable(calibration_correction
src/calibration.cpp
src/calibration_consumer.cpp
src/calibration_correction.cpp
)

5
ur_calibration/include/ur_calibration/calibration.h
View File

@@ -20,6 +20,8 @@
#include <yaml-cpp/yaml.h>
#include <fstream>
namespace ur_calibration
{
/*!
* \brief An internal representation of a DH-parametrized link.
*
@@ -152,7 +154,7 @@ public:
{
return robot_parameters_corrected_.toXacroProperties();
}
void writeToYaml(std::ofstream& ofstream) const;
YAML::Node toYaml() const;
std::vector<Eigen::Matrix4d> getSimplified() const;
@@ -169,4 +171,5 @@ private:
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

7
ur_calibration/src/calibration.cpp
View File

@@ -14,6 +14,8 @@
#include <ur_calibration/calibration.h>
namespace ur_calibration
{
Calibration::Calibration(const DHRobot& robot_parameters) : robot_parameters_(robot_parameters)
{
buildChain();
@@ -206,7 +208,7 @@ std::vector<Eigen::Matrix4d> Calibration::getSimplified() const
return simplified_chain;
}
void Calibration::writeToYaml(std::ofstream& ofstream) const
YAML::Node Calibration::toYaml() const
{
YAML::Node node;
@@ -226,5 +228,6 @@ void Calibration::writeToYaml(std::ofstream& ofstream) const
node["kinematics"][link_names_[i]] = link;
}
ofstream << node;
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

171
ur_calibration/src/calibration_correction.cpp
View File

@@ -12,170 +12,53 @@
*/
//----------------------------------------------------------------------
#include <ur_calibration/calibration.h>
#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>
sensor_msgs::JointStatePtr joint_state;
std::unique_ptr<Calibration> my_calibration;
std::unique_ptr<tf::TransformListener> tf_listener;
using namespace ur_driver;
using namespace primary_interface;
using namespace ur_calibration;
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);
}
static const int UR_PRIMARY_PORT = 30001;
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));
std::string robot_ip = "192.168.56.101";
comm::URStream<PackageHeader> stream(robot_ip, UR_PRIMARY_PORT);
primary_interface::PrimaryParser parser;
comm::URProducer<PackageHeader> prod(stream, parser);
CalibrationConsumer consumer;
/*
for (auto& calib : calibrations)
{
Eigen::Matrix<double, 6, 1> jointvalues;
comm::INotifier notifier;
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)
comm::Pipeline<PackageHeader> pipeline(prod, consumer, "Pipeline", notifier);
pipeline.run();
while (!consumer.isCalibrated())
{
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() << "]");
sleep(1);
// LOG_INFO("Still running");
}
pipeline.stop();
std::ofstream file;
file.open("test.yaml");
my_calibration->writeToYaml(file);
YAML::Node calibration_parameters = consumer.getCalibrationParameters();
file << calibration_parameters;
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();
ROS_INFO_STREAM("Corrected calibration: " << std::endl << calibration_parameters);
return 0;
}

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