From 79364b3a31f2493dd4af30e134019081aef68083 Mon Sep 17 00:00:00 2001 From: urmarp Date: Mon, 9 Jan 2023 15:36:40 +0100 Subject: [PATCH 1/6] Added service to set force mode --- .../ur_controllers/gpio_controller.hpp | 30 ++++++++ ur_controllers/src/gpio_controller.cpp | 69 +++++++++++++++++ ur_robot_driver/CMakeLists.txt | 2 + .../ur_robot_driver/hardware_interface.hpp | 8 ++ ur_robot_driver/src/hardware_interface.cpp | 77 +++++++++++++++++++ 5 files changed, 186 insertions(+) diff --git a/ur_controllers/include/ur_controllers/gpio_controller.hpp b/ur_controllers/include/ur_controllers/gpio_controller.hpp index c20bd680e..d62b22b63 100644 --- a/ur_controllers/include/ur_controllers/gpio_controller.hpp +++ b/ur_controllers/include/ur_controllers/gpio_controller.hpp @@ -53,6 +53,7 @@ #include "ur_msgs/srv/set_io.hpp" #include "ur_msgs/srv/set_speed_slider_fraction.hpp" #include "ur_msgs/srv/set_payload.hpp" +#include "ur_msgs/srv/set_force_mode.hpp" #include "rclcpp/time.hpp" #include "rclcpp/duration.hpp" #include "std_msgs/msg/bool.hpp" @@ -79,6 +80,32 @@ enum CommandInterfaces ZERO_FTSENSOR_ASYNC_SUCCESS = 32, HAND_BACK_CONTROL_CMD = 33, HAND_BACK_CONTROL_ASYNC_SUCCESS = 34, + FORCE_MODE_TASK_FRAME_X = 35, + FORCE_MODE_TASK_FRAME_Y = 36, + FORCE_MODE_TASK_FRAME_Z = 37, + FORCE_MODE_TASK_FRAME_RX = 38, + FORCE_MODE_TASK_FRAME_RY = 39, + FORCE_MODE_TASK_FRAME_RZ = 40, + FORCE_MODE_SELECTION_VECTOR_X = 41, + FORCE_MODE_SELECTION_VECTOR_Y = 42, + FORCE_MODE_SELECTION_VECTOR_Z = 43, + FORCE_MODE_SELECTION_VECTOR_RX = 44, + FORCE_MODE_SELECTION_VECTOR_RY = 45, + FORCE_MODE_SELECTION_VECTOR_RZ = 46, + FORCE_MODE_WRENCH_X = 47, + FORCE_MODE_WRENCH_Y = 48, + FORCE_MODE_WRENCH_Z = 49, + FORCE_MODE_WRENCH_RX = 50, + FORCE_MODE_WRENCH_RY = 51, + FORCE_MODE_WRENCH_RZ = 52, + FORCE_MODE_TYPE = 53, + FORCE_MODE_LIMITS_X = 54, + FORCE_MODE_LIMITS_Y = 55, + FORCE_MODE_LIMITS_Z = 56, + FORCE_MODE_LIMITS_RX = 57, + FORCE_MODE_LIMITS_RY = 58, + FORCE_MODE_LIMITS_RZ = 59, + FORCE_MODE_ASYNC_SUCCESS = 60, }; enum StateInterfaces @@ -135,6 +162,8 @@ class GPIOController : public controller_interface::ControllerInterface ur_msgs::srv::SetPayload::Response::SharedPtr resp); bool zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp); + bool setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req, + ur_msgs::srv::SetForceMode::Response::SharedPtr resp); void publishIO(); @@ -163,6 +192,7 @@ class GPIOController : public controller_interface::ControllerInterface rclcpp::Service::SharedPtr set_io_srv_; rclcpp::Service::SharedPtr set_payload_srv_; rclcpp::Service::SharedPtr tare_sensor_srv_; + rclcpp::Service::SharedPtr set_force_mode_srv_; std::shared_ptr> io_pub_; std::shared_ptr> tool_data_pub_; diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index ca65375f4..9aedebc6b 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -99,6 +99,34 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_cmd"); config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_async_success"); + // force mode + config.names.emplace_back("force_mode/task_frame_x"); + config.names.emplace_back("force_mode/task_frame_y"); + config.names.emplace_back("force_mode/task_frame_z"); + config.names.emplace_back("force_mode/task_frame_rx"); + config.names.emplace_back("force_mode/task_frame_ry"); + config.names.emplace_back("force_mode/task_frame_rz"); + config.names.emplace_back("force_mode/selection_vector_x"); + config.names.emplace_back("force_mode/selection_vector_y"); + config.names.emplace_back("force_mode/selection_vector_z"); + config.names.emplace_back("force_mode/selection_vector_rx"); + config.names.emplace_back("force_mode/selection_vector_ry"); + config.names.emplace_back("force_mode/selection_vector_rz"); + config.names.emplace_back("force_mode/wrench_x"); + config.names.emplace_back("force_mode/wrench_y"); + config.names.emplace_back("force_mode/wrench_z"); + config.names.emplace_back("force_mode/wrench_rx"); + config.names.emplace_back("force_mode/wrench_ry"); + config.names.emplace_back("force_mode/wrench_rz"); + config.names.emplace_back("force_mode/type"); + config.names.emplace_back("force_mode/limits_x"); + config.names.emplace_back("force_mode/limits_y"); + config.names.emplace_back("force_mode/limits_z"); + config.names.emplace_back("force_mode/limits_rx"); + config.names.emplace_back("force_mode/limits_ry"); + config.names.emplace_back("force_mode/limits_rz"); + config.names.emplace_back("force_mode/force_mode_async_success"); + return config; } @@ -318,6 +346,9 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre tare_sensor_srv_ = get_node()->create_service( "~/zero_ftsensor", std::bind(&GPIOController::zeroFTSensor, this, std::placeholders::_1, std::placeholders::_2)); + set_force_mode_srv_ = get_node()->create_service( + "~/set_force_mode", + std::bind(&GPIOController::setForceMode, this, std::placeholders::_1, std::placeholders::_2)); } catch (...) { return LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -526,6 +557,44 @@ bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*r return true; } +bool GPIOController::setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req, + ur_msgs::srv::SetForceMode::Response::SharedPtr resp) +{ + if (req->task_frame.size() != 6 || req->selection_vector.size() != 6 || req->wrench.size() != 6 || + req->limits.size() != 6) { + RCLCPP_WARN(get_node()->get_logger(), "Size of received force mode message is wrong"); + resp->success = false; + return false; + } + + // reset success flag + command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING); + + for (size_t i = 0; i < 6; i++) { + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_X + i].set_value(req->task_frame[i]); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_X + i].set_value(req->selection_vector[i]); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_X + i].set_value(req->wrench[i]); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X + i].set_value(req->limits[i]); + } + command_interfaces_[CommandInterfaces::FORCE_MODE_TYPE].set_value(unsigned(req->type)); + + while (command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) { + // Asynchronous wait until the hardware interface has set the force mode + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + resp->success = static_cast(command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value()); + + if (resp->success) { + RCLCPP_INFO(get_node()->get_logger(), "Force mode has been set successfully"); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Could not set the force mode"); + return false; + } + + return true; +} + void GPIOController::initMsgs() { io_msg_.digital_in_states.resize(standard_digital_output_cmd_.size()); diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 89492f54f..b5486350f 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -32,6 +32,7 @@ find_package(std_srvs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(ur_client_library REQUIRED) find_package(ur_dashboard_msgs REQUIRED) +find_package(ur_msgs REQUIRED) include_directories(include) @@ -48,6 +49,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2_geometry_msgs ur_client_library ur_dashboard_msgs + ur_msgs ) add_library(ur_robot_driver_plugin 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..d8df545e2 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -195,6 +195,14 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double payload_mass_; double payload_async_success_; + // force mode parameters + urcl::vector6d_t force_mode_task_frame_; + urcl::vector6d_t force_mode_selection_vector_; + urcl::vector6d_t force_mode_wrench_; + urcl::vector6d_t force_mode_limits_; + double force_mode_type_; + double force_mode_async_success_; + // copy of non double values std::array actual_dig_out_bits_copy_; std::array actual_dig_in_bits_copy_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index ddbf8b91a..7a03e96f1 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -279,6 +279,58 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back( hardware_interface::CommandInterface(tf_prefix + "payload", "payload_async_success", &payload_async_success_)); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "task_frame_x", &force_mode_task_frame_[0])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "task_frame_y", &force_mode_task_frame_[1])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "task_frame_z", &force_mode_task_frame_[2])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "task_frame_rx", &force_mode_task_frame_[3])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "task_frame_ry", &force_mode_task_frame_[4])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "task_frame_rz", &force_mode_task_frame_[5])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "selection_vector_x", &force_mode_selection_vector_[0])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "selection_vector_y", &force_mode_selection_vector_[1])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "selection_vector_z", &force_mode_selection_vector_[2])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "selection_vector_rx", &force_mode_selection_vector_[3])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "selection_vector_ry", &force_mode_selection_vector_[4])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "selection_vector_rz", &force_mode_selection_vector_[5])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "wrench_x", &force_mode_wrench_[0])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "wrench_y", &force_mode_wrench_[1])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "wrench_z", &force_mode_wrench_[2])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "wrench_rx", &force_mode_wrench_[3])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "wrench_ry", &force_mode_wrench_[4])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "wrench_rz", &force_mode_wrench_[5])); + command_interfaces.emplace_back(hardware_interface::CommandInterface("force_mode", "type", &force_mode_type_)); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "limits_x", &force_mode_limits_[0])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "limits_y", &force_mode_limits_[1])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "limits_z", &force_mode_limits_[2])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "limits_rx", &force_mode_limits_[3])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "limits_ry", &force_mode_limits_[4])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "limits_rz", &force_mode_limits_[5])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("force_mode", "force_mode_async_success", &force_mode_async_success_)); + for (size_t i = 0; i < 18; ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "gpio", "standard_digital_output_cmd_" + std::to_string(i), &standard_dig_out_bits_cmd_[i])); @@ -649,6 +701,14 @@ void URPositionHardwareInterface::initAsyncIO() payload_mass_ = NO_NEW_CMD_; payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ }; + + for (size_t i = 0; i < 6; i++) { + force_mode_task_frame_[i] = NO_NEW_CMD_; + force_mode_selection_vector_[i] = static_cast(NO_NEW_CMD_); + force_mode_wrench_[i] = NO_NEW_CMD_; + force_mode_limits_[i] = NO_NEW_CMD_; + } + force_mode_type_ = static_cast(NO_NEW_CMD_); } void URPositionHardwareInterface::checkAsyncIO() @@ -716,6 +776,23 @@ void URPositionHardwareInterface::checkAsyncIO() zero_ftsensor_async_success_ = ur_driver_->zeroFTSensor(); zero_ftsensor_cmd_ = NO_NEW_CMD_; } + if (!std::isnan(force_mode_task_frame_[0]) && !std::isnan(force_mode_selection_vector_[0]) && + !std::isnan(force_mode_wrench_[0]) && !std::isnan(force_mode_type_) && !std::isnan(force_mode_limits_[0]) && + ur_driver_ != nullptr) { + urcl::vector6uint32_t force_mode_selection_vector; + for (size_t i = 0; i < 6; i++) { + force_mode_selection_vector[i] = force_mode_selection_vector_[i]; + } + force_mode_async_success_ = ur_driver_->startForceMode(force_mode_task_frame_, force_mode_selection_vector, + force_mode_wrench_, force_mode_type_, force_mode_limits_); + for (size_t i = 0; i < 6; i++) { + force_mode_task_frame_[i] = NO_NEW_CMD_; + force_mode_selection_vector_[i] = static_cast(NO_NEW_CMD_); + force_mode_wrench_[i] = NO_NEW_CMD_; + force_mode_limits_[i] = NO_NEW_CMD_; + } + force_mode_type_ = static_cast(NO_NEW_CMD_); + } } void URPositionHardwareInterface::updateNonDoubleValues() From 50d28f0e8d200c61746486a6b2c4686c5a62ef0a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 27 Jun 2023 06:28:41 +0200 Subject: [PATCH 2/6] Use tf_prefix for force_mode handles --- ur_controllers/src/gpio_controller.cpp | 52 +++++++-------- ur_robot_driver/src/hardware_interface.cpp | 77 ++++++++-------------- 2 files changed, 52 insertions(+), 77 deletions(-) diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index 9aedebc6b..85dfaa339 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -100,32 +100,32 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_async_success"); // force mode - config.names.emplace_back("force_mode/task_frame_x"); - config.names.emplace_back("force_mode/task_frame_y"); - config.names.emplace_back("force_mode/task_frame_z"); - config.names.emplace_back("force_mode/task_frame_rx"); - config.names.emplace_back("force_mode/task_frame_ry"); - config.names.emplace_back("force_mode/task_frame_rz"); - config.names.emplace_back("force_mode/selection_vector_x"); - config.names.emplace_back("force_mode/selection_vector_y"); - config.names.emplace_back("force_mode/selection_vector_z"); - config.names.emplace_back("force_mode/selection_vector_rx"); - config.names.emplace_back("force_mode/selection_vector_ry"); - config.names.emplace_back("force_mode/selection_vector_rz"); - config.names.emplace_back("force_mode/wrench_x"); - config.names.emplace_back("force_mode/wrench_y"); - config.names.emplace_back("force_mode/wrench_z"); - config.names.emplace_back("force_mode/wrench_rx"); - config.names.emplace_back("force_mode/wrench_ry"); - config.names.emplace_back("force_mode/wrench_rz"); - config.names.emplace_back("force_mode/type"); - config.names.emplace_back("force_mode/limits_x"); - config.names.emplace_back("force_mode/limits_y"); - config.names.emplace_back("force_mode/limits_z"); - config.names.emplace_back("force_mode/limits_rx"); - config.names.emplace_back("force_mode/limits_ry"); - config.names.emplace_back("force_mode/limits_rz"); - config.names.emplace_back("force_mode/force_mode_async_success"); + config.names.emplace_back(tf_prefix + "force_mode/task_frame_x"); + config.names.emplace_back(tf_prefix + "force_mode/task_frame_y"); + config.names.emplace_back(tf_prefix + "force_mode/task_frame_z"); + config.names.emplace_back(tf_prefix + "force_mode/task_frame_rx"); + config.names.emplace_back(tf_prefix + "force_mode/task_frame_ry"); + config.names.emplace_back(tf_prefix + "force_mode/task_frame_rz"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_x"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_y"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_z"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_rx"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_ry"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_rz"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_x"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_y"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_z"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_rx"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_ry"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_rz"); + config.names.emplace_back(tf_prefix + "force_mode/type"); + config.names.emplace_back(tf_prefix + "force_mode/limits_x"); + config.names.emplace_back(tf_prefix + "force_mode/limits_y"); + config.names.emplace_back(tf_prefix + "force_mode/limits_z"); + config.names.emplace_back(tf_prefix + "force_mode/limits_rx"); + config.names.emplace_back(tf_prefix + "force_mode/limits_ry"); + config.names.emplace_back(tf_prefix + "force_mode/limits_rz"); + config.names.emplace_back(tf_prefix + "force_mode/force_mode_async_success"); return config; } diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 7a03e96f1..098b0272e 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -279,57 +279,32 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back( hardware_interface::CommandInterface(tf_prefix + "payload", "payload_async_success", &payload_async_success_)); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "task_frame_x", &force_mode_task_frame_[0])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "task_frame_y", &force_mode_task_frame_[1])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "task_frame_z", &force_mode_task_frame_[2])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "task_frame_rx", &force_mode_task_frame_[3])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "task_frame_ry", &force_mode_task_frame_[4])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "task_frame_rz", &force_mode_task_frame_[5])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "selection_vector_x", &force_mode_selection_vector_[0])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "selection_vector_y", &force_mode_selection_vector_[1])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "selection_vector_z", &force_mode_selection_vector_[2])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "selection_vector_rx", &force_mode_selection_vector_[3])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "selection_vector_ry", &force_mode_selection_vector_[4])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "selection_vector_rz", &force_mode_selection_vector_[5])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "wrench_x", &force_mode_wrench_[0])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "wrench_y", &force_mode_wrench_[1])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "wrench_z", &force_mode_wrench_[2])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "wrench_rx", &force_mode_wrench_[3])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "wrench_ry", &force_mode_wrench_[4])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "wrench_rz", &force_mode_wrench_[5])); - command_interfaces.emplace_back(hardware_interface::CommandInterface("force_mode", "type", &force_mode_type_)); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "limits_x", &force_mode_limits_[0])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "limits_y", &force_mode_limits_[1])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "limits_z", &force_mode_limits_[2])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "limits_rx", &force_mode_limits_[3])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "limits_ry", &force_mode_limits_[4])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "limits_rz", &force_mode_limits_[5])); - command_interfaces.emplace_back( - hardware_interface::CommandInterface("force_mode", "force_mode_async_success", &force_mode_async_success_)); + command_interfaces.emplace_back(tf_prefix + "force_mode", "task_frame_x", &force_mode_task_frame_[0]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "task_frame_y", &force_mode_task_frame_[1]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "task_frame_z", &force_mode_task_frame_[2]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "task_frame_rx", &force_mode_task_frame_[3]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "task_frame_ry", &force_mode_task_frame_[4]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "task_frame_rz", &force_mode_task_frame_[5]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "selection_vector_x", &force_mode_selection_vector_[0]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "selection_vector_y", &force_mode_selection_vector_[1]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "selection_vector_z", &force_mode_selection_vector_[2]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "selection_vector_rx", &force_mode_selection_vector_[3]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "selection_vector_ry", &force_mode_selection_vector_[4]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "selection_vector_rz", &force_mode_selection_vector_[5]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "wrench_x", &force_mode_wrench_[0]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "wrench_y", &force_mode_wrench_[1]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "wrench_z", &force_mode_wrench_[2]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "wrench_rx", &force_mode_wrench_[3]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "wrench_ry", &force_mode_wrench_[4]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "wrench_rz", &force_mode_wrench_[5]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "type", &force_mode_type_); + command_interfaces.emplace_back(tf_prefix + "force_mode", "limits_x", &force_mode_limits_[0]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "limits_y", &force_mode_limits_[1]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "limits_z", &force_mode_limits_[2]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "limits_rx", &force_mode_limits_[3]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "limits_ry", &force_mode_limits_[4]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "limits_rz", &force_mode_limits_[5]); + command_interfaces.emplace_back(tf_prefix + "force_mode", "force_mode_async_success", &force_mode_async_success_); for (size_t i = 0; i < 18; ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( From c5605c00d985e5955f82d3048f05d07c03d3e6ee Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 28 Jun 2023 14:04:21 +0200 Subject: [PATCH 3/6] Fix stuff --- ur_controllers/CMakeLists.txt | 6 ++ .../ur_controllers/gpio_controller.hpp | 6 ++ ur_controllers/package.xml | 3 + ur_controllers/src/gpio_controller.cpp | 57 ++++++++++++++----- 4 files changed, 59 insertions(+), 13 deletions(-) diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index a2b72e599..f0596d28f 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(geometry_msgs 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_ros 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 + geometry_msgs 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_ros ur_dashboard_msgs ur_msgs generate_parameter_library diff --git a/ur_controllers/include/ur_controllers/gpio_controller.hpp b/ur_controllers/include/ur_controllers/gpio_controller.hpp index d62b22b63..ab9361d7b 100644 --- a/ur_controllers/include/ur_controllers/gpio_controller.hpp +++ b/ur_controllers/include/ur_controllers/gpio_controller.hpp @@ -38,11 +38,14 @@ #ifndef UR_CONTROLLERS__GPIO_CONTROLLER_HPP_ #define UR_CONTROLLERS__GPIO_CONTROLLER_HPP_ +#include +#include #include #include #include #include +#include "geometry_msgs/msg/pose_stamped.hpp" #include "std_srvs/srv/trigger.hpp" #include "controller_interface/controller_interface.hpp" @@ -200,6 +203,9 @@ class GPIOController : public controller_interface::ControllerInterface std::shared_ptr> safety_mode_pub_; std::shared_ptr> program_state_pub_; + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + ur_msgs::msg::IOStates io_msg_; ur_msgs::msg::ToolDataMsg tool_data_msg_; ur_dashboard_msgs::msg::RobotMode robot_mode_msg_; diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index 183a7486c..318961745 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -22,6 +22,7 @@ angles controller_interface + geometry_msgs joint_trajectory_controller lifecycle_msgs pluginlib @@ -30,6 +31,8 @@ realtime_tools std_msgs std_srvs + tf2_geometry_msgs + tf2_ros ur_dashboard_msgs ur_msgs diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index 85dfaa339..e899099aa 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -35,9 +35,10 @@ */ //---------------------------------------------------------------------- -#include "ur_controllers/gpio_controller.hpp" +#include -#include +#include "ur_controllers/gpio_controller.hpp" +#include namespace ur_controllers { @@ -48,7 +49,6 @@ controller_interface::CallbackReturn GPIOController::on_init() // Create the parameter listener and get the parameters param_listener_ = std::make_shared(get_node()); params_ = param_listener_->get_params(); - } catch (const std::exception& e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; @@ -218,6 +218,9 @@ ur_controllers::GPIOController::on_configure(const rclcpp_lifecycle::State& /*pr // get parameters from the listener in case they were updated params_ = param_listener_->get_params(); + tf_buffer_ = std::make_unique(get_node()->get_clock()); + tf_listener_ = std::make_unique(*tf_buffer_); + return LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -560,24 +563,52 @@ bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*r bool GPIOController::setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req, ur_msgs::srv::SetForceMode::Response::SharedPtr resp) { - if (req->task_frame.size() != 6 || req->selection_vector.size() != 6 || req->wrench.size() != 6 || - req->limits.size() != 6) { - RCLCPP_WARN(get_node()->get_logger(), "Size of received force mode message is wrong"); - resp->success = false; - return false; - } - // reset success flag + RCLCPP_INFO(get_node()->get_logger(), "Currently we have %zu command interfaces", command_interfaces_.size()); + command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING); + // transform task frame into base + const std::string tf_prefix = params_.tf_prefix; + try { + auto task_frame_transformed = tf_buffer_->transform(req->task_frame, tf_prefix + "base"); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_X].set_value(task_frame_transformed.pose.position.x); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Y].set_value(task_frame_transformed.pose.position.y); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Z].set_value(task_frame_transformed.pose.position.z); + + tf2::Quaternion quat_tf; + tf2::convert(task_frame_transformed.pose.orientation, quat_tf); + tf2::Matrix3x3 rot_mat(quat_tf); + std::vector rot(3); + rot_mat.getRPY(rot[0], rot[1], rot[2]); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RX].set_value(task_frame_transformed.pose.position.x); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RY].set_value(task_frame_transformed.pose.position.y); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RZ].set_value(task_frame_transformed.pose.position.z); + } catch (const tf2::TransformException& ex) { + RCLCPP_ERROR(get_node()->get_logger(), "Could not transform %s to robot base: %s", + req->task_frame.header.frame_id.c_str(), ex.what()); + } + + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_X].set_value(req->selection_vector_x); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Y].set_value(req->selection_vector_y); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Z].set_value(req->selection_vector_z); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RX].set_value(req->selection_vector_rx); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RY].set_value(req->selection_vector_ry); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RZ].set_value(req->selection_vector_rz); + + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_X].set_value(req->wrench.force.x); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Y].set_value(req->wrench.force.y); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Z].set_value(req->wrench.force.z); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RX].set_value(req->wrench.torque.x); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RY].set_value(req->wrench.torque.y); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RZ].set_value(req->wrench.torque.z); + for (size_t i = 0; i < 6; i++) { - command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_X + i].set_value(req->task_frame[i]); - command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_X + i].set_value(req->selection_vector[i]); - command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_X + i].set_value(req->wrench[i]); command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X + i].set_value(req->limits[i]); } command_interfaces_[CommandInterfaces::FORCE_MODE_TYPE].set_value(unsigned(req->type)); + RCLCPP_INFO(get_node()->get_logger(), "Waiting for force mode to be set."); while (command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) { // Asynchronous wait until the hardware interface has set the force mode std::this_thread::sleep_for(std::chrono::milliseconds(50)); From 68be5932c9466be59e4c3605af233aedc6d0ab84 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 28 Jun 2023 22:35:37 +0200 Subject: [PATCH 4/6] More bug fixing --- ur_controllers/src/gpio_controller.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index e899099aa..4eaafc8de 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -564,7 +564,6 @@ bool GPIOController::setForceMode(const ur_msgs::srv::SetForceMode::Request::Sha ur_msgs::srv::SetForceMode::Response::SharedPtr resp) { // reset success flag - RCLCPP_INFO(get_node()->get_logger(), "Currently we have %zu command interfaces", command_interfaces_.size()); command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING); @@ -581,9 +580,9 @@ bool GPIOController::setForceMode(const ur_msgs::srv::SetForceMode::Request::Sha tf2::Matrix3x3 rot_mat(quat_tf); std::vector rot(3); rot_mat.getRPY(rot[0], rot[1], rot[2]); - command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RX].set_value(task_frame_transformed.pose.position.x); - command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RY].set_value(task_frame_transformed.pose.position.y); - command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RZ].set_value(task_frame_transformed.pose.position.z); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RX].set_value(rot[0]); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RY].set_value(rot[1]); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RZ].set_value(rot[2]); } catch (const tf2::TransformException& ex) { RCLCPP_ERROR(get_node()->get_logger(), "Could not transform %s to robot base: %s", req->task_frame.header.frame_id.c_str(), ex.what()); @@ -608,7 +607,7 @@ bool GPIOController::setForceMode(const ur_msgs::srv::SetForceMode::Request::Sha } command_interfaces_[CommandInterfaces::FORCE_MODE_TYPE].set_value(unsigned(req->type)); - RCLCPP_INFO(get_node()->get_logger(), "Waiting for force mode to be set."); + RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for force mode to be set."); while (command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) { // Asynchronous wait until the hardware interface has set the force mode std::this_thread::sleep_for(std::chrono::milliseconds(50)); @@ -617,9 +616,9 @@ bool GPIOController::setForceMode(const ur_msgs::srv::SetForceMode::Request::Sha resp->success = static_cast(command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value()); if (resp->success) { - RCLCPP_INFO(get_node()->get_logger(), "Force mode has been set successfully"); + RCLCPP_INFO(get_node()->get_logger(), "Force mode has been set successfully."); } else { - RCLCPP_ERROR(get_node()->get_logger(), "Could not set the force mode"); + RCLCPP_ERROR(get_node()->get_logger(), "Could not set the force mode."); return false; } From 4bbd1c091ea6a830a66a138fcdc0cbb165cd7420 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 28 Jun 2023 22:36:03 +0200 Subject: [PATCH 5/6] Add service to disable force mode --- .../ur_controllers/gpio_controller.hpp | 4 +++ ur_controllers/src/gpio_controller.cpp | 25 ++++++++++++++++++- .../ur_robot_driver/hardware_interface.hpp | 1 + ur_robot_driver/src/hardware_interface.cpp | 7 ++++++ 4 files changed, 36 insertions(+), 1 deletion(-) diff --git a/ur_controllers/include/ur_controllers/gpio_controller.hpp b/ur_controllers/include/ur_controllers/gpio_controller.hpp index ab9361d7b..a7d77806e 100644 --- a/ur_controllers/include/ur_controllers/gpio_controller.hpp +++ b/ur_controllers/include/ur_controllers/gpio_controller.hpp @@ -109,6 +109,7 @@ enum CommandInterfaces FORCE_MODE_LIMITS_RY = 58, FORCE_MODE_LIMITS_RZ = 59, FORCE_MODE_ASYNC_SUCCESS = 60, + FORCE_MODE_DISABLE_CMD = 61, }; enum StateInterfaces @@ -167,6 +168,8 @@ class GPIOController : public controller_interface::ControllerInterface bool zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp); bool setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req, ur_msgs::srv::SetForceMode::Response::SharedPtr resp); + bool disableForceMode(const std_srvs::srv::Trigger::Request::SharedPtr req, + std_srvs::srv::Trigger::Response::SharedPtr resp); void publishIO(); @@ -196,6 +199,7 @@ class GPIOController : public controller_interface::ControllerInterface rclcpp::Service::SharedPtr set_payload_srv_; rclcpp::Service::SharedPtr tare_sensor_srv_; rclcpp::Service::SharedPtr set_force_mode_srv_; + rclcpp::Service::SharedPtr disable_force_mode_srv_; std::shared_ptr> io_pub_; std::shared_ptr> tool_data_pub_; diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index 4eaafc8de..f76c5564a 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -126,6 +126,7 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c config.names.emplace_back(tf_prefix + "force_mode/limits_ry"); config.names.emplace_back(tf_prefix + "force_mode/limits_rz"); config.names.emplace_back(tf_prefix + "force_mode/force_mode_async_success"); + config.names.emplace_back(tf_prefix + "force_mode/disable_cmd"); return config; } @@ -352,6 +353,9 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre set_force_mode_srv_ = get_node()->create_service( "~/set_force_mode", std::bind(&GPIOController::setForceMode, this, std::placeholders::_1, std::placeholders::_2)); + disable_force_mode_srv_ = get_node()->create_service( + "~/disable_force_mode", + std::bind(&GPIOController::disableForceMode, this, std::placeholders::_1, std::placeholders::_2)); } catch (...) { return LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -564,7 +568,6 @@ bool GPIOController::setForceMode(const ur_msgs::srv::SetForceMode::Request::Sha ur_msgs::srv::SetForceMode::Response::SharedPtr resp) { // reset success flag - command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING); // transform task frame into base @@ -625,6 +628,26 @@ bool GPIOController::setForceMode(const ur_msgs::srv::SetForceMode::Request::Sha return true; } +bool GPIOController::disableForceMode(const std_srvs::srv::Trigger::Request::SharedPtr req, + std_srvs::srv::Trigger::Response::SharedPtr resp) +{ + command_interfaces_[CommandInterfaces::FORCE_MODE_DISABLE_CMD].set_value(1); + + RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for force mode to be disabled."); + while (command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) { + // Asynchronous wait until the hardware interface has set the force mode + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + resp->success = static_cast(command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value()); + if (resp->success) { + RCLCPP_INFO(get_node()->get_logger(), "Force mode has been disabled successfully."); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Could not disable force mode."); + return false; + } + return true; +} + void GPIOController::initMsgs() { io_msg_.digital_in_states.resize(standard_digital_output_cmd_.size()); 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 d8df545e2..60c04671d 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -202,6 +202,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::vector6d_t force_mode_limits_; double force_mode_type_; double force_mode_async_success_; + double force_mode_disable_cmd_; // copy of non double values std::array actual_dig_out_bits_copy_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 098b0272e..42b148293 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -305,6 +305,7 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(tf_prefix + "force_mode", "limits_ry", &force_mode_limits_[4]); command_interfaces.emplace_back(tf_prefix + "force_mode", "limits_rz", &force_mode_limits_[5]); command_interfaces.emplace_back(tf_prefix + "force_mode", "force_mode_async_success", &force_mode_async_success_); + command_interfaces.emplace_back(tf_prefix + "force_mode", "disable_cmd", &force_mode_disable_cmd_); for (size_t i = 0; i < 18; ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( @@ -619,6 +620,7 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: resend_robot_program_cmd_ = NO_NEW_CMD_; zero_ftsensor_cmd_ = NO_NEW_CMD_; hand_back_control_cmd_ = NO_NEW_CMD_; + force_mode_disable_cmd_ = NO_NEW_CMD_; initialized_ = true; } @@ -768,6 +770,11 @@ void URPositionHardwareInterface::checkAsyncIO() } force_mode_type_ = static_cast(NO_NEW_CMD_); } + + if (!std::isnan(force_mode_disable_cmd_) && ur_driver_ != nullptr) { + force_mode_async_success_ = ur_driver_->endForceMode(); + force_mode_disable_cmd_ = NO_NEW_CMD_; + } } void URPositionHardwareInterface::updateNonDoubleValues() From a267c8d5f3f6c79be831df1621164752761eb12c Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 30 Jun 2023 17:01:41 +0200 Subject: [PATCH 6/6] Use an own controller for force_mode --- ur_controllers/CMakeLists.txt | 6 + ur_controllers/controller_plugins.xml | 5 + .../ur_controllers/force_mode_controller.hpp | 124 +++++++++ .../ur_controllers/gpio_controller.hpp | 40 --- ur_controllers/src/force_mode_controller.cpp | 262 ++++++++++++++++++ .../src/force_mode_controller_parameters.yaml | 29 ++ ur_controllers/src/gpio_controller.cpp | 123 -------- ur_robot_driver/config/ur_controllers.yaml | 7 + .../ur_robot_driver/hardware_interface.hpp | 2 + ur_robot_driver/launch/ur_control.launch.py | 3 +- ur_robot_driver/src/hardware_interface.cpp | 2 + 11 files changed, 439 insertions(+), 164 deletions(-) create mode 100644 ur_controllers/include/ur_controllers/force_mode_controller.hpp create mode 100644 ur_controllers/src/force_mode_controller.cpp create mode 100644 ur_controllers/src/force_mode_controller_parameters.yaml diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index f0596d28f..46ef4536c 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -44,6 +44,10 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS include_directories(include) +generate_parameter_library( + force_mode_controller_parameters + src/force_mode_controller_parameters.yaml +) generate_parameter_library( gpio_controller_parameters @@ -61,6 +65,7 @@ generate_parameter_library( ) add_library(${PROJECT_NAME} SHARED + src/force_mode_controller.cpp src/scaled_joint_trajectory_controller.cpp src/speed_scaling_state_broadcaster.cpp src/gpio_controller.cpp) @@ -69,6 +74,7 @@ target_include_directories(${PROJECT_NAME} PRIVATE include ) target_link_libraries(${PROJECT_NAME} + force_mode_controller_parameters gpio_controller_parameters speed_scaling_state_broadcaster_parameters scaled_joint_trajectory_controller_parameters diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index f0058ab55..917bb0984 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -14,4 +14,9 @@ This controller publishes the Tool IO. + + + Controller to use UR's force_mode. + + diff --git a/ur_controllers/include/ur_controllers/force_mode_controller.hpp b/ur_controllers/include/ur_controllers/force_mode_controller.hpp new file mode 100644 index 000000000..28ab6cc18 --- /dev/null +++ b/ur_controllers/include/ur_controllers/force_mode_controller.hpp @@ -0,0 +1,124 @@ +// Copyright 2023, FZI Forschungszentrum Informatik, Created on behalf of Universal Robots A/S +// +// 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 exner@fzi.de + * \date 2023-06-29 + */ +//---------------------------------------------------------------------- + +#pragma once +#include +#include +#include + +#include +#include +#include +#include + +#include "force_mode_controller_parameters.hpp" + +namespace ur_controllers +{ +enum CommandInterfaces +{ + FORCE_MODE_TASK_FRAME_X = 0u, + FORCE_MODE_TASK_FRAME_Y = 1, + FORCE_MODE_TASK_FRAME_Z = 2, + FORCE_MODE_TASK_FRAME_RX = 3, + FORCE_MODE_TASK_FRAME_RY = 4, + FORCE_MODE_TASK_FRAME_RZ = 5, + FORCE_MODE_SELECTION_VECTOR_X = 6, + FORCE_MODE_SELECTION_VECTOR_Y = 7, + FORCE_MODE_SELECTION_VECTOR_Z = 8, + FORCE_MODE_SELECTION_VECTOR_RX = 9, + FORCE_MODE_SELECTION_VECTOR_RY = 10, + FORCE_MODE_SELECTION_VECTOR_RZ = 11, + FORCE_MODE_WRENCH_X = 12, + FORCE_MODE_WRENCH_Y = 13, + FORCE_MODE_WRENCH_Z = 14, + FORCE_MODE_WRENCH_RX = 15, + FORCE_MODE_WRENCH_RY = 16, + FORCE_MODE_WRENCH_RZ = 17, + FORCE_MODE_TYPE = 18, + FORCE_MODE_LIMITS_X = 19, + FORCE_MODE_LIMITS_Y = 20, + FORCE_MODE_LIMITS_Z = 21, + FORCE_MODE_LIMITS_RX = 22, + FORCE_MODE_LIMITS_RY = 23, + FORCE_MODE_LIMITS_RZ = 24, + FORCE_MODE_ASYNC_SUCCESS = 25, + FORCE_MODE_DISABLE_CMD = 26, + FORCE_MODE_DAMPING = 27, + FORCE_MODE_GAIN_SCALING = 28, +}; +enum StateInterfaces +{ + INITIALIZED_FLAG = 0u, +}; + +class ForceModeController : public controller_interface::ControllerInterface +{ +public: + 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; + + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_init() override; + +private: + bool setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req, + ur_msgs::srv::SetForceMode::Response::SharedPtr resp); + bool disableForceMode(); + rclcpp::Service::SharedPtr set_force_mode_srv_; + + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + + std::shared_ptr param_listener_; + force_mode_controller::Params params_; + + static constexpr double ASYNC_WAITING = 2.0; + /** + * @brief wait until a command interface isn't in state ASYNC_WAITING anymore or until the parameter maximum_retries + * have been reached + */ + bool waitForAsyncCommand(std::function get_value); +}; +} // namespace ur_controllers diff --git a/ur_controllers/include/ur_controllers/gpio_controller.hpp b/ur_controllers/include/ur_controllers/gpio_controller.hpp index a7d77806e..c20bd680e 100644 --- a/ur_controllers/include/ur_controllers/gpio_controller.hpp +++ b/ur_controllers/include/ur_controllers/gpio_controller.hpp @@ -38,14 +38,11 @@ #ifndef UR_CONTROLLERS__GPIO_CONTROLLER_HPP_ #define UR_CONTROLLERS__GPIO_CONTROLLER_HPP_ -#include -#include #include #include #include #include -#include "geometry_msgs/msg/pose_stamped.hpp" #include "std_srvs/srv/trigger.hpp" #include "controller_interface/controller_interface.hpp" @@ -56,7 +53,6 @@ #include "ur_msgs/srv/set_io.hpp" #include "ur_msgs/srv/set_speed_slider_fraction.hpp" #include "ur_msgs/srv/set_payload.hpp" -#include "ur_msgs/srv/set_force_mode.hpp" #include "rclcpp/time.hpp" #include "rclcpp/duration.hpp" #include "std_msgs/msg/bool.hpp" @@ -83,33 +79,6 @@ enum CommandInterfaces ZERO_FTSENSOR_ASYNC_SUCCESS = 32, HAND_BACK_CONTROL_CMD = 33, HAND_BACK_CONTROL_ASYNC_SUCCESS = 34, - FORCE_MODE_TASK_FRAME_X = 35, - FORCE_MODE_TASK_FRAME_Y = 36, - FORCE_MODE_TASK_FRAME_Z = 37, - FORCE_MODE_TASK_FRAME_RX = 38, - FORCE_MODE_TASK_FRAME_RY = 39, - FORCE_MODE_TASK_FRAME_RZ = 40, - FORCE_MODE_SELECTION_VECTOR_X = 41, - FORCE_MODE_SELECTION_VECTOR_Y = 42, - FORCE_MODE_SELECTION_VECTOR_Z = 43, - FORCE_MODE_SELECTION_VECTOR_RX = 44, - FORCE_MODE_SELECTION_VECTOR_RY = 45, - FORCE_MODE_SELECTION_VECTOR_RZ = 46, - FORCE_MODE_WRENCH_X = 47, - FORCE_MODE_WRENCH_Y = 48, - FORCE_MODE_WRENCH_Z = 49, - FORCE_MODE_WRENCH_RX = 50, - FORCE_MODE_WRENCH_RY = 51, - FORCE_MODE_WRENCH_RZ = 52, - FORCE_MODE_TYPE = 53, - FORCE_MODE_LIMITS_X = 54, - FORCE_MODE_LIMITS_Y = 55, - FORCE_MODE_LIMITS_Z = 56, - FORCE_MODE_LIMITS_RX = 57, - FORCE_MODE_LIMITS_RY = 58, - FORCE_MODE_LIMITS_RZ = 59, - FORCE_MODE_ASYNC_SUCCESS = 60, - FORCE_MODE_DISABLE_CMD = 61, }; enum StateInterfaces @@ -166,10 +135,6 @@ class GPIOController : public controller_interface::ControllerInterface ur_msgs::srv::SetPayload::Response::SharedPtr resp); bool zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp); - bool setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req, - ur_msgs::srv::SetForceMode::Response::SharedPtr resp); - bool disableForceMode(const std_srvs::srv::Trigger::Request::SharedPtr req, - std_srvs::srv::Trigger::Response::SharedPtr resp); void publishIO(); @@ -198,8 +163,6 @@ class GPIOController : public controller_interface::ControllerInterface rclcpp::Service::SharedPtr set_io_srv_; rclcpp::Service::SharedPtr set_payload_srv_; rclcpp::Service::SharedPtr tare_sensor_srv_; - rclcpp::Service::SharedPtr set_force_mode_srv_; - rclcpp::Service::SharedPtr disable_force_mode_srv_; std::shared_ptr> io_pub_; std::shared_ptr> tool_data_pub_; @@ -207,9 +170,6 @@ class GPIOController : public controller_interface::ControllerInterface std::shared_ptr> safety_mode_pub_; std::shared_ptr> program_state_pub_; - std::unique_ptr tf_buffer_; - std::unique_ptr tf_listener_; - ur_msgs::msg::IOStates io_msg_; ur_msgs::msg::ToolDataMsg tool_data_msg_; ur_dashboard_msgs::msg::RobotMode robot_mode_msg_; diff --git a/ur_controllers/src/force_mode_controller.cpp b/ur_controllers/src/force_mode_controller.cpp new file mode 100644 index 000000000..bde84ba79 --- /dev/null +++ b/ur_controllers/src/force_mode_controller.cpp @@ -0,0 +1,262 @@ +// Copyright 2023, FZI Forschungszentrum Informatik, Created on behalf of Universal Robots A/S +// +// 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 exner@fzi.de + * \date 2023-06-29 + */ +//---------------------------------------------------------------------- + +#include +#include + +#include +namespace ur_controllers +{ +controller_interface::CallbackReturn ForceModeController::on_init() +{ + try { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } catch (const std::exception& e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} +controller_interface::InterfaceConfiguration ForceModeController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + const std::string tf_prefix = params_.tf_prefix; + RCLCPP_DEBUG(get_node()->get_logger(), "Configure UR force_mode controller with tf_prefix: %s", tf_prefix.c_str()); + + config.names.emplace_back(tf_prefix + "force_mode/task_frame_x"); + config.names.emplace_back(tf_prefix + "force_mode/task_frame_y"); + config.names.emplace_back(tf_prefix + "force_mode/task_frame_z"); + config.names.emplace_back(tf_prefix + "force_mode/task_frame_rx"); + config.names.emplace_back(tf_prefix + "force_mode/task_frame_ry"); + config.names.emplace_back(tf_prefix + "force_mode/task_frame_rz"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_x"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_y"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_z"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_rx"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_ry"); + config.names.emplace_back(tf_prefix + "force_mode/selection_vector_rz"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_x"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_y"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_z"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_rx"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_ry"); + config.names.emplace_back(tf_prefix + "force_mode/wrench_rz"); + config.names.emplace_back(tf_prefix + "force_mode/type"); + config.names.emplace_back(tf_prefix + "force_mode/limits_x"); + config.names.emplace_back(tf_prefix + "force_mode/limits_y"); + config.names.emplace_back(tf_prefix + "force_mode/limits_z"); + config.names.emplace_back(tf_prefix + "force_mode/limits_rx"); + config.names.emplace_back(tf_prefix + "force_mode/limits_ry"); + config.names.emplace_back(tf_prefix + "force_mode/limits_rz"); + config.names.emplace_back(tf_prefix + "force_mode/force_mode_async_success"); + config.names.emplace_back(tf_prefix + "force_mode/disable_cmd"); + config.names.emplace_back(tf_prefix + "force_mode/damping"); + config.names.emplace_back(tf_prefix + "force_mode/gain_scaling"); + + return config; +} + +controller_interface::InterfaceConfiguration ur_controllers::ForceModeController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + const std::string tf_prefix = params_.tf_prefix; + + config.names.emplace_back(tf_prefix + "system_interface/initialized"); + + return config; +} + +controller_interface::return_type ur_controllers::ForceModeController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) +{ + // Publish state of force_mode? + + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn +ur_controllers::ForceModeController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) +{ + const auto logger = get_node()->get_logger(); + + if (!param_listener_) { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + + tf_buffer_ = std::make_unique(get_node()->get_clock()); + tf_listener_ = std::make_unique(*tf_buffer_); + + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ur_controllers::ForceModeController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) +{ + RCLCPP_WARN(get_node()->get_logger(), "The ForceModeController is considered a beta feature. Its interface might " + "change in the future."); + while (state_interfaces_[StateInterfaces::INITIALIZED_FLAG].get_value() == 0.0) { + RCLCPP_INFO(get_node()->get_logger(), "Waiting for system interface to initialize..."); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + try { + set_force_mode_srv_ = get_node()->create_service( + "~/set_force_mode", + std::bind(&ForceModeController::setForceMode, this, std::placeholders::_1, std::placeholders::_2)); + } catch (...) { + return LifecycleNodeInterface::CallbackReturn::ERROR; + } + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ur_controllers::ForceModeController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) +{ + disableForceMode(); + try { + set_force_mode_srv_.reset(); + } catch (...) { + return LifecycleNodeInterface::CallbackReturn::ERROR; + } + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req, + ur_msgs::srv::SetForceMode::Response::SharedPtr resp) +{ + // reset success flag + command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING); + + // transform task frame into base + const std::string tf_prefix = params_.tf_prefix; + try { + auto task_frame_transformed = tf_buffer_->transform(req->task_frame, tf_prefix + "base"); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_X].set_value(task_frame_transformed.pose.position.x); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Y].set_value(task_frame_transformed.pose.position.y); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Z].set_value(task_frame_transformed.pose.position.z); + + tf2::Quaternion quat_tf; + tf2::convert(task_frame_transformed.pose.orientation, quat_tf); + tf2::Matrix3x3 rot_mat(quat_tf); + std::vector rot(3); + rot_mat.getRPY(rot[0], rot[1], rot[2]); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RX].set_value(rot[0]); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RY].set_value(rot[1]); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RZ].set_value(rot[2]); + } catch (const tf2::TransformException& ex) { + RCLCPP_ERROR(get_node()->get_logger(), "Could not transform %s to robot base: %s", + req->task_frame.header.frame_id.c_str(), ex.what()); + } + + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_X].set_value(req->selection_vector_x); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Y].set_value(req->selection_vector_y); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Z].set_value(req->selection_vector_z); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RX].set_value(req->selection_vector_rx); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RY].set_value(req->selection_vector_ry); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RZ].set_value(req->selection_vector_rz); + + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_X].set_value(req->wrench.force.x); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Y].set_value(req->wrench.force.y); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Z].set_value(req->wrench.force.z); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RX].set_value(req->wrench.torque.x); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RY].set_value(req->wrench.torque.y); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RZ].set_value(req->wrench.torque.z); + + for (size_t i = 0; i < 6; i++) { + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X + i].set_value(req->limits[i]); + } + command_interfaces_[CommandInterfaces::FORCE_MODE_TYPE].set_value(unsigned(req->type)); + + RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for force mode to be set."); + const auto maximum_retries = params_.check_io_successful_retries; + int retries = 0; + while (command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + retries++; + + if (retries > maximum_retries) { + resp->success = false; + } + } + resp->success = static_cast(command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value()); + + if (resp->success) { + RCLCPP_INFO(get_node()->get_logger(), "Force mode has been set successfully."); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Could not set the force mode."); + return false; + } + + return true; +} + +bool ForceModeController::disableForceMode() +{ + command_interfaces_[CommandInterfaces::FORCE_MODE_DISABLE_CMD].set_value(1); + + RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for force mode to be disabled."); + while (command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) { + // Asynchronous wait until the hardware interface has set the force mode + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + bool success = static_cast(command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value()); + if (success) { + RCLCPP_INFO(get_node()->get_logger(), "Force mode has been disabled successfully."); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Could not disable force mode."); + return false; + } + return true; +} +} // namespace ur_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(ur_controllers::ForceModeController, controller_interface::ControllerInterface) diff --git a/ur_controllers/src/force_mode_controller_parameters.yaml b/ur_controllers/src/force_mode_controller_parameters.yaml new file mode 100644 index 000000000..afef9a247 --- /dev/null +++ b/ur_controllers/src/force_mode_controller_parameters.yaml @@ -0,0 +1,29 @@ +force_mode_controller: + damping: { + type: double, + default_value: 0.5, + description: "Sets the damping parameter in force mode. A value of 1 is full damping, so the robot will decelerate quickly if no force is present. A value of 0 is no damping, here the robot will maintain the speed.", + read_only: true, + validation: { + bounds<>: [0.0, 1.0] + } + } + gain_scaling: { + type: double, + default_value: 1.0, + description: "Scales the gain in force mode. A value larger than 1 can make force mode unstable, e.g. in case of collisions or pushing against hard surfaces.", + read_only: true, + validation: { + bounds<>: [0.0, 2.0] + } + } + tf_prefix: { + type: string, + default_value: "", + description: "Urdf prefix of the corresponding arm" + } + check_io_successful_retries: { + type: int, + default_value: 10, + description: "Amount of retries for checking if setting force_mode was successful" + } diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index f76c5564a..9510cd304 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -38,7 +38,6 @@ #include #include "ur_controllers/gpio_controller.hpp" -#include namespace ur_controllers { @@ -99,35 +98,6 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_cmd"); config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_async_success"); - // force mode - config.names.emplace_back(tf_prefix + "force_mode/task_frame_x"); - config.names.emplace_back(tf_prefix + "force_mode/task_frame_y"); - config.names.emplace_back(tf_prefix + "force_mode/task_frame_z"); - config.names.emplace_back(tf_prefix + "force_mode/task_frame_rx"); - config.names.emplace_back(tf_prefix + "force_mode/task_frame_ry"); - config.names.emplace_back(tf_prefix + "force_mode/task_frame_rz"); - config.names.emplace_back(tf_prefix + "force_mode/selection_vector_x"); - config.names.emplace_back(tf_prefix + "force_mode/selection_vector_y"); - config.names.emplace_back(tf_prefix + "force_mode/selection_vector_z"); - config.names.emplace_back(tf_prefix + "force_mode/selection_vector_rx"); - config.names.emplace_back(tf_prefix + "force_mode/selection_vector_ry"); - config.names.emplace_back(tf_prefix + "force_mode/selection_vector_rz"); - config.names.emplace_back(tf_prefix + "force_mode/wrench_x"); - config.names.emplace_back(tf_prefix + "force_mode/wrench_y"); - config.names.emplace_back(tf_prefix + "force_mode/wrench_z"); - config.names.emplace_back(tf_prefix + "force_mode/wrench_rx"); - config.names.emplace_back(tf_prefix + "force_mode/wrench_ry"); - config.names.emplace_back(tf_prefix + "force_mode/wrench_rz"); - config.names.emplace_back(tf_prefix + "force_mode/type"); - config.names.emplace_back(tf_prefix + "force_mode/limits_x"); - config.names.emplace_back(tf_prefix + "force_mode/limits_y"); - config.names.emplace_back(tf_prefix + "force_mode/limits_z"); - config.names.emplace_back(tf_prefix + "force_mode/limits_rx"); - config.names.emplace_back(tf_prefix + "force_mode/limits_ry"); - config.names.emplace_back(tf_prefix + "force_mode/limits_rz"); - config.names.emplace_back(tf_prefix + "force_mode/force_mode_async_success"); - config.names.emplace_back(tf_prefix + "force_mode/disable_cmd"); - return config; } @@ -219,9 +189,6 @@ ur_controllers::GPIOController::on_configure(const rclcpp_lifecycle::State& /*pr // get parameters from the listener in case they were updated params_ = param_listener_->get_params(); - tf_buffer_ = std::make_unique(get_node()->get_clock()); - tf_listener_ = std::make_unique(*tf_buffer_); - return LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -350,12 +317,6 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre tare_sensor_srv_ = get_node()->create_service( "~/zero_ftsensor", std::bind(&GPIOController::zeroFTSensor, this, std::placeholders::_1, std::placeholders::_2)); - set_force_mode_srv_ = get_node()->create_service( - "~/set_force_mode", - std::bind(&GPIOController::setForceMode, this, std::placeholders::_1, std::placeholders::_2)); - disable_force_mode_srv_ = get_node()->create_service( - "~/disable_force_mode", - std::bind(&GPIOController::disableForceMode, this, std::placeholders::_1, std::placeholders::_2)); } catch (...) { return LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -564,90 +525,6 @@ bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*r return true; } -bool GPIOController::setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req, - ur_msgs::srv::SetForceMode::Response::SharedPtr resp) -{ - // reset success flag - command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING); - - // transform task frame into base - const std::string tf_prefix = params_.tf_prefix; - try { - auto task_frame_transformed = tf_buffer_->transform(req->task_frame, tf_prefix + "base"); - command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_X].set_value(task_frame_transformed.pose.position.x); - command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Y].set_value(task_frame_transformed.pose.position.y); - command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Z].set_value(task_frame_transformed.pose.position.z); - - tf2::Quaternion quat_tf; - tf2::convert(task_frame_transformed.pose.orientation, quat_tf); - tf2::Matrix3x3 rot_mat(quat_tf); - std::vector rot(3); - rot_mat.getRPY(rot[0], rot[1], rot[2]); - command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RX].set_value(rot[0]); - command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RY].set_value(rot[1]); - command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RZ].set_value(rot[2]); - } catch (const tf2::TransformException& ex) { - RCLCPP_ERROR(get_node()->get_logger(), "Could not transform %s to robot base: %s", - req->task_frame.header.frame_id.c_str(), ex.what()); - } - - command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_X].set_value(req->selection_vector_x); - command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Y].set_value(req->selection_vector_y); - command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Z].set_value(req->selection_vector_z); - command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RX].set_value(req->selection_vector_rx); - command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RY].set_value(req->selection_vector_ry); - command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RZ].set_value(req->selection_vector_rz); - - command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_X].set_value(req->wrench.force.x); - command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Y].set_value(req->wrench.force.y); - command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Z].set_value(req->wrench.force.z); - command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RX].set_value(req->wrench.torque.x); - command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RY].set_value(req->wrench.torque.y); - command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RZ].set_value(req->wrench.torque.z); - - for (size_t i = 0; i < 6; i++) { - command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X + i].set_value(req->limits[i]); - } - command_interfaces_[CommandInterfaces::FORCE_MODE_TYPE].set_value(unsigned(req->type)); - - RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for force mode to be set."); - while (command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) { - // Asynchronous wait until the hardware interface has set the force mode - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - } - - resp->success = static_cast(command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value()); - - if (resp->success) { - RCLCPP_INFO(get_node()->get_logger(), "Force mode has been set successfully."); - } else { - RCLCPP_ERROR(get_node()->get_logger(), "Could not set the force mode."); - return false; - } - - return true; -} - -bool GPIOController::disableForceMode(const std_srvs::srv::Trigger::Request::SharedPtr req, - std_srvs::srv::Trigger::Response::SharedPtr resp) -{ - command_interfaces_[CommandInterfaces::FORCE_MODE_DISABLE_CMD].set_value(1); - - RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for force mode to be disabled."); - while (command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) { - // Asynchronous wait until the hardware interface has set the force mode - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - } - resp->success = static_cast(command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value()); - if (resp->success) { - RCLCPP_INFO(get_node()->get_logger(), "Force mode has been disabled successfully."); - } else { - RCLCPP_ERROR(get_node()->get_logger(), "Could not disable force mode."); - return false; - } - return true; -} - void GPIOController::initMsgs() { io_msg_.digital_in_states.resize(standard_digital_output_cmd_.size()); diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 01eb555d4..63408e618 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -24,6 +24,9 @@ controller_manager: forward_position_controller: type: position_controllers/JointGroupPositionController + force_mode_controller: + type: ur_controllers/ForceModeController + speed_scaling_state_broadcaster: ros__parameters: @@ -124,3 +127,7 @@ forward_position_controller: - wrist_1_joint - wrist_2_joint - wrist_3_joint + +force_mode_controller: + ros__parameters: + tf_prefix: "" 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 60c04671d..0d0ab47dd 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -203,6 +203,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double force_mode_type_; double force_mode_async_success_; double force_mode_disable_cmd_; + double force_mode_damping_; + double force_mode_gain_scaling_; // copy of non double values std::array actual_dig_out_bits_copy_; diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 11c9d3482..ccd3701c7 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -214,6 +214,7 @@ def launch_setup(context, *args, **kwargs): executable="ur_ros2_control_node", parameters=[robot_description, update_rate_config_file, initial_joint_controllers], output="screen", + # prefix=["xterm -e gdb -ex run --args"], condition=UnlessCondition(use_fake_hardware), ) @@ -301,7 +302,7 @@ def controller_spawner(name, active=True): "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", ] - controller_spawner_inactive_names = ["forward_position_controller"] + controller_spawner_inactive_names = ["forward_position_controller", "force_mode_controller"] controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [ controller_spawner(name, active=False) for name in controller_spawner_inactive_names diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 42b148293..f93e661c3 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -306,6 +306,8 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(tf_prefix + "force_mode", "limits_rz", &force_mode_limits_[5]); command_interfaces.emplace_back(tf_prefix + "force_mode", "force_mode_async_success", &force_mode_async_success_); command_interfaces.emplace_back(tf_prefix + "force_mode", "disable_cmd", &force_mode_disable_cmd_); + command_interfaces.emplace_back(tf_prefix + "force_mode", "damping", &force_mode_damping_); + command_interfaces.emplace_back(tf_prefix + "force_mode", "gain_scaling", &force_mode_gain_scaling_); for (size_t i = 0; i < 18; ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface(