diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index a2b72e599..46ef4536c 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 @@ -38,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 @@ -55,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) @@ -63,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/package.xml b/ur_controllers/package.xml index 183a7486c..318961745 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -22,6 +22,7 @@ angles controller_interface + geometry_msgs joint_trajectory_controller lifecycle_msgs pluginlib @@ -30,6 +31,8 @@ realtime_tools std_msgs std_srvs + tf2_geometry_msgs + tf2_ros ur_dashboard_msgs ur_msgs diff --git a/ur_controllers/src/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 ca65375f4..9510cd304 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -35,9 +35,9 @@ */ //---------------------------------------------------------------------- -#include "ur_controllers/gpio_controller.hpp" +#include -#include +#include "ur_controllers/gpio_controller.hpp" namespace ur_controllers { @@ -48,7 +48,6 @@ controller_interface::CallbackReturn GPIOController::on_init() // Create the parameter listener and get the parameters param_listener_ = std::make_shared(get_node()); params_ = param_listener_->get_params(); - } catch (const std::exception& e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 89492f54f..b5486350f 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -32,6 +32,7 @@ find_package(std_srvs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(ur_client_library REQUIRED) find_package(ur_dashboard_msgs REQUIRED) +find_package(ur_msgs REQUIRED) include_directories(include) @@ -48,6 +49,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2_geometry_msgs ur_client_library ur_dashboard_msgs + ur_msgs ) add_library(ur_robot_driver_plugin diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 01eb555d4..63408e618 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -24,6 +24,9 @@ controller_manager: forward_position_controller: type: position_controllers/JointGroupPositionController + force_mode_controller: + type: ur_controllers/ForceModeController + speed_scaling_state_broadcaster: ros__parameters: @@ -124,3 +127,7 @@ forward_position_controller: - wrist_1_joint - wrist_2_joint - wrist_3_joint + +force_mode_controller: + ros__parameters: + tf_prefix: "" diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 63f762f09..0d0ab47dd 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -195,6 +195,17 @@ 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_; + 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_; std::array actual_dig_in_bits_copy_; diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 11c9d3482..ccd3701c7 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -214,6 +214,7 @@ def launch_setup(context, *args, **kwargs): executable="ur_ros2_control_node", parameters=[robot_description, update_rate_config_file, initial_joint_controllers], output="screen", + # prefix=["xterm -e gdb -ex run --args"], condition=UnlessCondition(use_fake_hardware), ) @@ -301,7 +302,7 @@ def controller_spawner(name, active=True): "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", ] - controller_spawner_inactive_names = ["forward_position_controller"] + controller_spawner_inactive_names = ["forward_position_controller", "force_mode_controller"] controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [ controller_spawner(name, active=False) for name in controller_spawner_inactive_names diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index ddbf8b91a..f93e661c3 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -279,6 +279,36 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back( hardware_interface::CommandInterface(tf_prefix + "payload", "payload_async_success", &payload_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_); + 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( tf_prefix + "gpio", "standard_digital_output_cmd_" + std::to_string(i), &standard_dig_out_bits_cmd_[i])); @@ -592,6 +622,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; } @@ -649,6 +680,14 @@ void URPositionHardwareInterface::initAsyncIO() payload_mass_ = NO_NEW_CMD_; payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ }; + + for (size_t i = 0; i < 6; i++) { + force_mode_task_frame_[i] = NO_NEW_CMD_; + force_mode_selection_vector_[i] = static_cast(NO_NEW_CMD_); + force_mode_wrench_[i] = NO_NEW_CMD_; + force_mode_limits_[i] = NO_NEW_CMD_; + } + force_mode_type_ = static_cast(NO_NEW_CMD_); } void URPositionHardwareInterface::checkAsyncIO() @@ -716,6 +755,28 @@ 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_); + } + + 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()