Import transformation of force-torque into tcp frame from ROS1 driver (…

…https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/src/hardware_interface.cpp). (UniversalRobots#237)

* Transform force-torque into tcp frame.

* Point out the original source of the code.

ur_robot_driver/CMakeLists.txt

@@ -16,7 +16,9 @@ find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 find_package(ur_client_library REQUIRED)
 find_package(ur_dashboard_msgs REQUIRED)
 

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -45,6 +45,8 @@
 // ROS
 #include "rclcpp/macros.hpp"
 #include "rclcpp_lifecycle/state.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
 
 namespace ur_robot_driver
 {
@@ -113,6 +115,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   void initAsyncIO();
   void checkAsyncIO();
   void updateNonDoubleValues();
+  void extractToolPose();
+  void transformForceTorque();
 
   urcl::vector6d_t urcl_position_commands_;
   urcl::vector6d_t urcl_position_commands_old_;
@@ -147,6 +151,11 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   std::bitset<4> robot_status_bits_;
   std::bitset<11> safety_status_bits_;
 
+  // transform stuff
+  tf2::Vector3 tcp_force_;
+  tf2::Vector3 tcp_torque_;
+  geometry_msgs::msg::TransformStamped tcp_transform_;
+
   // asynchronous commands
   std::array<double, 18> standard_dig_out_bits_cmd_;
   std::array<double, 2> standard_analog_output_cmd_;

ur_robot_driver/package.xml

@@ -27,6 +27,8 @@
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>std_srvs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
   <!-- TODO: rosdep cannot find ur_client_library -->
   <depend>ur_client_library</depend>
   <depend>ur_dashboard_msgs</depend>

ur_robot_driver/src/hardware_interface.cpp

@@ -467,6 +467,10 @@ hardware_interface::return_type URPositionHardwareInterface::read()
     readBitsetData<uint32_t>(data_pkg, "analog_io_types", analog_io_types_);
     readBitsetData<uint32_t>(data_pkg, "tool_analog_input_types", tool_analog_input_types_);
 
+    // required transforms
+    extractToolPose();
+    transformForceTorque();
+
     // TODO(anyone): logic for sending other stuff to higher level interface
 
     // pausing state follows runtime state when pausing
@@ -623,6 +627,43 @@ void URPositionHardwareInterface::updateNonDoubleValues()
   system_interface_initialized_ = initialized_ ? 1.0 : 0.0;
 }
 
+void URPositionHardwareInterface::transformForceTorque()
+{
+  // imported from ROS1 driver - hardware_interface.cpp#L867-L876
+  tcp_force_.setValue(urcl_ft_sensor_measurements_[0], urcl_ft_sensor_measurements_[1],
+                      urcl_ft_sensor_measurements_[2]);
+  tcp_torque_.setValue(urcl_ft_sensor_measurements_[3], urcl_ft_sensor_measurements_[4],
+                       urcl_ft_sensor_measurements_[5]);
+
+  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_);
+
+  urcl_ft_sensor_measurements_ = { tcp_force_.x(),  tcp_force_.y(),  tcp_force_.z(),
+                                   tcp_torque_.x(), tcp_torque_.y(), tcp_torque_.z() };
+}
+
+void URPositionHardwareInterface::extractToolPose()
+{
+  // imported from ROS1 driver hardware_interface.cpp#L911-L928
+  double tcp_angle =
+      std::sqrt(std::pow(urcl_tcp_pose_[3], 2) + std::pow(urcl_tcp_pose_[4], 2) + std::pow(urcl_tcp_pose_[5], 2));
+
+  tf2::Vector3 rotation_vec(urcl_tcp_pose_[3], urcl_tcp_pose_[4], urcl_tcp_pose_[5]);
+  tf2::Quaternion rotation;
+  if (tcp_angle > 1e-16) {
+    rotation.setRotation(rotation_vec.normalized(), tcp_angle);
+  } else {
+    rotation.setValue(0.0, 0.0, 0.0, 1.0);  // default Quaternion is 0,0,0,0 which is invalid
+  }
+  tcp_transform_.transform.translation.x = urcl_tcp_pose_[0];
+  tcp_transform_.transform.translation.y = urcl_tcp_pose_[1];
+  tcp_transform_.transform.translation.z = urcl_tcp_pose_[2];
+
+  tcp_transform_.transform.rotation = tf2::toMsg(rotation);
+}
+
 hardware_interface::return_type URPositionHardwareInterface::prepare_command_mode_switch(
     const std::vector<std::string>& start_interfaces, const std::vector<std::string>& stop_interfaces)
 {