Use tf for transforming the fts measurements

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

ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h

@@ -33,6 +33,8 @@
 #include <hardware_interface/joint_state_interface.h>
 #include <algorithm>
 #include <std_msgs/Bool.h>
+#include "tf2_msgs/TFMessage.h"
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 #include <ur_controllers/speed_scaling_interface.h>
 #include <ur_controllers/scaled_joint_command_interface.h>
@@ -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<UrDriver> 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_;

ur_rtde_driver/package.xml

@@ -28,7 +28,8 @@
   <depend>industrial_msgs</depend>
   <depend>roscpp</depend>
   <depend>sensor_msgs</depend>
-  <depend>eigen</depend>
+  <depend>tf2_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>trajectory_msgs</depend>
   <depend>ur_controllers</depend>
   <depend>ur_msgs</depend>

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<std::string>("tcp_link", "tool0");
   program_state_pub_ = robot_hw_nh.advertise<std_msgs::Bool>("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<bool>("use_tool_communication", "false");
   std::unique_ptr<ToolCommSetup> 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