From 1f973742fabd729c486e9caeea7cae1267e53104 Mon Sep 17 00:00:00 2001 From: livanov93 Date: Wed, 10 Nov 2021 10:32:11 +0100 Subject: [PATCH] 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). (#237) * Transform force-torque into tcp frame. * Point out the original source of the code. --- ur_robot_driver/CMakeLists.txt | 2 + .../ur_robot_driver/hardware_interface.hpp | 9 ++++ ur_robot_driver/package.xml | 2 + ur_robot_driver/src/hardware_interface.cpp | 41 +++++++++++++++++++ 4 files changed, 54 insertions(+) diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 0c67e2dc..b807c0a2 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/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) diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index e08c35e0..69a5ecc7 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/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 standard_dig_out_bits_cmd_; std::array standard_analog_output_cmd_; diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index f741fa15..b3745476 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -27,6 +27,8 @@ pluginlib rclcpp std_srvs + geometry_msgs + tf2_geometry_msgs ur_client_library ur_dashboard_msgs diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index d8720046..3f9a6268 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -467,6 +467,10 @@ hardware_interface::return_type URPositionHardwareInterface::read() readBitsetData(data_pkg, "analog_io_types", analog_io_types_); readBitsetData(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& start_interfaces, const std::vector& stop_interfaces) {