From dedc84605b4a4f8919d9ca211e1ff5536cd73f3f Mon Sep 17 00:00:00 2001 From: urmarp Date: Mon, 9 Jan 2023 15:36:40 +0100 Subject: [PATCH 1/8] 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 d5e6d16ee..89ca8cb16 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -97,6 +97,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; } @@ -312,6 +340,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; } @@ -520,6 +551,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 d341b911f..c9e471180 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -33,6 +33,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) @@ -49,6 +50,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 67e27a222..90593797f 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -197,6 +197,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 5826cf595..34897ce91 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])); @@ -660,6 +712,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() @@ -727,6 +787,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 f3720256b5b6d1ca62e1be1a984a24cd20b44438 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 27 Jun 2023 06:28:41 +0200 Subject: [PATCH 2/8] 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 89ca8cb16..a25358ee0 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -98,32 +98,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 34897ce91..1b8ec8b00 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 040c76a4998a151ac1f2984e4fd8279e71757933 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 28 Jun 2023 14:04:21 +0200 Subject: [PATCH 3/8] Fix stuff --- ur_controllers/CMakeLists.txt | 6 ++ .../ur_controllers/gpio_controller.hpp | 6 ++ ur_controllers/package.xml | 3 + ur_controllers/src/gpio_controller.cpp | 56 +++++++++++++++---- 4 files changed, 59 insertions(+), 12 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 3b77510bd..b0020999d 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 a25358ee0..95cc23623 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 { @@ -216,6 +217,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; } @@ -554,24 +558,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 3c5298607ae5e2207ad4ba29f5bbc939961093bb Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 28 Jun 2023 22:35:37 +0200 Subject: [PATCH 4/8] 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 95cc23623..76263a5d0 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -559,7 +559,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); @@ -576,9 +575,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()); @@ -603,7 +602,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)); @@ -612,9 +611,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 f0c5ea64e8fa14c16b1b6b32d74f80cab29d0fe7 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 28 Jun 2023 22:36:03 +0200 Subject: [PATCH 5/8] 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 76263a5d0..d000215dd 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -125,6 +125,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; } @@ -347,6 +348,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; } @@ -559,7 +563,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 @@ -620,6 +623,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 90593797f..4d7931dfd 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -204,6 +204,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 1b8ec8b00..22ff034aa 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( @@ -630,6 +631,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; } @@ -779,6 +781,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 949610cf07686cc4dee9214e106dae40e57a3206 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 30 Jun 2023 17:01:41 +0200 Subject: [PATCH 6/8] 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 | 2 +- ur_robot_driver/src/hardware_interface.cpp | 2 + 11 files changed, 438 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 d000215dd..9549b77f0 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 { @@ -98,35 +97,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; } @@ -218,9 +188,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; } @@ -345,12 +312,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; } @@ -559,90 +520,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 a512dc1ca..b4dbca0a6 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: - $(var tf_prefix)wrist_1_joint - $(var tf_prefix)wrist_2_joint - $(var tf_prefix)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 4d7931dfd..461aa734f 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -205,6 +205,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 461f15034..76f1442a7 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -159,7 +159,7 @@ def controller_spawner(controllers, active=True): "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", ] - controllers_inactive = ["forward_position_controller"] + controllers_inactive = ["forward_position_controller", "force_mode_controller"] controller_spawners = [controller_spawner(controllers_active)] + [ controller_spawner(controllers_inactive, active=False) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 22ff034aa..1671e838c 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( From 62f02eaaa1840895b8eaf24c0f2809a384e77d16 Mon Sep 17 00:00:00 2001 From: URJala Date: Mon, 8 Jul 2024 12:46:30 +0000 Subject: [PATCH 7/8] Add force mode arguments: damping and gain scale to force mode controller --- ur_controllers/src/force_mode_controller.cpp | 48 +++++-- .../src/force_mode_controller_parameters.yaml | 4 +- ur_robot_driver/config/ur_controllers.yaml | 2 +- ur_robot_driver/src/hardware_interface.cpp | 24 +++- ur_robot_driver/test/robot_driver.py | 118 +++++++++++++++++- ur_robot_driver/test/test_common.py | 11 +- 6 files changed, 184 insertions(+), 23 deletions(-) diff --git a/ur_controllers/src/force_mode_controller.cpp b/ur_controllers/src/force_mode_controller.cpp index bde84ba79..1b9a03c1b 100644 --- a/ur_controllers/src/force_mode_controller.cpp +++ b/ur_controllers/src/force_mode_controller.cpp @@ -61,6 +61,7 @@ controller_interface::InterfaceConfiguration ForceModeController::command_interf 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()); + // Get all the command interfaces needed for force mode from the hardware interface 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"); @@ -100,7 +101,7 @@ controller_interface::InterfaceConfiguration ur_controllers::ForceModeController config.type = controller_interface::interface_configuration_type::INDIVIDUAL; const std::string tf_prefix = params_.tf_prefix; - + // Get the state interface indicating whether the hardware interface has been initialized config.names.emplace_back(tf_prefix + "system_interface/initialized"); return config; @@ -120,7 +121,7 @@ ur_controllers::ForceModeController::on_configure(const rclcpp_lifecycle::State& const auto logger = get_node()->get_logger(); if (!param_listener_) { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during configuration"); return controller_interface::CallbackReturn::ERROR; } @@ -146,9 +147,10 @@ ur_controllers::ForceModeController::on_activate(const rclcpp_lifecycle::State& std::this_thread::sleep_for(std::chrono::milliseconds(50)); } + // Create the service server that will be used to start force mode try { set_force_mode_srv_ = get_node()->create_service( - "~/set_force_mode", + "~/start_force_mode", std::bind(&ForceModeController::setForceMode, this, std::placeholders::_1, std::placeholders::_2)); } catch (...) { return LifecycleNodeInterface::CallbackReturn::ERROR; @@ -159,6 +161,7 @@ ur_controllers::ForceModeController::on_activate(const rclcpp_lifecycle::State& controller_interface::CallbackReturn ur_controllers::ForceModeController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) { + // Stop force mode if this controller is deactivated. disableForceMode(); try { set_force_mode_srv_.reset(); @@ -195,6 +198,7 @@ bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request req->task_frame.header.frame_id.c_str(), ex.what()); } + /* The selection vector dictates which axes the robot should be compliant along and around */ 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); @@ -202,18 +206,34 @@ bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request 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]); - } + /* The wrench parameters dictate the amount of force/torque the robot will apply to its environment. The robot will + * move along/around compliant axes to match the specified force/torque. Has no effect for non-compliant axes. */ + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_X].set_value(req->wrench.wrench.force.x); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Y].set_value(req->wrench.wrench.force.y); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Z].set_value(req->wrench.wrench.force.z); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RX].set_value(req->wrench.wrench.torque.x); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RY].set_value(req->wrench.wrench.torque.y); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RZ].set_value(req->wrench.wrench.torque.z); + + /* The limits specifies the maximum allowed speed along/around compliant axes. For non-compliant axes this value is + * the maximum allowed deviation between actual tcp position and the one that has been programmed. */ + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X].set_value(req->limits.twist.linear.x); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Y].set_value(req->limits.twist.linear.y); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Z].set_value(req->limits.twist.linear.z); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RX].set_value(req->limits.twist.angular.x); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RY].set_value(req->limits.twist.angular.y); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RZ].set_value(req->limits.twist.angular.z); + + /* The type decides how the robot interprets the force frame (the one defined in task_frame). See ur_script manual for + * explanation, under force_mode. */ command_interfaces_[CommandInterfaces::FORCE_MODE_TYPE].set_value(unsigned(req->type)); + /* The damping factor decides how fast the robot decelarates if no force is present. 0 means no deceleration, 1 means + * quick deceleration*/ + command_interfaces_[CommandInterfaces::FORCE_MODE_DAMPING].set_value(req->damping_factor); + /*The gain scaling factor scales the force mode gain. A value larger than 1 may make force mode unstable. */ + command_interfaces_[CommandInterfaces::FORCE_MODE_GAIN_SCALING].set_value(req->gain_scaling); + 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; @@ -225,6 +245,7 @@ bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request resp->success = false; } } + resp->success = static_cast(command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value()); if (resp->success) { @@ -239,6 +260,7 @@ bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request bool ForceModeController::disableForceMode() { + command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING); command_interfaces_[CommandInterfaces::FORCE_MODE_DISABLE_CMD].set_value(1); RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for force mode to be disabled."); diff --git a/ur_controllers/src/force_mode_controller_parameters.yaml b/ur_controllers/src/force_mode_controller_parameters.yaml index afef9a247..835606780 100644 --- a/ur_controllers/src/force_mode_controller_parameters.yaml +++ b/ur_controllers/src/force_mode_controller_parameters.yaml @@ -1,7 +1,7 @@ force_mode_controller: damping: { type: double, - default_value: 0.5, + default_value: 0.025, 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: { @@ -10,7 +10,7 @@ force_mode_controller: } gain_scaling: { type: double, - default_value: 1.0, + default_value: 0.5, 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: { diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index b4dbca0a6..c139e82d4 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -130,4 +130,4 @@ forward_position_controller: force_mode_controller: ros__parameters: - tf_prefix: "" + tf_prefix: "$(var tf_prefix)" diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 1671e838c..9b4f58779 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -768,13 +768,27 @@ void URPositionHardwareInterface::checkAsyncIO() } 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) { + !std::isnan(force_mode_damping_) && !std::isnan(force_mode_gain_scaling_) && 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_); + /* Check version of robot to ensure that the correct startForceMode is called. */ + if (ur_driver_->getVersion().major < 5) { + force_mode_async_success_ = + ur_driver_->startForceMode(force_mode_task_frame_, force_mode_selection_vector, force_mode_wrench_, + force_mode_type_, force_mode_limits_, force_mode_damping_); + if (force_mode_gain_scaling_ != 0.5) { + RCLCPP_WARN(rclcpp::get_logger("URPositionHardwareInterface"), "Force mode gain scaling cannot be used on CB3 " + "robots. Starting force mode, but disregarding " + "gain scaling."); + } + } else { + force_mode_async_success_ = ur_driver_->startForceMode(force_mode_task_frame_, force_mode_selection_vector, + force_mode_wrench_, force_mode_type_, force_mode_limits_, + force_mode_damping_, force_mode_gain_scaling_); + } + 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_); @@ -782,9 +796,11 @@ void URPositionHardwareInterface::checkAsyncIO() force_mode_limits_[i] = NO_NEW_CMD_; } force_mode_type_ = static_cast(NO_NEW_CMD_); + force_mode_damping_ = NO_NEW_CMD_; + force_mode_gain_scaling_ = NO_NEW_CMD_; } - if (!std::isnan(force_mode_disable_cmd_) && ur_driver_ != nullptr) { + if (!std::isnan(force_mode_disable_cmd_) && ur_driver_ != nullptr && force_mode_async_success_ == 2.0) { force_mode_async_success_ = ur_driver_->endForceMode(); force_mode_disable_cmd_ = NO_NEW_CMD_; } diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 96c859947..cdfb63431 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -35,12 +35,24 @@ import launch_testing import pytest import rclpy +import std_msgs from builtin_interfaces.msg import Duration from control_msgs.action import FollowJointTrajectory from controller_manager_msgs.srv import SwitchController from rclpy.node import Node from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from geometry_msgs.msg import ( + Pose, + PoseStamped, + Quaternion, + Point, + WrenchStamped, + TwistStamped, + Wrench, + Twist, + Vector3, +) from ur_msgs.msg import IOStates sys.path.append(os.path.dirname(__file__)) @@ -49,6 +61,7 @@ ControllerManagerInterface, DashboardInterface, IoStatusInterface, + ForceModeInterface, generate_driver_test_description, ) @@ -92,7 +105,6 @@ def init_robot(self): self._dashboard_interface = DashboardInterface(self.node) self._controller_manager_interface = ControllerManagerInterface(self.node) self._io_status_controller_interface = IoStatusInterface(self.node) - self._scaled_follow_joint_trajectory = ActionInterface( self.node, "/scaled_joint_trajectory_controller/follow_joint_trajectory", @@ -100,13 +112,115 @@ def init_robot(self): ) def setUp(self): - self._dashboard_interface.start_robot() + # self._dashboard_interface.start_robot() time.sleep(1) self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) # # Test functions # + def test_force_mode_controller(self, tf_prefix): + # Activate force mode and trajectory controller, to make sure the robot can be moved. + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "force_mode_controller", + "scaled_joint_trajectory_controller", + ], + ).ok + ) + self._force_mode_controller_interface = ForceModeInterface(self.node) + + # Move the robot away from any singularities created in other tests. + # Construct test trajectory + test_trajectory = [ + (Duration(sec=10, nanosec=0), [-1.5 for j in ROBOT_JOINTS]), + ] + + trajectory = JointTrajectory( + joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS], + points=[ + JointTrajectoryPoint(positions=test_pos, time_from_start=test_time) + for (test_time, test_pos) in test_trajectory + ], + ) + + # Sending trajectory goal + logging.info("Sending simple goal") + goal_handle = self._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory) + self.assertTrue(goal_handle.accepted) + + # Verify execution + result = self._scaled_follow_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) + # Finished moving + + # Create task frame for force mode + point = Point(x=0.8, y=0.8, z=0.8) + orientation = Quaternion(x=0.0, y=0.2, z=0.5, w=0.3) + task_frame_pose = Pose() + task_frame_pose.position = point + task_frame_pose.orientation = orientation + header = std_msgs.msg.Header(seq=1, frame_id="world") + header.stamp.sec = int(time.time()) + 1 + header.stamp.nanosec = 0 + frame_stamp = PoseStamped() + frame_stamp.header = header + frame_stamp.pose = task_frame_pose + + # Create compliance vector (which axes should be force controlled) + compliance = [False, False, True, False, False, True] + + # Create Wrench message for force mode + wrench_vec = Wrench() + wrench_vec.force = Vector3(x=1.0, y=2.0, z=3.0) + wrench_vec.torque = wrench_vec.force + wrench_stamp = WrenchStamped() + wrench_stamp.header = header + wrench_stamp.wrench = wrench_vec + + # Specify interpretation of task frame (no transform) + type_spec = 2 + + # Specify max speeds and deviations of force mode + limits = Twist() + limits.linear = Vector3(x=1.1, y=1.1, z=1.1) + limits.angular = limits.linear + limits_stamp = TwistStamped() + limits_stamp.header = header + limits_stamp.twist = limits + + # specify damping and gain scaling + damping_factor = 0.8 + gain_scale = 0.8 + + # Send request to controller + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, + selection_vector_x=compliance[0], + selection_vector_y=compliance[1], + selection_vector_z=compliance[2], + selection_vector_rx=compliance[3], + selection_vector_ry=compliance[4], + selection_vector_rz=compliance[5], + wrench=wrench_stamp, + type=type_spec, + limits=limits_stamp, + damping_factor=damping_factor, + gain_scaling=gain_scale, + ) + self.assertEqual(res.success, True) + + # Deactivate controller to stop force mode + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["force_mode_controller"], + ).ok + ) def test_start_scaled_jtc_controller(self): self.assertTrue( diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 4a0b2ad1b..f1b970d82 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -52,7 +52,7 @@ IsProgramRunning, Load, ) -from ur_msgs.srv import SetIO +from ur_msgs.srv import SetIO, SetForceMode TIMEOUT_WAIT_SERVICE = 10 TIMEOUT_WAIT_SERVICE_INITIAL = 120 # If we download the docker image simultaneously to the tests, it can take quite some time until the dashboard server is reachable and usable. @@ -245,6 +245,15 @@ class IoStatusInterface( pass +class ForceModeInterface( + _ServiceInterface, + namespace="/force_mode_controller", + initial_services={}, + services={"start_force_mode": SetForceMode}, +): + pass + + def _declare_launch_arguments(): declared_arguments = [] From b454a76eb5abc6aff77557146e9ed1c8db45eacd Mon Sep 17 00:00:00 2001 From: URJala Date: Wed, 10 Jul 2024 09:05:25 +0000 Subject: [PATCH 8/8] Update controller to match updated service definition Also add example of using force mode controller --- ur_controllers/src/force_mode_controller.cpp | 12 +- ur_robot_driver/CMakeLists.txt | 2 + ur_robot_driver/examples/examples.py | 175 +++++++++++++++++++ ur_robot_driver/examples/force_mode.py | 136 ++++++++++++++ ur_robot_driver/test/robot_driver.py | 13 +- 5 files changed, 322 insertions(+), 16 deletions(-) create mode 100644 ur_robot_driver/examples/examples.py create mode 100644 ur_robot_driver/examples/force_mode.py diff --git a/ur_controllers/src/force_mode_controller.cpp b/ur_controllers/src/force_mode_controller.cpp index 1b9a03c1b..04f5b6702 100644 --- a/ur_controllers/src/force_mode_controller.cpp +++ b/ur_controllers/src/force_mode_controller.cpp @@ -217,12 +217,12 @@ bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request /* The limits specifies the maximum allowed speed along/around compliant axes. For non-compliant axes this value is * the maximum allowed deviation between actual tcp position and the one that has been programmed. */ - command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X].set_value(req->limits.twist.linear.x); - command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Y].set_value(req->limits.twist.linear.y); - command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Z].set_value(req->limits.twist.linear.z); - command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RX].set_value(req->limits.twist.angular.x); - command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RY].set_value(req->limits.twist.angular.y); - command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RZ].set_value(req->limits.twist.angular.z); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X].set_value(req->limits[0]); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Y].set_value(req->limits[1]); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Z].set_value(req->limits[2]); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RX].set_value(req->limits[3]); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RY].set_value(req->limits[4]); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RZ].set_value(req->limits[5]); /* The type decides how the robot interprets the force frame (the one defined in task_frame). See ur_script manual for * explanation, under force_mode. */ diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index c9e471180..c58b50204 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -149,6 +149,8 @@ install(PROGRAMS scripts/tool_communication.py scripts/wait_for_robot_description scripts/start_ursim.sh + examples/examples.py + examples/force_mode.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/ur_robot_driver/examples/examples.py b/ur_robot_driver/examples/examples.py new file mode 100644 index 000000000..59336cc6a --- /dev/null +++ b/ur_robot_driver/examples/examples.py @@ -0,0 +1,175 @@ +#!/usr/bin/env python3 +# Copyright 2024, 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. + +import rclpy +from builtin_interfaces.msg import Duration +from control_msgs.action import FollowJointTrajectory + +from rclpy.action import ActionClient +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from ur_msgs.srv import SetIO +from controller_manager_msgs.srv import SwitchController +from std_srvs.srv import Trigger + +TIMEOUT_WAIT_SERVICE = 10 +TIMEOUT_WAIT_SERVICE_INITIAL = 60 +TIMEOUT_WAIT_ACTION = 10 + +ROBOT_JOINTS = [ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", +] + + +# Helper functions +def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE): + client = node.create_client(srv_type, srv_name) + if client.wait_for_service(timeout) is False: + raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}") + + node.get_logger().info(f"Successfully connected to service '{srv_name}'") + return client + + +def waitForAction(node, action_name, action_type, timeout=TIMEOUT_WAIT_ACTION): + client = ActionClient(node, action_type, action_name) + if client.wait_for_server(timeout) is False: + raise Exception( + f"Could not reach action server '{action_name}' within timeout of {timeout}" + ) + + node.get_logger().info(f"Successfully connected to action '{action_name}'") + return client + + +class Robot: + def __init__(self, node): + self.node = node + self.service_interfaces = { + "/io_and_status_controller/set_io": SetIO, + "/dashboard_client/play": Trigger, + "/controller_manager/switch_controller": SwitchController, + } + self.init_robot() + + def init_robot(self): + self.service_clients = { + srv_name: waitForService(self.node, srv_name, srv_type) + for (srv_name, srv_type) in self.service_interfaces.items() + } + + self.jtc_action_client = waitForAction( + self.node, + "/scaled_joint_trajectory_controller/follow_joint_trajectory", + FollowJointTrajectory, + ) + + def set_io(self, pin, value): + """Test to set an IO.""" + set_io_req = SetIO.Request() + set_io_req.fun = 1 + set_io_req.pin = pin + set_io_req.state = value + + self.call_service("/io_and_status_controller/set_io", set_io_req) + + def send_trajectory(self, waypts, time_vec): + """Send robot trajectory.""" + if len(waypts) != len(time_vec): + raise Exception("waypoints vector and time vec should be same length") + + # Construct test trajectory + joint_trajectory = JointTrajectory() + joint_trajectory.joint_names = ROBOT_JOINTS + for i in range(len(waypts)): + point = JointTrajectoryPoint() + point.positions = waypts[i] + point.time_from_start = time_vec[i] + joint_trajectory.points.append(point) + + # Sending trajectory goal + goal_response = self.call_action( + self.jtc_action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory) + ) + if not goal_response.accepted: + raise Exception("trajectory was not accepted") + + # Verify execution + result = self.get_result(self.jtc_action_client, goal_response) + return result.error_code == FollowJointTrajectory.Result.SUCCESSFUL + + def call_service(self, srv_name, request): + future = self.service_clients[srv_name].call_async(request) + rclpy.spin_until_future_complete(self.node, future) + if future.result() is not None: + return future.result() + else: + raise Exception(f"Exception while calling service: {future.exception()}") + + def call_action(self, ac_client, g): + future = ac_client.send_goal_async(g) + rclpy.spin_until_future_complete(self.node, future) + + if future.result() is not None: + return future.result() + else: + raise Exception(f"Exception while calling action: {future.exception()}") + + def get_result(self, ac_client, goal_response): + future_res = ac_client._get_result_async(goal_response) + rclpy.spin_until_future_complete(self.node, future_res) + if future_res.result() is not None: + return future_res.result().result + else: + raise Exception(f"Exception while calling action: {future_res.exception()}") + + +if __name__ == "__main__": + rclpy.init() + node = Node("robot_driver_test") + robot = Robot(node) + + # The following list are arbitrary joint positions, change according to your own needs + waypts = [ + [-1.6006, -1.7272, -2.2030, -0.8079, 1.5951, -0.0311], + [-1.2, -1.4, -1.9, -1.2, 1.5951, -0.0311], + [-1.6006, -1.7272, -2.2030, -0.8079, 1.5951, -0.0311], + ] + time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)] + + # Execute trajectory on robot, make sure that the robot is booted and the control script is running + robot.send_trajectory(waypts, time_vec) + + # Set digital output 1 to true + robot.set_io(1, 1.0) diff --git a/ur_robot_driver/examples/force_mode.py b/ur_robot_driver/examples/force_mode.py new file mode 100644 index 000000000..9e65c020d --- /dev/null +++ b/ur_robot_driver/examples/force_mode.py @@ -0,0 +1,136 @@ +#!/usr/bin/env python3 +# Copyright 2024, 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. + +import time + +import rclpy +from rclpy.node import Node +from controller_manager_msgs.srv import SwitchController +from builtin_interfaces.msg import Duration +from std_msgs.msg import Header +from std_srvs.srv import Trigger + +from geometry_msgs.msg import ( + Point, + Quaternion, + Pose, + PoseStamped, + Wrench, + WrenchStamped, + Vector3, +) + +from ur_msgs.srv import SetForceMode + +from examples import Robot + +if __name__ == "__main__": + rclpy.init() + node = Node("robot_driver_test") + robot = Robot(node) + + # Activate force mode controller + robot.call_service( + "/controller_manager/switch_controller", + SwitchController.Request( + activate_controllers=["force_mode_controller", "scaled_joint_trajectory_controller"], + strictness=SwitchController.Request.BEST_EFFORT, + ), + ) + + # Add force mode service to service interfaces and re-init robot + robot.service_interfaces.update({"/force_mode_controller/start_force_mode": SetForceMode}) + robot.init_robot() + time.sleep(2) + # Press play on the robot + robot.call_service("/dashboard_client/play", Trigger.Request()) + + # Move robot in to position + robot.send_trajectory( + waypts=[[-1.5707, -1.5707, -1.5707, -1.5707, 1.5707, 0]], + time_vec=[Duration(sec=6, nanosec=0)], + ) + + # Finished moving + + # Create task frame for force mode + point = Point(x=0.0, y=0.0, z=0.0) + orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=0.0) + task_frame_pose = Pose() + task_frame_pose.position = point + task_frame_pose.orientation = orientation + header = Header(seq=1, frame_id="world") + header.stamp.sec = int(time.time()) + 1 + header.stamp.nanosec = 0 + frame_stamp = PoseStamped() + frame_stamp.header = header + frame_stamp.pose = task_frame_pose + + # Create compliance vector (which axes should be force controlled) + compliance = [False, False, True, False, False, False] + + # Create Wrench message for force mode + wrench_vec = Wrench(force=Vector3(x=0.0, y=0.0, z=-10.0), torque=Vector3(x=0.0, y=0.0, z=0.0)) + wrench_stamp = WrenchStamped(header=header, wrench=wrench_vec) + # Specify interpretation of task frame (no transform) + type_spec = SetForceMode.Request.NO_TRANSFORM + + # Specify max speeds and deviations of force mode + limits = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + + # specify damping and gain scaling + damping_factor = 0.025 + gain_scale = 0.5 + + req = SetForceMode.Request() + req.task_frame = frame_stamp + req.selection_vector_x = compliance[0] + req.selection_vector_y = compliance[1] + req.selection_vector_z = compliance[2] + req.selection_vector_rx = compliance[3] + req.selection_vector_ry = compliance[4] + req.selection_vector_rz = compliance[5] + req.wrench = wrench_stamp + req.type = type_spec + req.limits = limits + req.damping_factor = damping_factor + req.gain_scaling = gain_scale + + # Send request to controller + robot.call_service("/force_mode_controller/start_force_mode", req) + + time.sleep(15) + # Deactivate force mode controller + robot.call_service( + "/controller_manager/switch_controller", + SwitchController.Request( + deactivate_controllers=["force_mode_controller"], + strictness=SwitchController.Request.BEST_EFFORT, + ), + ) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index cdfb63431..a94619ece 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -48,9 +48,7 @@ Quaternion, Point, WrenchStamped, - TwistStamped, Wrench, - Twist, Vector3, ) from ur_msgs.msg import IOStates @@ -186,15 +184,10 @@ def test_force_mode_controller(self, tf_prefix): type_spec = 2 # Specify max speeds and deviations of force mode - limits = Twist() - limits.linear = Vector3(x=1.1, y=1.1, z=1.1) - limits.angular = limits.linear - limits_stamp = TwistStamped() - limits_stamp.header = header - limits_stamp.twist = limits + limits = [1.0, 1.0, 1.0, 0.5, 1.0, 1.0, 1.0] # specify damping and gain scaling - damping_factor = 0.8 + damping_factor = 0.1 gain_scale = 0.8 # Send request to controller @@ -208,7 +201,7 @@ def test_force_mode_controller(self, tf_prefix): selection_vector_rz=compliance[5], wrench=wrench_stamp, type=type_spec, - limits=limits_stamp, + limits=limits, damping_factor=damping_factor, gain_scaling=gain_scale, )