From 157b5a696cc0d1fc31b37af69d8d2224fc4d9260 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Tue, 28 May 2019 10:03:23 +0200 Subject: [PATCH] Made calibration extraction a working node. --- ur_calibration/CMakeLists.txt | 1 + .../include/ur_calibration/calibration.h | 5 +- .../ur_calibration/calibration_consumer.h | 60 ++++++ ur_calibration/src/calibration.cpp | 7 +- ur_calibration/src/calibration_consumer.cpp | 53 ++++++ ur_calibration/src/calibration_correction.cpp | 175 +++--------------- .../ur_rtde_driver/primary/primary_parser.h | 2 +- 7 files changed, 153 insertions(+), 150 deletions(-) create mode 100644 ur_calibration/include/ur_calibration/calibration_consumer.h create mode 100644 ur_calibration/src/calibration_consumer.cpp diff --git a/ur_calibration/CMakeLists.txt b/ur_calibration/CMakeLists.txt index 59508cd..6b8c026 100644 --- a/ur_calibration/CMakeLists.txt +++ b/ur_calibration/CMakeLists.txt @@ -127,6 +127,7 @@ include_directories( add_executable(calibration_correction src/calibration.cpp + src/calibration_consumer.cpp src/calibration_correction.cpp ) diff --git a/ur_calibration/include/ur_calibration/calibration.h b/ur_calibration/include/ur_calibration/calibration.h index 6175286..3c98f58 100644 --- a/ur_calibration/include/ur_calibration/calibration.h +++ b/ur_calibration/include/ur_calibration/calibration.h @@ -20,6 +20,8 @@ #include #include +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 getSimplified() const; @@ -169,4 +171,5 @@ private: std::vector chain_; }; +} // namespace ur_calibration #endif // ifndef UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED diff --git a/ur_calibration/include/ur_calibration/calibration_consumer.h b/ur_calibration/include/ur_calibration/calibration_consumer.h new file mode 100644 index 0000000..e871cf8 --- /dev/null +++ b/ur_calibration/include/ur_calibration/calibration_consumer.h @@ -0,0 +1,60 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@fzi.de + * \date 2019-05-28 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CALIBRATION_CALIBRATION_CONSUMER_H_INCLUDED +#define UR_CALIBRATION_CALIBRATION_CONSUMER_H_INCLUDED +#include + +#include + +#include + +namespace ur_calibration +{ +class CalibrationConsumer + : public ur_driver::comm::IConsumer> +{ +public: + CalibrationConsumer(); + virtual ~CalibrationConsumer() = default; + + virtual void setupConsumer() + { + } + virtual void teardownConsumer() + { + } + virtual void stopConsumer() + { + } + virtual void onTimeout() + { + } + + virtual bool + consume(std::shared_ptr> product); + + bool isCalibrated() const + { + return calibrated_; + } + + YAML::Node getCalibrationParameters() const; + +private: + bool calibrated_; + YAML::Node calibration_parameters_; +}; +} // namespace ur_calibration +#endif // ifndef UR_CALIBRATION_CALIBRATION_CONSUMER_H_INCLUDED diff --git a/ur_calibration/src/calibration.cpp b/ur_calibration/src/calibration.cpp index cdff728..ca98ccd 100644 --- a/ur_calibration/src/calibration.cpp +++ b/ur_calibration/src/calibration.cpp @@ -14,6 +14,8 @@ #include +namespace ur_calibration +{ Calibration::Calibration(const DHRobot& robot_parameters) : robot_parameters_(robot_parameters) { buildChain(); @@ -206,7 +208,7 @@ std::vector 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 diff --git a/ur_calibration/src/calibration_consumer.cpp b/ur_calibration/src/calibration_consumer.cpp new file mode 100644 index 0000000..12ef2a7 --- /dev/null +++ b/ur_calibration/src/calibration_consumer.cpp @@ -0,0 +1,53 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@fzi.de + * \date 2019-05-28 + * + */ +//---------------------------------------------------------------------- + +#include + +namespace ur_calibration +{ +CalibrationConsumer::CalibrationConsumer() : calibrated_(false) +{ +} + +bool CalibrationConsumer::consume( + std::shared_ptr> product) +{ + auto kin_info = std::dynamic_pointer_cast(product); + if (kin_info != nullptr) + { + LOG_INFO("%s", product->toString().c_str()); + DHRobot my_robot; + for (size_t i = 0; i < kin_info->dh_a_.size(); ++i) + { + my_robot.segments_.push_back( + DHSegment(kin_info->dh_d_[i], kin_info->dh_a_[i], kin_info->dh_theta_[i], kin_info->dh_alpha_[i])); + } + Calibration calibration(my_robot); + calibration.correctChain(); + + calibration_parameters_ = calibration.toYaml(); + calibrated_ = true; + } + return true; +} + +YAML::Node CalibrationConsumer::getCalibrationParameters() const +{ + if (!calibrated_) + { + throw(std::runtime_error("Cannot get calibration, as no calibration data received yet")); + } + return calibration_parameters_; +} +} // namespace ur_calibration diff --git a/ur_calibration/src/calibration_correction.cpp b/ur_calibration/src/calibration_correction.cpp index 68f6424..75663ec 100644 --- a/ur_calibration/src/calibration_correction.cpp +++ b/ur_calibration/src/calibration_correction.cpp @@ -12,170 +12,53 @@ */ //---------------------------------------------------------------------- -#include +#include + +#include +#include +#include +#include +#include +#include #include #include -sensor_msgs::JointStatePtr joint_state; -std::unique_ptr my_calibration; -std::unique_ptr 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 jointvalues = Eigen::Matrix(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 calibrations; - // d, a, theta, alpha + std::string robot_ip = "192.168.56.101"; + comm::URStream stream(robot_ip, UR_PRIMARY_PORT); + primary_interface::PrimaryParser parser; + comm::URProducer prod(stream, parser); + CalibrationConsumer consumer; + + comm::INotifier notifier; + + comm::Pipeline pipeline(prod, consumer, "Pipeline", notifier); + pipeline.run(); + while (!consumer.isCalibrated()) { - 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 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::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(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(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 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; } diff --git a/ur_rtde_driver/include/ur_rtde_driver/primary/primary_parser.h b/ur_rtde_driver/include/ur_rtde_driver/primary/primary_parser.h index 3510342..6805643 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/primary/primary_parser.h +++ b/ur_rtde_driver/include/ur_rtde_driver/primary/primary_parser.h @@ -123,7 +123,7 @@ public: default: { - LOG_WARN("Invalid robot package type recieved: %u", static_cast(type)); + LOG_DEBUG("Invalid robot package type recieved: %u", static_cast(type)); bp.consume(); return true; }