From ec319fe2f25b0f1050ff11b74535a4b94d0b1c38 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 18 Sep 2023 12:20:36 +0000 Subject: [PATCH 1/3] Dump commit pose broadcaster --- ur_controllers/CMakeLists.txt | 15 +- ur_controllers/controller_plugins.xml | 5 + .../ur_controllers/pose_broadcaster.hpp | 84 +++++++++++ .../include/ur_controllers/sc_pose.hpp | 123 ++++++++++++++++ ur_controllers/package.xml | 3 + ur_controllers/src/pose_broadcaster.cpp | 134 ++++++++++++++++++ .../src/pose_broadcaster_parameters.yaml | 16 +++ ur_robot_driver/config/ur_controllers.yaml | 9 ++ ur_robot_driver/launch/ur_control.launch.py | 2 + ur_robot_driver/src/hardware_interface.cpp | 18 ++- 10 files changed, 405 insertions(+), 4 deletions(-) create mode 100644 ur_controllers/include/ur_controllers/pose_broadcaster.hpp create mode 100644 ur_controllers/include/ur_controllers/sc_pose.hpp create mode 100644 ur_controllers/src/pose_broadcaster.cpp create mode 100644 ur_controllers/src/pose_broadcaster_parameters.yaml diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index a2b72e599..cd9ad55c5 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -8,6 +8,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) find_package(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) find_package(joint_trajectory_controller REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(pluginlib REQUIRED) @@ -16,6 +17,8 @@ find_package(rcutils REQUIRED) find_package(realtime_tools REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_msgs REQUIRED) find_package(ur_dashboard_msgs REQUIRED) find_package(ur_msgs REQUIRED) find_package(generate_parameter_library REQUIRED) @@ -23,6 +26,7 @@ find_package(generate_parameter_library REQUIRED) set(THIS_PACKAGE_INCLUDE_DEPENDS angles controller_interface + hardware_interface joint_trajectory_controller lifecycle_msgs pluginlib @@ -31,6 +35,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS realtime_tools std_msgs std_srvs + tf2_geometry_msgs + tf2_msgs ur_dashboard_msgs ur_msgs generate_parameter_library @@ -54,7 +60,13 @@ generate_parameter_library( src/scaled_joint_trajectory_controller_parameters.yaml ) +generate_parameter_library( + pose_broadcaster_parameters + src/pose_broadcaster_parameters.yaml +) + add_library(${PROJECT_NAME} SHARED + src/pose_broadcaster.cpp src/scaled_joint_trajectory_controller.cpp src/speed_scaling_state_broadcaster.cpp src/gpio_controller.cpp) @@ -64,8 +76,9 @@ target_include_directories(${PROJECT_NAME} PRIVATE ) target_link_libraries(${PROJECT_NAME} gpio_controller_parameters - speed_scaling_state_broadcaster_parameters + pose_broadcaster_parameters scaled_joint_trajectory_controller_parameters + speed_scaling_state_broadcaster_parameters ) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index f0058ab55..0b862f23b 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -14,4 +14,9 @@ This controller publishes the Tool IO. + + + This controller publishes a tf2 message. + + diff --git a/ur_controllers/include/ur_controllers/pose_broadcaster.hpp b/ur_controllers/include/ur_controllers/pose_broadcaster.hpp new file mode 100644 index 000000000..34f4ab577 --- /dev/null +++ b/ur_controllers/include/ur_controllers/pose_broadcaster.hpp @@ -0,0 +1,84 @@ +// Copyright 2023, FZI Forschungszentrum Informatik +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner + * \date 2023-09-11 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CONTROLLERS__POSE_BROADCASTER_HPP +#define UR_CONTROLLERS__POSE_BROADCASTER_HPP + +#include "geometry_msgs/msg/pose.hpp" +#include "pose_broadcaster_parameters.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +#include +#include +#include +#include + +namespace ur_controllers +{ +class PoseBroadcaster : public controller_interface::ControllerInterface +{ +public: + PoseBroadcaster(); + virtual ~PoseBroadcaster() = default; + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; + + controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + + controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + controller_interface::CallbackReturn on_init() override; + +private: + std::unique_ptr pose_component_; + std::shared_ptr param_listener_; + pose_broadcaster::Params params_; + + using StatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr sensor_state_publisher_; + std::unique_ptr realtime_publisher_; + + geometry_msgs::msg::Pose pose_; +}; +} // namespace ur_controllers + +#endif // ifndef UR_CONTROLLERS__POSE_BROADCASTER_HPP diff --git a/ur_controllers/include/ur_controllers/sc_pose.hpp b/ur_controllers/include/ur_controllers/sc_pose.hpp new file mode 100644 index 000000000..e11acf6bd --- /dev/null +++ b/ur_controllers/include/ur_controllers/sc_pose.hpp @@ -0,0 +1,123 @@ +// Copyright 2023, FZI Forschungszentrum Informatik +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner + * \date 2023-09-11 + * + */ +//---------------------------------------------------------------------- + +#include +#ifndef UR_CONTROLLERS__SC_POSE_HPP_ +#include +#include + +#include +#include +#include + +#include +#include + +namespace ur_controllers +{ +class PoseComponent : public semantic_components::SemanticComponentInterface +{ +public: + explicit PoseComponent(const std::string& name) : SemanticComponentInterface(name, 6) + { + interface_names_.emplace_back(name_ + "/" + "x"); + interface_names_.emplace_back(name_ + "/" + "y"); + interface_names_.emplace_back(name_ + "/" + "z"); + interface_names_.emplace_back(name_ + "/" + "rx"); + interface_names_.emplace_back(name_ + "/" + "ry"); + interface_names_.emplace_back(name_ + "/" + "rz"); + + std::fill(position_.begin(), position_.end(), std::numeric_limits::quiet_NaN()); + std::fill(orientation_.begin(), orientation_.end(), std::numeric_limits::quiet_NaN()); + } + + virtual ~PoseComponent() = default; + + /** + * Return Pose message with full pose, parent and child frame + * + * \return Pose from values; + */ + bool get_values_as_message(geometry_msgs::msg::Pose& message) + { + get_position(); + get_orientation(); + + std::cout << orientation_[0] << std::endl; + + // update the message values + message.position.x = position_[0]; + message.position.y = position_[1]; + message.position.z = position_[2]; + + tf2::Quaternion q; + q.setEuler(orientation_[2], orientation_[1], orientation_[0]); + + message.orientation.w = q.w(); + message.orientation.x = q.x(); + message.orientation.y = q.y(); + message.orientation.z = q.z(); + + return true; + } + +protected: + std::array get_position() + { + size_t interface_counter = 0; + for (size_t i = 0; i < 3; ++i) { + position_[i] = state_interfaces_[interface_counter].get().get_value(); + ++interface_counter; + } + return position_; + } + std::array get_orientation() + { + size_t interface_counter = 0; + for (size_t i = 3; i < 6; ++i) { + orientation_[i] = state_interfaces_[interface_counter].get().get_value(); + ++interface_counter; + } + return orientation_; + } + std::array position_; + std::array orientation_; +}; + +} // namespace ur_controllers +#define UR_CONTROLLERS__SC_POSE_HPP_ +#endif // ifndef UR_CONTROLLERS__SC_POSE_HPP_ diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index e8a492e3f..f7010ccdf 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -22,6 +22,7 @@ angles controller_interface + hardware_interface joint_trajectory_controller lifecycle_msgs pluginlib @@ -30,6 +31,8 @@ realtime_tools std_msgs std_srvs + tf2_geometry_msgs + tf2_msgs ur_dashboard_msgs ur_msgs diff --git a/ur_controllers/src/pose_broadcaster.cpp b/ur_controllers/src/pose_broadcaster.cpp new file mode 100644 index 000000000..e99a8492c --- /dev/null +++ b/ur_controllers/src/pose_broadcaster.cpp @@ -0,0 +1,134 @@ +// Copyright 2023, FZI Forschungszentrum Informatik +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner + * \date 2023-09-11 + * + */ +//---------------------------------------------------------------------- +// +#include "ur_controllers/pose_broadcaster.hpp" +#include +#include "tf2_msgs/msg/tf_message.hpp" + +namespace ur_controllers +{ + +PoseBroadcaster::PoseBroadcaster() : controller_interface::ControllerInterface() +{ +} + +controller_interface::InterfaceConfiguration PoseBroadcaster::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration PoseBroadcaster::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = pose_component_->get_state_interface_names(); + return state_interfaces_config; +} + +controller_interface::return_type PoseBroadcaster::update(const rclcpp::Time& time, const rclcpp::Duration& period) +{ + if (realtime_publisher_ && realtime_publisher_->trylock()) { + pose_component_->get_values_as_message(pose_); + realtime_publisher_->msg_.transforms[0].header.stamp = time; + realtime_publisher_->msg_.transforms[0].transform.translation.x = pose_.position.x; + realtime_publisher_->msg_.transforms[0].transform.translation.y = pose_.position.y; + realtime_publisher_->msg_.transforms[0].transform.translation.z = pose_.position.z; + realtime_publisher_->msg_.transforms[0].transform.rotation.w = pose_.orientation.w; + realtime_publisher_->msg_.transforms[0].transform.rotation.x = pose_.orientation.x; + realtime_publisher_->msg_.transforms[0].transform.rotation.y = pose_.orientation.y; + realtime_publisher_->msg_.transforms[0].transform.rotation.z = pose_.orientation.z; + realtime_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_configure(const rclcpp_lifecycle::State& previous_state) +{ + try { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } catch (const std::exception& e) { + fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + pose_component_ = std::make_unique(ur_controllers::PoseComponent(params_.sensor_name)); + + try { + // register sensor data publisher + sensor_state_publisher_ = + get_node()->create_publisher("/tf", rclcpp::SystemDefaultsQoS()); + realtime_publisher_ = std::make_unique(sensor_state_publisher_); + } catch (const std::exception& e) { + RCLCPP_ERROR(get_node()->get_logger(), "Could not create publisher: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + realtime_publisher_->lock(); + realtime_publisher_->msg_.transforms.resize(1); + realtime_publisher_->msg_.transforms[0].header.frame_id = params_.parent_frame_id; + realtime_publisher_->msg_.transforms[0].child_frame_id = params_.child_frame_id; + realtime_publisher_->unlock(); + + RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_activate(const rclcpp_lifecycle::State& previous_state) +{ + pose_component_->assign_loaned_state_interfaces(state_interfaces_); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_deactivate(const rclcpp_lifecycle::State& previous_state) +{ + pose_component_->release_interfaces(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_init() +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +} // namespace ur_controllers +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(ur_controllers::PoseBroadcaster, controller_interface::ControllerInterface) diff --git a/ur_controllers/src/pose_broadcaster_parameters.yaml b/ur_controllers/src/pose_broadcaster_parameters.yaml new file mode 100644 index 000000000..cc5a4d596 --- /dev/null +++ b/ur_controllers/src/pose_broadcaster_parameters.yaml @@ -0,0 +1,16 @@ +pose_broadcaster: + sensor_name: { + type: string, + default_value: "tcp_pose", + description: "State interface used to encode the pose." + } + parent_frame_id: { + type: string, + default_value: "base", + description: "Parent frame id of the published transform." + } + child_frame_id: { + type: string, + default_value: "tool0_controller", + description: "Child frame id of the published transform." + } diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index a512dc1ca..9f218868e 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -12,6 +12,9 @@ controller_manager: force_torque_sensor_broadcaster: type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + tcp_pose_broadcaster: + type: ur_controllers/PoseBroadcaster + joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController @@ -47,6 +50,12 @@ force_torque_sensor_broadcaster: frame_id: $(var tf_prefix)tool0 topic_name: ft_data +tcp_pose_broadcaster: + ros__parameters: + sensor_name: $(var tf_prefix)tcp_pose_sensor + child_frame_id: $(var tf_prefix)tool0_controller + parent_frame_id: $(var tf_prefix)base + joint_trajectory_controller: ros__parameters: diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 3ac85dbbf..e961a7937 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -285,6 +285,7 @@ def launch_setup(context, *args, **kwargs): "consistent_controllers": [ "io_and_status_controller", "force_torque_sensor_broadcaster", + "tcp_pose_broadcaster", "joint_state_broadcaster", "speed_scaling_state_broadcaster", ] @@ -329,6 +330,7 @@ def controller_spawner(name, active=True): "io_and_status_controller", "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", + # "tcp_pose_broadcaster" ] controller_spawner_inactive_names = ["forward_position_controller"] diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 2434e5248..cc884c75f 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -38,6 +38,7 @@ //---------------------------------------------------------------------- #include #include +#include #include #include #include @@ -169,9 +170,16 @@ std::vector URPositionHardwareInterface::exp &speed_scaling_combined_)); for (auto& sensor : info_.sensors) { - for (uint j = 0; j < sensor.state_interfaces.size(); ++j) { - state_interfaces.emplace_back(hardware_interface::StateInterface(sensor.name, sensor.state_interfaces[j].name, - &urcl_ft_sensor_measurements_[j])); + if (sensor.name == tf_prefix + "tcp_fts_sensor") { + for (uint j = 0; j < sensor.state_interfaces.size(); ++j) { + state_interfaces.emplace_back(hardware_interface::StateInterface(sensor.name, sensor.state_interfaces[j].name, + &urcl_ft_sensor_measurements_[j])); + } + } else if (sensor.name == tf_prefix + "tcp_pose_sensor") { + for (uint j = 0; j < sensor.state_interfaces.size(); ++j) { + state_interfaces.emplace_back( + hardware_interface::StateInterface(sensor.name, sensor.state_interfaces[j].name, &urcl_tcp_pose_[j])); + } } } @@ -421,6 +429,8 @@ URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous // distiguishable in the log const std::string tf_prefix = info_.hardware_parameters.at("tf_prefix"); RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Initializing driver..."); + tcp_transform_.header.frame_id = tf_prefix + "base"; + tcp_transform_.child_frame_id = tf_prefix + "tool0_controller"; registerUrclLogHandler(tf_prefix); try { rtde_comm_has_been_started_ = false; @@ -784,6 +794,8 @@ void URPositionHardwareInterface::extractToolPose() tcp_transform_.transform.translation.z = urcl_tcp_pose_[2]; tcp_transform_.transform.rotation = tf2::toMsg(rotation); + RCLCPP_INFO_STREAM_THROTTLE(rclcpp::get_logger("URPositionHardwareInterface"), *this->get_clock(), 500, + "TCP pose" << rosidl_generator_traits::to_yaml(tcp_transform_)); } hardware_interface::return_type URPositionHardwareInterface::prepare_command_mode_switch( From 32470f078c2079409c19e75232b0c849b7a9a970 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 20 Oct 2023 11:23:27 +0000 Subject: [PATCH 2/3] Add a pose_broadcaster controller to publish the controller's flange pose --- .../include/ur_controllers/sc_pose.hpp | 44 +++++----- ur_robot_driver/config/ur_controllers.yaml | 4 +- .../ur_robot_driver/hardware_interface.hpp | 7 ++ .../resources/rtde_output_recipe.txt | 2 + ur_robot_driver/src/hardware_interface.cpp | 83 +++++++++++++------ 5 files changed, 89 insertions(+), 51 deletions(-) diff --git a/ur_controllers/include/ur_controllers/sc_pose.hpp b/ur_controllers/include/ur_controllers/sc_pose.hpp index e11acf6bd..03d2897a6 100644 --- a/ur_controllers/include/ur_controllers/sc_pose.hpp +++ b/ur_controllers/include/ur_controllers/sc_pose.hpp @@ -35,15 +35,17 @@ */ //---------------------------------------------------------------------- +#pragma once + #include -#ifndef UR_CONTROLLERS__SC_POSE_HPP_ -#include -#include #include #include #include +#include +#include + #include #include @@ -54,12 +56,13 @@ class PoseComponent : public semantic_components::SemanticComponentInterface::quiet_NaN()); std::fill(orientation_.begin(), orientation_.end(), std::numeric_limits::quiet_NaN()); @@ -77,20 +80,15 @@ class PoseComponent : public semantic_components::SemanticComponentInterface get_orientation() + std::array get_orientation() { - size_t interface_counter = 0; - for (size_t i = 3; i < 6; ++i) { + size_t interface_counter = 3; + for (size_t i = 0; i < 4; ++i) { orientation_[i] = state_interfaces_[interface_counter].get().get_value(); ++interface_counter; } return orientation_; } std::array position_; - std::array orientation_; + std::array orientation_; }; } // namespace ur_controllers -#define UR_CONTROLLERS__SC_POSE_HPP_ -#endif // ifndef UR_CONTROLLERS__SC_POSE_HPP_ diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 9f218868e..b791183c2 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -52,8 +52,8 @@ force_torque_sensor_broadcaster: tcp_pose_broadcaster: ros__parameters: - sensor_name: $(var tf_prefix)tcp_pose_sensor - child_frame_id: $(var tf_prefix)tool0_controller + sensor_name: $(var tf_prefix)flange_pose_sensor + child_frame_id: $(var tf_prefix)flange_controller parent_frame_id: $(var tf_prefix)base 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 63f762f09..0065da985 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -40,6 +40,8 @@ #define UR_ROBOT_DRIVER__HARDWARE_INTERFACE_HPP_ // System +#include +#include #include #include #include @@ -127,6 +129,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface template void readBitsetData(const std::unique_ptr& data_pkg, const std::string& var_name, std::bitset& data); + tf2::Quaternion quaternionFromURPose(const urcl::vector6d_t& rotation_vec); void initAsyncIO(); void checkAsyncIO(); @@ -142,6 +145,10 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::vector6d_t urcl_joint_efforts_; urcl::vector6d_t urcl_ft_sensor_measurements_; urcl::vector6d_t urcl_tcp_pose_; + urcl::vector6d_t urcl_tcp_target_pose_; + urcl::vector6d_t urcl_tcp_offset_; + urcl::vector3d_t urcl_flange_position_; + std::array urcl_flange_orientation_; bool packet_read_; diff --git a/ur_robot_driver/resources/rtde_output_recipe.txt b/ur_robot_driver/resources/rtde_output_recipe.txt index 8d10be849..c06ebfeb5 100644 --- a/ur_robot_driver/resources/rtde_output_recipe.txt +++ b/ur_robot_driver/resources/rtde_output_recipe.txt @@ -6,6 +6,8 @@ target_speed_fraction runtime_state actual_TCP_force actual_TCP_pose +target_TCP_pose +tcp_offset actual_digital_input_bits actual_digital_output_bits standard_analog_input0 diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index cc884c75f..835cd89c0 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -36,18 +36,29 @@ * */ //---------------------------------------------------------------------- +#include +#include + +#include +#include + #include +#include #include -#include #include #include #include -#include "ur_client_library/exceptions.h" -#include "ur_client_library/ur/tool_communication.h" +#include +#include +#include +#include +#include + +#include + +#include -#include "rclcpp/rclcpp.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "ur_robot_driver/hardware_interface.hpp" #include "ur_robot_driver/urcl_log_handler.hpp" @@ -78,6 +89,10 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys urcl_joint_efforts_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; urcl_ft_sensor_measurements_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; urcl_tcp_pose_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; + urcl_tcp_target_pose_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; + urcl_tcp_offset_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; + urcl_flange_position_ = { { 0.0, 0.0, 0.0 } }; + urcl_flange_orientation_ = { { 0.0, 0.0, 0.0, 0.0 } }; urcl_position_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; urcl_position_commands_old_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; @@ -175,10 +190,14 @@ std::vector URPositionHardwareInterface::exp state_interfaces.emplace_back(hardware_interface::StateInterface(sensor.name, sensor.state_interfaces[j].name, &urcl_ft_sensor_measurements_[j])); } - } else if (sensor.name == tf_prefix + "tcp_pose_sensor") { - for (uint j = 0; j < sensor.state_interfaces.size(); ++j) { - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor.name, sensor.state_interfaces[j].name, &urcl_tcp_pose_[j])); + } else if (sensor.name == tf_prefix + "flange_pose_sensor") { + for (uint j = 0; j < 3; ++j) { + state_interfaces.emplace_back(hardware_interface::StateInterface(sensor.name, sensor.state_interfaces[j].name, + &urcl_flange_position_[j])); + } + for (uint j = 0; j < 4; ++j) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + sensor.name, sensor.state_interfaces[j + 3].name, &urcl_flange_orientation_[j])); } } } @@ -545,6 +564,8 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: readData(data_pkg, "runtime_state", runtime_state_); readData(data_pkg, "actual_TCP_force", urcl_ft_sensor_measurements_); readData(data_pkg, "actual_TCP_pose", urcl_tcp_pose_); + readData(data_pkg, "target_TCP_pose", urcl_tcp_target_pose_); + readData(data_pkg, "tcp_offset", urcl_tcp_offset_); readData(data_pkg, "standard_analog_input0", standard_analog_input_[0]); readData(data_pkg, "standard_analog_input1", standard_analog_input_[1]); readData(data_pkg, "standard_analog_output0", standard_analog_output_[0]); @@ -776,26 +797,38 @@ void URPositionHardwareInterface::transformForceTorque() tcp_torque_.x(), tcp_torque_.y(), tcp_torque_.z() }; } -void URPositionHardwareInterface::extractToolPose() +tf2::Quaternion URPositionHardwareInterface::quaternionFromURPose(const urcl::vector6d_t& pose) { - // 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 + double rot_angle = std::sqrt(std::pow(pose[3], 2) + std::pow(pose[4], 2) + std::pow(pose[5], 2)); + if (rot_angle > 1e-16) { + return tf2::Quaternion{ { pose[3] / rot_angle, pose[4] / rot_angle, pose[5] / rot_angle }, rot_angle }; } - 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]; + return tf2::Quaternion(0, 0, 0, 1); +} + +void URPositionHardwareInterface::extractToolPose() +{ + tf2::Quaternion rot_tool = quaternionFromURPose(urcl_tcp_pose_); + tf2::Quaternion rot_offset = quaternionFromURPose(urcl_tcp_offset_); + + tf2::Vector3 tcp_offset_in_tcp = tf2::quatRotate( + rot_offset.inverse(), tf2::Vector3(urcl_tcp_offset_[0], urcl_tcp_offset_[1], urcl_tcp_offset_[2])); + tf2::Vector3 tcp_offset_in_base = tf2::quatRotate(rot_tool, tcp_offset_in_tcp); + + urcl_flange_position_[0] = urcl_tcp_pose_[0] - tcp_offset_in_base.x(); + urcl_flange_position_[1] = urcl_tcp_pose_[1] - tcp_offset_in_base.y(); + urcl_flange_position_[2] = urcl_tcp_pose_[2] - tcp_offset_in_base.z(); + tcp_transform_.transform.translation.x = urcl_flange_position_[0]; + tcp_transform_.transform.translation.y = urcl_flange_position_[1]; + tcp_transform_.transform.translation.z = urcl_flange_position_[2]; + + tf2::Quaternion rotation = rot_tool * rot_offset.inverse(); + urcl_flange_orientation_[0] = rotation.x(); + urcl_flange_orientation_[1] = rotation.y(); + urcl_flange_orientation_[2] = rotation.z(); + urcl_flange_orientation_[3] = rotation.w(); tcp_transform_.transform.rotation = tf2::toMsg(rotation); - RCLCPP_INFO_STREAM_THROTTLE(rclcpp::get_logger("URPositionHardwareInterface"), *this->get_clock(), 500, - "TCP pose" << rosidl_generator_traits::to_yaml(tcp_transform_)); } hardware_interface::return_type URPositionHardwareInterface::prepare_command_mode_switch( From 324ec772148737a1470bcf9d06d0b5d467730299 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 25 Oct 2023 04:13:48 +0000 Subject: [PATCH 3/3] Automatically start tcp_pose_broadcaster --- ur_robot_driver/launch/ur_control.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index e961a7937..234ae4a49 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -330,7 +330,7 @@ def controller_spawner(name, active=True): "io_and_status_controller", "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", - # "tcp_pose_broadcaster" + "tcp_pose_broadcaster", ] controller_spawner_inactive_names = ["forward_position_controller"]