From 00e0fbee3d845c70d77e1655f755d06004cc7cc3 Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Wed, 5 Jun 2019 16:21:08 +0200
Subject: [PATCH] transform fts measurements to tcp

They are reported in base coordinate frame from the robot.
---
 ur_rtde_driver/CMakeLists.txt                 |  2 ++
 .../ur_rtde_driver/ros/hardware_interface.h   |  6 ++++
 ur_rtde_driver/package.xml                    |  1 +
 ur_rtde_driver/resources/rtde_recipe.txt      |  1 +
 ur_rtde_driver/src/ros/hardware_interface.cpp | 35 +++++++++++++++++++
 5 files changed, 45 insertions(+)

diff --git a/ur_rtde_driver/CMakeLists.txt b/ur_rtde_driver/CMakeLists.txt
index ce85153..e9ef869 100644
--- a/ur_rtde_driver/CMakeLists.txt
+++ b/ur_rtde_driver/CMakeLists.txt
@@ -24,6 +24,7 @@ find_package(catkin REQUIRED
     ur_msgs
 )
 find_package(Boost REQUIRED)
+find_package(Eigen3 REQUIRED)
 
 catkin_package(
   INCLUDE_DIRS
@@ -69,6 +70,7 @@ 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 7b0b0be..5f9439e 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
@@ -63,6 +63,11 @@ public:
   uint32_t getControlFrequency() const;
 
 protected:
+  /*!
+   * \brief Transforms force-torque measurements reported from the robot from base to tool frame
+   */
+  void transform_force_torque();
+
   std::unique_ptr<UrDriver> ur_driver_;
 
   hardware_interface::JointStateInterface js_interface_;
@@ -78,6 +83,7 @@ protected:
   vector6d_t joint_velocities_;
   vector6d_t joint_efforts_;
   vector6d_t fts_measurements_;
+  vector6d_t tcp_pose_;
   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 8b3c8c1..75770ec 100644
--- a/ur_rtde_driver/package.xml
+++ b/ur_rtde_driver/package.xml
@@ -28,6 +28,7 @@
   <depend>industrial_msgs</depend>
   <depend>roscpp</depend>
   <depend>sensor_msgs</depend>
+  <depend>Eigen3</depend>
   <depend>trajectory_msgs</depend>
   <depend>ur_controllers</depend>
   <depend>ur_msgs</depend>
diff --git a/ur_rtde_driver/resources/rtde_recipe.txt b/ur_rtde_driver/resources/rtde_recipe.txt
index e063431..a27d836 100644
--- a/ur_rtde_driver/resources/rtde_recipe.txt
+++ b/ur_rtde_driver/resources/rtde_recipe.txt
@@ -5,3 +5,4 @@ speed_scaling
 target_speed_fraction
 runtime_state
 actual_TCP_force
+actual_TCP_pose
diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp
index 256ecf9..9e2e137 100644
--- a/ur_rtde_driver/src/ros/hardware_interface.cpp
+++ b/ur_rtde_driver/src/ros/hardware_interface.cpp
@@ -27,6 +27,8 @@
 
 #include "ur_rtde_driver/ros/hardware_interface.h"
 
+#include <Eigen/Geometry>
+
 namespace ur_driver
 {
 HardwareInterface::HardwareInterface()
@@ -142,6 +144,14 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period
       // This throwing should never happen unless misconfigured
       throw std::runtime_error("Did not find actual_TCP_force in data sent from robot. This should not happen!");
     }
+    if (!data_pkg->getData("actual_TCP_pose", tcp_pose_))
+    {
+      // This throwing should never happen unless misconfigured
+      throw std::runtime_error("Did not find actual_TCP_pose in data sent from robot. This should not happen!");
+    }
+
+    // Transform fts measurements to tool frame
+    transform_force_torque();
 
     // pausing state follows runtime state when pausing
     if (runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PAUSED))
@@ -228,4 +238,29 @@ uint32_t HardwareInterface ::getControlFrequency() const
   // TODO: Do this nicer than throwing an exception
   throw std::runtime_error("ur_driver is not yet initialized");
 }
+
+void HardwareInterface ::transform_force_torque()
+{
+  double tcp_angle = std::sqrt(std::pow(tcp_pose_[3], 2) + std::pow(tcp_pose_[4], 2) + std::pow(tcp_pose_[5], 2));
+
+  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);
+  }
+
+  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] };
+}
 }  // namespace ur_driver