From c3d92dcde5e4676ed4b4cc5a48e751b50664c104 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Thu, 13 Jun 2019 10:51:46 +0200 Subject: [PATCH] Use tf for transforming the fts measurements --- ur_rtde_driver/CMakeLists.txt | 6 ++- .../ur_rtde_driver/ros/hardware_interface.h | 15 +++++- ur_rtde_driver/package.xml | 3 +- ur_rtde_driver/src/ros/hardware_interface.cpp | 47 +++++++++++-------- 4 files changed, 48 insertions(+), 23 deletions(-) diff --git a/ur_rtde_driver/CMakeLists.txt b/ur_rtde_driver/CMakeLists.txt index b29b9be..23279a5 100644 --- a/ur_rtde_driver/CMakeLists.txt +++ b/ur_rtde_driver/CMakeLists.txt @@ -19,12 +19,13 @@ find_package(catkin REQUIRED roscpp sensor_msgs std_srvs + tf2_geometry_msgs + tf2_msgs trajectory_msgs ur_controllers ur_msgs ) find_package(Boost REQUIRED) -find_package(Eigen3 REQUIRED) catkin_package( INCLUDE_DIRS @@ -40,6 +41,8 @@ catkin_package( industrial_msgs roscpp sensor_msgs + tf2_geometry_msgs + tf2_msgs trajectory_msgs ur_controllers ur_msgs @@ -70,7 +73,6 @@ include_directories( include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} ) add_library(ur_rtde_driver diff --git a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h index 4422274..c46cd78 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h +++ b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h @@ -33,6 +33,8 @@ #include #include #include +#include "tf2_msgs/TFMessage.h" +#include #include #include @@ -71,10 +73,18 @@ public: protected: /*! - * \brief Transforms force-torque measurements reported from the robot from base to tool frame + * \brief Transforms force-torque measurements reported from the robot from base to tool frame. + * + * Requires extractToolPose() to be run first. */ void transformForceTorque(); + /*! + * \brief Stores the raw tool pose data from the robot in a transformation msg + * + * \param timestamp Timestamp of read data + */ + void extractToolPose(const ros::Time& timestamp); std::unique_ptr ur_driver_; hardware_interface::JointStateInterface js_interface_; @@ -91,6 +101,9 @@ protected: vector6d_t joint_efforts_; vector6d_t fts_measurements_; vector6d_t tcp_pose_; + tf2::Vector3 tcp_force_; + tf2::Vector3 tcp_torque_; + geometry_msgs::TransformStamped tcp_transform_; double speed_scaling_; double target_speed_fraction_; double speed_scaling_combined_; diff --git a/ur_rtde_driver/package.xml b/ur_rtde_driver/package.xml index b2bc895..8f2a569 100644 --- a/ur_rtde_driver/package.xml +++ b/ur_rtde_driver/package.xml @@ -28,7 +28,8 @@ industrial_msgs roscpp sensor_msgs - eigen + tf2_msgs + tf2_geometry_msgs trajectory_msgs ur_controllers ur_msgs diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index d368fc2..5373b2f 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -67,6 +67,8 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h tcp_link_ = robot_hw_nh.param("tcp_link", "tool0"); program_state_pub_ = robot_hw_nh.advertise("robot_program_running", 10, true); + tcp_transform_.header.frame_id = "base"; + tcp_transform_.child_frame_id = "tool_0_controller"; bool use_tool_communication = robot_hw_nh.param("use_tool_communication", "false"); std::unique_ptr tool_comm_setup; @@ -226,6 +228,7 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period } // Transform fts measurements to tool frame + extractToolPose(time); transformForceTorque(); // pausing state follows runtime state when pausing @@ -314,27 +317,16 @@ uint32_t HardwareInterface ::getControlFrequency() const void HardwareInterface ::transformForceTorque() { - double tcp_angle = std::sqrt(std::pow(tcp_pose_[3], 2) + std::pow(tcp_pose_[4], 2) + std::pow(tcp_pose_[5], 2)); + tcp_force_.setValue(fts_measurements_[0], fts_measurements_[1], fts_measurements_[2]); + tcp_torque_.setValue(fts_measurements_[3], fts_measurements_[4], fts_measurements_[5]); - Eigen::Vector3d rotation_vec; - rotation_vec = Eigen::Vector3d(tcp_pose_[3], tcp_pose_[4], tcp_pose_[5]).normalized(); - Eigen::AngleAxisd rotation; - if (tcp_angle < 1e-16) - { - rotation = Eigen::AngleAxisd::Identity(); - } - else - { - rotation = Eigen::AngleAxisd(tcp_angle, rotation_vec); - } + tf2::Quaternion rotation_quat; + tf2::fromMsg(tcp_transform_.transform.rotation, rotation_quat); + tcp_force_ = tf2::quatRotate(rotation_quat.inverse(), tcp_force_); + tcp_torque_ = tf2::quatRotate(rotation_quat.inverse(), tcp_torque_); - Eigen::Vector3d tcp_force = Eigen::Vector3d(fts_measurements_[0], fts_measurements_[1], fts_measurements_[2]); - Eigen::Vector3d tcp_torque = Eigen::Vector3d(fts_measurements_[3], fts_measurements_[4], fts_measurements_[5]); - - tcp_force = rotation.inverse() * tcp_force; - tcp_torque = rotation.inverse() * tcp_torque; - - fts_measurements_ = { tcp_force[0], tcp_force[1], tcp_force[2], tcp_torque[0], tcp_torque[1], tcp_torque[2] }; + fts_measurements_ = { tcp_force_.x(), tcp_force_.y(), tcp_force_.z(), + tcp_torque_.x(), tcp_torque_.y(), tcp_torque_.z() }; } bool HardwareInterface ::isRobotProgramRunning() const @@ -367,4 +359,21 @@ bool HardwareInterface ::shouldResetControllers() } } +void HardwareInterface ::extractToolPose(const ros::Time& timestamp) +{ + double tcp_angle = std::sqrt(std::pow(tcp_pose_[3], 2) + std::pow(tcp_pose_[4], 2) + std::pow(tcp_pose_[5], 2)); + + tf2::Vector3 rotation_vec(tcp_pose_[3], tcp_pose_[4], tcp_pose_[5]); + tf2::Quaternion rotation; + if (tcp_angle > 1e-16) + { + rotation.setRotation(rotation_vec.normalized(), tcp_angle); + } + tcp_transform_.header.stamp = timestamp; + tcp_transform_.transform.translation.x = tcp_pose_[0]; + tcp_transform_.transform.translation.y = tcp_pose_[1]; + tcp_transform_.transform.translation.z = tcp_pose_[2]; + + tcp_transform_.transform.rotation = tf2::toMsg(rotation); +} } // namespace ur_driver