From e9499954614229f089a672b64cd63c1e2a49c328 Mon Sep 17 00:00:00 2001 From: URJala <159417921+URJala@users.noreply.github.com> Date: Thu, 28 Nov 2024 00:36:59 +1100 Subject: [PATCH] Add force mode controller (#1049) This enables using the robot's force_mode in a standalone controller. Force mode can be enabled and parametrized using a service call provided by the controller. --------- Co-authored-by: urmarp Co-authored-by: Felix Exner --- ur_controllers/CMakeLists.txt | 31 ++ ur_controllers/controller_plugins.xml | 5 + ur_controllers/doc/index.rst | 76 +++ .../ur_controllers/force_mode_controller.hpp | 147 +++++ ur_controllers/package.xml | 3 + ur_controllers/src/force_mode_controller.cpp | 380 +++++++++++++ .../src/force_mode_controller_parameters.yaml | 12 + .../test/force_mode_controller_params.yaml | 7 + .../test/test_load_force_mode_controller.cpp | 60 ++ ur_robot_driver/CMakeLists.txt | 8 + ur_robot_driver/config/ur_controllers.yaml | 8 + ur_robot_driver/doc/usage/controllers.rst | 7 + ur_robot_driver/examples/examples.py | 180 ++++++ ur_robot_driver/examples/force_mode.py | 142 +++++ .../ur_robot_driver/hardware_interface.hpp | 23 +- ur_robot_driver/launch/ur_control.launch.py | 1 + ur_robot_driver/src/hardware_interface.cpp | 260 +++++++-- .../integration_test_controller_switch.py | 112 +++- .../test/integration_test_force_mode.py | 524 ++++++++++++++++++ ur_robot_driver/test/robot_driver.py | 10 +- ur_robot_driver/test/test_common.py | 11 +- ur_robot_driver/urdf/ur.ros2_control.xacro | 32 ++ 22 files changed, 1961 insertions(+), 78 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 create mode 100644 ur_controllers/test/force_mode_controller_params.yaml create mode 100644 ur_controllers/test/test_load_force_mode_controller.cpp create mode 100644 ur_robot_driver/examples/examples.py create mode 100755 ur_robot_driver/examples/force_mode.py create mode 100644 ur_robot_driver/test/integration_test_force_mode.py diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index e94be4f0..f310a32e 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) @@ -27,6 +30,7 @@ find_package(action_msgs REQUIRED) set(THIS_PACKAGE_INCLUDE_DEPENDS angles controller_interface + geometry_msgs joint_trajectory_controller lifecycle_msgs pluginlib @@ -35,6 +39,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 @@ -45,6 +51,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 @@ -72,6 +82,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 @@ -82,6 +93,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 @@ -124,4 +136,23 @@ ament_export_libraries( ${PROJECT_NAME} ) +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_force_mode_controller + test/test_load_force_mode_controller.cpp + ) + target_link_libraries(test_load_force_mode_controller + ${PROJECT_NAME} + ) + ament_target_dependencies(test_load_force_mode_controller + controller_manager + ros2_control_test_assets + ) +endif() + ament_package() diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index c784b35d..f3365a26 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -14,6 +14,11 @@ This controller publishes the Tool IO. + + + Controller to use UR's force_mode. + + This controller forwards a joint-based trajectory to the robot controller for interpolation. diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index a488d07e..38211d08 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -264,3 +264,79 @@ Implementation details / dataflow * When the hardware reports that execution has been aborted (The ``passthrough_trajectory_abort`` command interface), the action will be aborted. * When the action is preempted, execution on the hardware is preempted. + +.. _force_mode_controller: + +ur_controllers/ForceModeController +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller activates the robot's *Force Mode*. This allows direct force control running on the +robot control box. This controller basically interfaces the URScript function ``force_mode(...)``. + +Force mode can be combined with (and only with) the :ref:`passthrough trajectory controller +` in order to execute motions under a given force constraints. + +.. note:: + This is not an admittance controller, as given force constraints in a certain Cartesian + dimension will overwrite the motion commands in that dimension. E.g. when specifying a certain + force in the base frame's ``z`` direction, any motion resulting from the move command in the + base frame's ``z`` axis will not be executed. + +Parameters +"""""""""" + ++----------------------------------+--------+---------------+---------------------------------------------------------------------+ +| Parameter name | Type | Default value | Description | +| | | | | ++----------------------------------+--------+---------------+---------------------------------------------------------------------+ +| ``tf_prefix`` | string | | Urdf prefix of the corresponding arm | ++----------------------------------+--------+---------------+---------------------------------------------------------------------+ +| ``check_io_successful_retries`` | int | 10 | Amount of retries for checking if setting force_mode was successful | ++----------------------------------+--------+---------------+---------------------------------------------------------------------+ + +Service interface / usage +""""""""""""""""""""""""" + +The controller provides two services: One for activating force_mode and one for leaving it. To use +those services, the controller has to be in ``active`` state. + +* ``~/stop_force_mode [std_srvs/srv/Trigger]``: Stop force mode +* ``~/start_force_mode [ur_msgs/srv/SetForceMode]``: Start force mode + +In ``ur_msgs/srv/SetForceMode`` the fields have the following meanings: + +task_frame + All information (selection vector, wrench, limits, etc) will be considered to be relative + to that pose. The pose's frame_id can be anything that is transformable to the robot's + ``base`` frame. +selection_vector_ + 1 means that the robot will be compliant in the corresponding axis of the task frame. +wrench + The forces/torques the robot will apply to its environment. The robot adjusts its position + along/about compliant axis in order to achieve the specified force/torque. Values have no effect for non- + compliant axes. + Actual wrench applied may be lower than requested due to joint safety limits. +type + An integer [1;3] specifying how the robot interprets the force frame + + 1 + The force frame is transformed in a way such that its y-axis is aligned with a vector pointing + from the robot tcp towards the origin of the force frame. + 2 + The force frame is not transformed. + 3 + The force frame is transformed in a way such that its x-axis is the projection of the robot tcp + velocity vector onto the x-y plane of the force frame. +speed_limits + Maximum allowed tcp speed (relative to the task frame). This is **only relevant for axes marked as + compliant** in the selection_vector. +deviation_limits + For **non-compliant axes**, these values are the maximum allowed deviation along/about an axis + between the actual tcp position and the one set by the program. +damping_factor + Force mode damping factor. Sets the damping parameter in force mode. In range [0;1], default value is 0.025 + 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. +gain_scaling + Force mode gain scaling factor. Scales the gain in force mode. scaling parameter is in range [0;2], default is 0.5. + A value larger than 1 can make force mode unstable, e.g. in case of collisions or pushing against hard surfaces. 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 00000000..9ae45a04 --- /dev/null +++ b/ur_controllers/include/ur_controllers/force_mode_controller.hpp @@ -0,0 +1,147 @@ +// 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 +#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, +}; + +struct ForceModeParameters +{ + std::array task_frame; + std::array selection_vec; + std::array limits; + geometry_msgs::msg::Wrench wrench; + double type; + double damping_factor; + double gain_scaling; +}; + +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; + + CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override; + +private: + 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); + rclcpp::Service::SharedPtr set_force_mode_srv_; + rclcpp::Service::SharedPtr disable_force_mode_srv_; + + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + + std::shared_ptr param_listener_; + force_mode_controller::Params params_; + + realtime_tools::RealtimeBuffer force_mode_params_buffer_; + std::atomic force_mode_active_; + std::atomic change_requested_; + std::atomic async_state_; + + 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 cae30341..c7f0e77b 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -23,6 +23,7 @@ angles controller_interface + geometry_msgs joint_trajectory_controller lifecycle_msgs pluginlib @@ -31,6 +32,8 @@ realtime_tools std_msgs std_srvs + tf2_geometry_msgs + tf2_ros ur_dashboard_msgs ur_msgs control_msgs diff --git a/ur_controllers/src/force_mode_controller.cpp b/ur_controllers/src/force_mode_controller.cpp new file mode 100644 index 00000000..f13d5b7f --- /dev/null +++ b/ur_controllers/src/force_mode_controller.cpp @@ -0,0 +1,380 @@ +// 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 +#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()); + + // 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"); + 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; + // Get the state interface indicating whether the hardware interface has been initialized + config.names.emplace_back(tf_prefix + "system_interface/initialized"); + + return config; +} + +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 configuration"); + 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_); + + // Create the service server that will be used to start force mode + try { + set_force_mode_srv_ = get_node()->create_service( + "~/start_force_mode", + std::bind(&ForceModeController::setForceMode, this, std::placeholders::_1, std::placeholders::_2)); + disable_force_mode_srv_ = get_node()->create_service( + "~/stop_force_mode", + std::bind(&ForceModeController::disableForceMode, this, std::placeholders::_1, std::placeholders::_2)); + } catch (...) { + return LifecycleNodeInterface::CallbackReturn::ERROR; + } + + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ur_controllers::ForceModeController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) +{ + change_requested_ = false; + force_mode_active_ = false; + async_state_ = std::numeric_limits::quiet_NaN(); + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ur_controllers::ForceModeController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) +{ + // Stop force mode if this controller is deactivated. + command_interfaces_[CommandInterfaces::FORCE_MODE_DISABLE_CMD].set_value(1.0); + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ur_controllers::ForceModeController::on_cleanup(const rclcpp_lifecycle::State& /*previous_state*/) +{ + set_force_mode_srv_.reset(); + disable_force_mode_srv_.reset(); + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type ur_controllers::ForceModeController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) +{ + async_state_ = command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value(); + + // Publish state of force_mode? + if (change_requested_) { + if (force_mode_active_) { + const auto force_mode_parameters = force_mode_params_buffer_.readFromRT(); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_X].set_value(force_mode_parameters->task_frame[0]); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Y].set_value(force_mode_parameters->task_frame[1]); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Z].set_value(force_mode_parameters->task_frame[2]); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RX].set_value(force_mode_parameters->task_frame[3]); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RY].set_value(force_mode_parameters->task_frame[4]); + command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RZ].set_value(force_mode_parameters->task_frame[5]); + + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_X].set_value( + force_mode_parameters->selection_vec[0]); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Y].set_value( + force_mode_parameters->selection_vec[1]); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Z].set_value( + force_mode_parameters->selection_vec[2]); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RX].set_value( + force_mode_parameters->selection_vec[3]); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RY].set_value( + force_mode_parameters->selection_vec[4]); + command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RZ].set_value( + force_mode_parameters->selection_vec[5]); + + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_X].set_value(force_mode_parameters->wrench.force.x); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Y].set_value(force_mode_parameters->wrench.force.y); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Z].set_value(force_mode_parameters->wrench.force.z); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RX].set_value(force_mode_parameters->wrench.torque.x); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RY].set_value(force_mode_parameters->wrench.torque.y); + command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RZ].set_value(force_mode_parameters->wrench.torque.z); + + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X].set_value(force_mode_parameters->limits[0]); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Y].set_value(force_mode_parameters->limits[1]); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Z].set_value(force_mode_parameters->limits[2]); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RX].set_value(force_mode_parameters->limits[3]); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RY].set_value(force_mode_parameters->limits[4]); + command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RZ].set_value(force_mode_parameters->limits[5]); + + command_interfaces_[CommandInterfaces::FORCE_MODE_TYPE].set_value(force_mode_parameters->type); + command_interfaces_[CommandInterfaces::FORCE_MODE_DAMPING].set_value(force_mode_parameters->damping_factor); + command_interfaces_[CommandInterfaces::FORCE_MODE_GAIN_SCALING].set_value(force_mode_parameters->gain_scaling); + + // Signal that we are waiting for confirmation that force mode is activated + command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING); + async_state_ = ASYNC_WAITING; + } else { + command_interfaces_[CommandInterfaces::FORCE_MODE_DISABLE_CMD].set_value(1.0); + command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING); + async_state_ = ASYNC_WAITING; + } + change_requested_ = false; + } + + return controller_interface::return_type::OK; +} + +bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req, + ur_msgs::srv::SetForceMode::Response::SharedPtr resp) +{ + // Reject if controller is not active + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new requests. Controller is not running."); + resp->success = false; + return false; + } + + ForceModeParameters force_mode_parameters; + + // transform task frame into base + const std::string tf_prefix = params_.tf_prefix; + if (std::abs(req->task_frame.pose.orientation.x) < 1e-6 && std::abs(req->task_frame.pose.orientation.y) < 1e-6 && + std::abs(req->task_frame.pose.orientation.z) < 1e-6 && std::abs(req->task_frame.pose.orientation.w) < 1e-6) { + RCLCPP_ERROR(get_node()->get_logger(), "Received task frame with all-zeros quaternion. It should have at least one " + "non-zero entry."); + resp->success = false; + return false; + } + + try { + auto task_frame_transformed = tf_buffer_->transform(req->task_frame, tf_prefix + "base"); + + force_mode_parameters.task_frame[0] = task_frame_transformed.pose.position.x; + force_mode_parameters.task_frame[1] = task_frame_transformed.pose.position.y; + force_mode_parameters.task_frame[2] = 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); + rot_mat.getRPY(force_mode_parameters.task_frame[3], force_mode_parameters.task_frame[4], + force_mode_parameters.task_frame[5]); + } 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()); + resp->success = false; + return false; + } + + // The selection vector dictates which axes the robot should be compliant along and around + force_mode_parameters.selection_vec[0] = req->selection_vector_x; + force_mode_parameters.selection_vec[1] = req->selection_vector_y; + force_mode_parameters.selection_vec[2] = req->selection_vector_z; + force_mode_parameters.selection_vec[3] = req->selection_vector_rx; + force_mode_parameters.selection_vec[4] = req->selection_vector_ry; + force_mode_parameters.selection_vec[5] = req->selection_vector_rz; + + // 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. + force_mode_parameters.wrench = req->wrench; + + /* 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. */ + force_mode_parameters.limits[0] = req->selection_vector_x ? req->speed_limits.linear.x : req->deviation_limits[0]; + force_mode_parameters.limits[1] = req->selection_vector_y ? req->speed_limits.linear.y : req->deviation_limits[1]; + force_mode_parameters.limits[2] = req->selection_vector_z ? req->speed_limits.linear.z : req->deviation_limits[2]; + force_mode_parameters.limits[3] = req->selection_vector_rx ? req->speed_limits.angular.x : req->deviation_limits[3]; + force_mode_parameters.limits[4] = req->selection_vector_ry ? req->speed_limits.angular.y : req->deviation_limits[4]; + force_mode_parameters.limits[5] = req->selection_vector_rz ? req->speed_limits.angular.z : req->deviation_limits[5]; + + if (req->type < 1 || req->type > 3) { + RCLCPP_ERROR(get_node()->get_logger(), "The force mode type has to be 1, 2, or 3. Received %u", req->type); + resp->success = false; + return false; + } + + /* 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. */ + force_mode_parameters.type = static_cast(req->type); + + /* The damping factor decides how fast the robot decelarates if no force is present. 0 means no deceleration, 1 + * means quick deceleration*/ + if (req->damping_factor < 0.0 || req->damping_factor > 1.0) { + RCLCPP_ERROR(get_node()->get_logger(), "The damping factor has to be between 0 and 1. Received %f", + req->damping_factor); + resp->success = false; + return false; + } + force_mode_parameters.damping_factor = req->damping_factor; + + /*The gain scaling factor scales the force mode gain. A value larger than 1 may make force mode unstable. */ + if (req->gain_scaling < 0.0 || req->gain_scaling > 2.0) { + RCLCPP_ERROR(get_node()->get_logger(), "The gain scaling has to be between 0 and 2. Received %f", + req->gain_scaling); + resp->success = false; + return false; + } + if (req->gain_scaling > 1.0) { + RCLCPP_WARN(get_node()->get_logger(), + "A gain_scaling >1.0 can make force mode unstable, e.g. in case of collisions or pushing against " + "hard surfaces. Received %f", + req->gain_scaling); + } + force_mode_parameters.gain_scaling = req->gain_scaling; + + force_mode_params_buffer_.writeFromNonRT(force_mode_parameters); + force_mode_active_ = true; + change_requested_ = true; + + 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 (async_state_ == ASYNC_WAITING || change_requested_) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + retries++; + + if (retries > maximum_retries) { + resp->success = false; + } + } + + resp->success = async_state_ == 1.0; + + 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(const std_srvs::srv::Trigger::Request::SharedPtr /*req*/, + std_srvs::srv::Trigger::Response::SharedPtr resp) +{ + force_mode_active_ = false; + change_requested_ = true; + RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for force mode to be disabled."); + while (async_state_ == ASYNC_WAITING || change_requested_) { + // Asynchronous wait until the hardware interface has set the force mode + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + resp->success = async_state_ == 1.0; + 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; +} +} // 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 00000000..4eb69efb --- /dev/null +++ b/ur_controllers/src/force_mode_controller_parameters.yaml @@ -0,0 +1,12 @@ +--- +force_mode_controller: + 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/test/force_mode_controller_params.yaml b/ur_controllers/test/force_mode_controller_params.yaml new file mode 100644 index 00000000..648b9195 --- /dev/null +++ b/ur_controllers/test/force_mode_controller_params.yaml @@ -0,0 +1,7 @@ +--- +force_mode_controller: + ros__parameters: + tf_prefix: "" + damping: 0.025 + gain_scaling: 0.5 + check_io_successful_retries: 10 diff --git a/ur_controllers/test/test_load_force_mode_controller.cpp b/ur_controllers/test/test_load_force_mode_controller.cpp new file mode 100644 index 00000000..8f25b96e --- /dev/null +++ b/ur_controllers/test/test_load_force_mode_controller.cpp @@ -0,0 +1,60 @@ +// 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. + +#include +#include "controller_manager/controller_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadForceModeController, load_controller) +{ + std::shared_ptr executor = std::make_shared(); + + controller_manager::ControllerManager cm{ executor, ros2_control_test_assets::minimal_robot_urdf, true, + "test_controller_manager" }; + + const std::string test_file_path = std::string{ TEST_FILES_DIRECTORY } + "/force_mode_controller_params.yaml"; + cm.set_parameter({ "test_force_mode_controller.params_file", test_file_path }); + + cm.set_parameter({ "test_force_mode_controller.type", "ur_controllers/ForceModeController" }); + + ASSERT_NE(cm.load_controller("test_force_mode_controller"), nullptr); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return result; +} diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 0d0487a0..05b59e62 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 @@ -148,6 +150,8 @@ install(PROGRAMS scripts/wait_for_robot_description scripts/example_move.py scripts/start_ursim.sh + examples/examples.py + examples/force_mode.py DESTINATION lib/${PROJECT_NAME} ) @@ -188,6 +192,10 @@ if(BUILD_TESTING) TIMEOUT 800 ) + add_launch_test(test/integration_test_force_mode.py + TIMEOUT + 800 + ) add_launch_test(test/urscript_interface.py TIMEOUT 500 diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 182d6eac..232e081e 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -24,8 +24,12 @@ controller_manager: forward_position_controller: type: position_controllers/JointGroupPositionController + force_mode_controller: + type: ur_controllers/ForceModeController + passthrough_trajectory_controller: type: ur_controllers/PassthroughTrajectoryController + tcp_pose_broadcaster: type: pose_broadcaster/PoseBroadcaster @@ -151,6 +155,10 @@ forward_position_controller: - $(var tf_prefix)wrist_2_joint - $(var tf_prefix)wrist_3_joint +force_mode_controller: + ros__parameters: + tf_prefix: "$(var tf_prefix)" + tcp_pose_broadcaster: ros__parameters: frame_id: $(var tf_prefix)base diff --git a/ur_robot_driver/doc/usage/controllers.rst b/ur_robot_driver/doc/usage/controllers.rst index d1566758..a35a8b08 100644 --- a/ur_robot_driver/doc/usage/controllers.rst +++ b/ur_robot_driver/doc/usage/controllers.rst @@ -110,3 +110,10 @@ Type: `position_controllers/JointGroupPositionController ` + +Allows utilizing the robot's builtin *Force Mode*. diff --git a/ur_robot_driver/examples/examples.py b/ur_robot_driver/examples/examples.py new file mode 100644 index 00000000..17b03a91 --- /dev/null +++ b/ur_robot_driver/examples/examples.py @@ -0,0 +1,180 @@ +#!/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, + ) + self.passthrough_trajectory_action_client = waitForAction( + self.node, + "/passthrough_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, action_client): + """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( + action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory) + ) + if not goal_response.accepted: + raise Exception("trajectory was not accepted") + + # Verify execution + result = self.get_result(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 100755 index 00000000..86ea05b7 --- /dev/null +++ b/ur_robot_driver/examples/force_mode.py @@ -0,0 +1,142 @@ +#!/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 geometry_msgs.msg import Twist +from std_msgs.msg import Header +from std_srvs.srv import Trigger + +from geometry_msgs.msg import ( + Point, + Quaternion, + Pose, + PoseStamped, + Wrench, + Vector3, +) + +from ur_msgs.srv import SetForceMode + +from examples import Robot + +if __name__ == "__main__": + rclpy.init() + node = Node("robot_driver_test") + robot = Robot(node) + + # Add force mode service to service interfaces and re-init robot + robot.service_interfaces.update({"/force_mode_controller/start_force_mode": SetForceMode}) + robot.service_interfaces.update({"/force_mode_controller/stop_force_mode": Trigger}) + robot.init_robot() + time.sleep(0.5) + # Press play on the robot + robot.call_service("/dashboard_client/play", Trigger.Request()) + + time.sleep(0.5) + # Start controllers + robot.call_service( + "/controller_manager/switch_controller", + SwitchController.Request( + deactivate_controllers=["scaled_joint_trajectory_controller"], + activate_controllers=["passthrough_trajectory_controller", "force_mode_controller"], + strictness=SwitchController.Request.BEST_EFFORT, + ), + ) + + # Move robot in to position + robot.send_trajectory( + waypts=[[-1.5707, -1.5707, -1.5707, -1.5707, 1.5707, 0]], + time_vec=[Duration(sec=5, nanosec=0)], + action_client=robot.passthrough_trajectory_action_client, + ) + + # 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=1.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=-20.0), torque=Vector3(x=0.0, y=0.0, z=0.0)) + # Specify interpretation of task frame (no transform) + type_spec = SetForceMode.Request.NO_TRANSFORM + + # Specify max speeds and deviations of force mode + speed_limits = Twist() + speed_limits.linear = Vector3(x=0.0, y=0.0, z=1.0) + speed_limits.angular = Vector3(x=0.0, y=0.0, z=1.0) + deviation_limits = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + + # specify damping and gain scaling + damping_factor = 0.1 + gain_scale = 0.8 + + 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_vec + req.type = type_spec + req.speed_limits = speed_limits + req.deviation_limits = deviation_limits + req.damping_factor = damping_factor + req.gain_scaling = gain_scale + + # Send request to controller + node.get_logger().info(f"Starting force mode with {req}") + robot.call_service("/force_mode_controller/start_force_mode", req) + robot.send_trajectory( + waypts=[[1.5707, -1.5707, -1.5707, -1.5707, 1.5707, 0]], + time_vec=[Duration(sec=5, nanosec=0)], + action_client=robot.passthrough_trajectory_action_client, + ) + + time.sleep(3) + node.get_logger().info("Deactivating force mode controller.") + robot.call_service("/force_mode_controller/stop_force_mode", Trigger.Request()) 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 42845611..1f665a1b 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -78,7 +78,8 @@ enum StoppingInterface NONE, STOP_POSITION, STOP_VELOCITY, - STOP_PASSTHROUGH + STOP_PASSTHROUGH, + STOP_FORCE_MODE }; // We define our own quaternion to use it as a buffer, since we need to pass pointers to the state @@ -158,6 +159,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface void updateNonDoubleValues(); void extractToolPose(); void transformForceTorque(); + void start_force_mode(); + void stop_force_mode(); void check_passthrough_trajectory_controller(); void trajectory_done_callback(urcl::control::TrajectoryResult result); bool has_accelerations(std::vector> accelerations); @@ -238,6 +241,18 @@ 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::vector6uint32_t force_mode_selection_vector_copy_; + 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_; @@ -265,10 +280,11 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double pausing_ramp_up_increment_; // resources switching aux vars - std::vector stop_modes_; - std::vector start_modes_; + std::vector> stop_modes_; + std::vector> start_modes_; bool position_controller_running_; bool velocity_controller_running_; + bool force_mode_controller_running_ = false; std::unique_ptr ur_driver_; std::shared_ptr async_thread_; @@ -277,6 +293,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20); + const std::string FORCE_MODE_GPIO = "force_mode"; const std::string PASSTHROUGH_GPIO = "trajectory_passthrough"; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 9e1ef380..8804cc4e 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -175,6 +175,7 @@ def controller_spawner(controllers, active=True): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "force_mode_controller", "passthrough_trajectory_controller", ] if activate_joint_controller.perform(context) == "true": diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 3fe6dbec..e06dc537 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -81,9 +81,6 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys urcl_position_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; urcl_position_commands_old_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; - stop_modes_ = { StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE, - StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE }; - start_modes_ = {}; position_controller_running_ = false; velocity_controller_running_ = false; passthrough_trajectory_controller_running_ = false; @@ -317,6 +314,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_GPIO, "task_frame_x", &force_mode_task_frame_[0]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "task_frame_y", &force_mode_task_frame_[1]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "task_frame_z", &force_mode_task_frame_[2]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "task_frame_rx", &force_mode_task_frame_[3]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "task_frame_ry", &force_mode_task_frame_[4]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "task_frame_rz", &force_mode_task_frame_[5]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_x", &force_mode_selection_vector_[0]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_y", &force_mode_selection_vector_[1]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_z", &force_mode_selection_vector_[2]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_rx", &force_mode_selection_vector_[3]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_ry", &force_mode_selection_vector_[4]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_rz", &force_mode_selection_vector_[5]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_x", &force_mode_wrench_[0]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_y", &force_mode_wrench_[1]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_z", &force_mode_wrench_[2]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_rx", &force_mode_wrench_[3]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_ry", &force_mode_wrench_[4]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_rz", &force_mode_wrench_[5]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "type", &force_mode_type_); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_x", &force_mode_limits_[0]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_y", &force_mode_limits_[1]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_z", &force_mode_limits_[2]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_rx", &force_mode_limits_[3]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_ry", &force_mode_limits_[4]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_rz", &force_mode_limits_[5]); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "force_mode_async_success", &force_mode_async_success_); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "disable_cmd", &force_mode_disable_cmd_); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "damping", &force_mode_damping_); + command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "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])); @@ -541,6 +568,14 @@ hardware_interface::CallbackReturn URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous_state) { RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Activating HW interface"); + + 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_); return hardware_interface::CallbackReturn::SUCCESS; } @@ -680,6 +715,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; } @@ -713,6 +749,14 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: check_passthrough_trajectory_controller(); } else { ur_driver_->writeKeepalive(); + + 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]) && + !std::isnan(force_mode_damping_) && !std::isnan(force_mode_gain_scaling_) && ur_driver_ != nullptr) { + start_force_mode(); + } else if (!std::isnan(force_mode_disable_cmd_) && ur_driver_ != nullptr && force_mode_async_success_ == 2.0) { + stop_force_mode(); + } } packet_read_ = false; @@ -880,26 +924,29 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod { hardware_interface::return_type ret_val = hardware_interface::return_type::OK; - start_modes_ = std::vector(info_.joints.size(), "UNDEFINED"); - stop_modes_.clear(); - std::vector control_modes(info_.joints.size()); + start_modes_ = std::vector>(info_.joints.size()); + stop_modes_ = std::vector>(info_.joints.size()); + std::vector> control_modes(info_.joints.size()); const std::string tf_prefix = info_.hardware_parameters.at("tf_prefix"); // Assess current state for (auto i = 0u; i < info_.joints.size(); i++) { if (position_controller_running_) { - control_modes[i] = hardware_interface::HW_IF_POSITION; + control_modes[i] = { hardware_interface::HW_IF_POSITION }; } if (velocity_controller_running_) { - control_modes[i] = hardware_interface::HW_IF_VELOCITY; + control_modes[i] = { hardware_interface::HW_IF_VELOCITY }; + } + if (force_mode_controller_running_) { + control_modes[i].push_back(FORCE_MODE_GPIO); } if (passthrough_trajectory_controller_running_) { - control_modes[i] = PASSTHROUGH_GPIO; + control_modes[i].push_back(PASSTHROUGH_GPIO); } } if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(), - [&](const std::string& other) { return other == start_modes_[0]; })) { + [&](const std::vector& other) { return other == start_modes_[0]; })) { RCLCPP_ERROR(get_logger(), "Start modes of all joints have to be the same."); return hardware_interface::return_type::ERROR; } @@ -910,20 +957,37 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod for (const auto& key : start_interfaces) { for (auto i = 0u; i < info_.joints.size(); i++) { if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { - if (start_modes_[i] != "UNDEFINED") { + if (!start_modes_[i].empty()) { + RCLCPP_ERROR(get_logger(), "Attempting to start position control while there is another control mode already " + "requested."); return hardware_interface::return_type::ERROR; } - start_modes_[i] = hardware_interface::HW_IF_POSITION; + start_modes_[i] = { hardware_interface::HW_IF_POSITION }; } else if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { - if (start_modes_[i] != "UNDEFINED") { + if (!start_modes_[i].empty()) { + RCLCPP_ERROR(get_logger(), "Attempting to start velocity control while there is another control mode already " + "requested."); return hardware_interface::return_type::ERROR; } - start_modes_[i] = hardware_interface::HW_IF_VELOCITY; + start_modes_[i] = { hardware_interface::HW_IF_VELOCITY }; + } else if (key == tf_prefix + FORCE_MODE_GPIO + "/type") { + if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) { + return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY; + })) { + RCLCPP_ERROR(get_logger(), "Attempting to start force_mode control while there is either position or " + "velocity mode already requested by another controller."); + return hardware_interface::return_type::ERROR; + } + start_modes_[i].push_back(FORCE_MODE_GPIO); } else if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) { - if (start_modes_[i] != "UNDEFINED") { + if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) { + return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY; + })) { + RCLCPP_ERROR(get_logger(), "Attempting to start trajectory passthrough control while there is either " + "position or velocity mode already requested by another controller."); return hardware_interface::return_type::ERROR; } - start_modes_[i] = PASSTHROUGH_GPIO; + start_modes_[i].push_back(PASSTHROUGH_GPIO); } } } @@ -933,56 +997,95 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod for (const auto& key : stop_interfaces) { for (auto i = 0u; i < info_.joints.size(); i++) { if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { - stop_modes_.push_back(StoppingInterface::STOP_POSITION); - if (control_modes[i] == hardware_interface::HW_IF_POSITION) { - control_modes[i] = "UNDEFINED"; - } + stop_modes_[i].push_back(StoppingInterface::STOP_POSITION); + control_modes[i].erase( + std::remove_if(control_modes[i].begin(), control_modes[i].end(), + [](const std::string& item) { return item == hardware_interface::HW_IF_POSITION; }), + control_modes[i].end()); } if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { - stop_modes_.push_back(StoppingInterface::STOP_VELOCITY); - if (control_modes[i] == hardware_interface::HW_IF_VELOCITY) { - control_modes[i] = "UNDEFINED"; - } + stop_modes_[i].push_back(StoppingInterface::STOP_VELOCITY); + control_modes[i].erase( + std::remove_if(control_modes[i].begin(), control_modes[i].end(), + [](const std::string& item) { return item == hardware_interface::HW_IF_VELOCITY; }), + control_modes[i].end()); + } + if (key == tf_prefix + FORCE_MODE_GPIO + "/disable_cmd") { + stop_modes_[i].push_back(StoppingInterface::STOP_FORCE_MODE); + control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(), + [&](const std::string& item) { return item == FORCE_MODE_GPIO; }), + control_modes[i].end()); } if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) { - stop_modes_.push_back(StoppingInterface::STOP_PASSTHROUGH); - if (control_modes[i] == PASSTHROUGH_GPIO) { - control_modes[i] = "UNDEFINED"; - } + stop_modes_[i].push_back(StoppingInterface::STOP_PASSTHROUGH); + control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(), + [&](const std::string& item) { return item == PASSTHROUGH_GPIO; }), + control_modes[i].end()); } } } // Do not start conflicting controllers - if (std::any_of(start_modes_.begin(), start_modes_.end(), + if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [this](auto& item) { return (item == PASSTHROUGH_GPIO); }) && - (std::any_of(start_modes_.begin(), start_modes_.end(), + (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + }) || + std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + }))) { + RCLCPP_ERROR(get_logger(), "Attempting to start passthrough_trajectory control while there is either position or " + "velocity mode is running."); + ret_val = hardware_interface::return_type::ERROR; + } + + if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [this](auto& item) { return (item == FORCE_MODE_GPIO); }) && + (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); }) || - std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) { + std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO); + item == FORCE_MODE_GPIO); }))) { + RCLCPP_ERROR(get_logger(), "Attempting to start force mode control while there is either position or " + "velocity mode running."); ret_val = hardware_interface::return_type::ERROR; } - if (std::any_of(start_modes_.begin(), start_modes_.end(), + + // Position mode requested to start + if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [](auto& item) { return (item == hardware_interface::HW_IF_POSITION); }) && - (std::any_of( - start_modes_.begin(), start_modes_.end(), - [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO); }) || - std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) { + (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO || + item == FORCE_MODE_GPIO); + }) || + std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO); + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO); }))) { + RCLCPP_ERROR(get_logger(), "Attempting to start position control while there is either trajectory passthrough or " + "velocity mode or force_mode running."); ret_val = hardware_interface::return_type::ERROR; } - if (std::any_of(start_modes_.begin(), start_modes_.end(), + + // Velocity mode requested to start + if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY); }) && - std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || - item == PASSTHROUGH_GPIO); - })) { + (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [this](auto& item) { + return (item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO || + item == FORCE_MODE_GPIO); + }) || + std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO); + }))) { + RCLCPP_ERROR(get_logger(), "Attempting to start velosity control while there is either trajectory passthrough or " + "position mode or force_mode running."); ret_val = hardware_interface::return_type::ERROR; } @@ -995,16 +1098,20 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod { hardware_interface::return_type ret_val = hardware_interface::return_type::OK; - if (stop_modes_.size() != 0 && - std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_POSITION) != stop_modes_.end()) { + if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_POSITION) != stop_modes_[0].end()) { position_controller_running_ = false; urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_; - } else if (stop_modes_.size() != 0 && - std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_VELOCITY) != stop_modes_.end()) { + } else if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_VELOCITY) != stop_modes_[0].end()) { velocity_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; - } else if (stop_modes_.size() != 0 && std::find(stop_modes_.begin(), stop_modes_.end(), - StoppingInterface::STOP_PASSTHROUGH) != stop_modes_.end()) { + } else if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_FORCE_MODE) != stop_modes_[0].end()) { + force_mode_controller_running_ = false; + stop_force_mode(); + } else if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_PASSTHROUGH) != stop_modes_[0].end()) { passthrough_trajectory_controller_running_ = false; passthrough_trajectory_abort_ = 1.0; trajectory_joint_positions_.clear(); @@ -1012,21 +1119,24 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod trajectory_joint_velocities_.clear(); } - if (start_modes_.size() != 0 && - std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_POSITION) != start_modes_.end()) { + if (start_modes_.size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), + hardware_interface::HW_IF_POSITION) != start_modes_[0].end()) { velocity_controller_running_ = false; passthrough_trajectory_controller_running_ = false; urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_; position_controller_running_ = true; - } else if (start_modes_.size() != 0 && std::find(start_modes_.begin(), start_modes_.end(), - hardware_interface::HW_IF_VELOCITY) != start_modes_.end()) { + } else if (start_modes_[0].size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), + hardware_interface::HW_IF_VELOCITY) != start_modes_[0].end()) { position_controller_running_ = false; passthrough_trajectory_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; velocity_controller_running_ = true; - } else if (start_modes_.size() != 0 && - std::find(start_modes_.begin(), start_modes_.end(), PASSTHROUGH_GPIO) != start_modes_.end()) { + } else if (start_modes_[0].size() != 0 && + std::find(start_modes_[0].begin(), start_modes_[0].end(), FORCE_MODE_GPIO) != start_modes_[0].end()) { + force_mode_controller_running_ = true; + } else if (start_modes_[0].size() != 0 && + std::find(start_modes_[0].begin(), start_modes_[0].end(), PASSTHROUGH_GPIO) != start_modes_[0].end()) { velocity_controller_running_ = false; position_controller_running_ = false; passthrough_trajectory_controller_running_ = true; @@ -1039,6 +1149,46 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod return ret_val; } +void URPositionHardwareInterface::start_force_mode() +{ + for (size_t i = 0; i < force_mode_selection_vector_.size(); i++) { + force_mode_selection_vector_copy_[i] = force_mode_selection_vector_[i]; + } + /* 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_copy_, 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_copy_, 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_); + force_mode_wrench_[i] = NO_NEW_CMD_; + 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_; +} + +void URPositionHardwareInterface::stop_force_mode() +{ + force_mode_async_success_ = ur_driver_->endForceMode(); + force_mode_disable_cmd_ = NO_NEW_CMD_; +} + void URPositionHardwareInterface::check_passthrough_trajectory_controller() { static double last_time = 0.0; diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py index 20ea6919..bdc9a0ec 100644 --- a/ur_robot_driver/test/integration_test_controller_switch.py +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -52,7 +52,8 @@ @pytest.mark.launch_test @launch_testing.parametrize( "tf_prefix", - [(""), ("my_ur_")], + [("")], + # [(""), ("my_ur_")], ) def generate_test_description(tf_prefix): return generate_driver_test_description(tf_prefix=tf_prefix) @@ -94,6 +95,7 @@ def test_activating_multiple_controllers_same_interface_fails(self): "forward_position_controller", "forward_velocity_controller", "passthrough_trajectory_controller", + "force_mode_controller", ], ).ok ) @@ -117,15 +119,6 @@ def test_activating_multiple_controllers_same_interface_fails(self): ], ).ok ) - self.assertFalse( - self._controller_manager_interface.switch_controller( - strictness=SwitchController.Request.STRICT, - activate_controllers=[ - "scaled_joint_trajectory_controller", - "forward_position_controller", - ], - ).ok - ) def test_activating_multiple_controllers_different_interface_fails(self): # Deactivate all writing controllers @@ -137,6 +130,7 @@ def test_activating_multiple_controllers_different_interface_fails(self): "joint_trajectory_controller", "forward_position_controller", "forward_velocity_controller", + "force_mode_controller", "passthrough_trajectory_controller", ], ).ok @@ -177,6 +171,15 @@ def test_activating_multiple_controllers_different_interface_fails(self): ], ).ok ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "force_mode_controller", + ], + ).ok + ) def test_activating_controller_with_running_position_controller_fails(self): # Having a position-based controller active, no other controller should be able to @@ -191,6 +194,7 @@ def test_activating_controller_with_running_position_controller_fails(self): "joint_trajectory_controller", "forward_position_controller", "forward_velocity_controller", + "force_mode_controller", "passthrough_trajectory_controller", ], ).ok @@ -219,6 +223,14 @@ def test_activating_controller_with_running_position_controller_fails(self): ], ).ok ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "force_mode_controller", + ], + ).ok + ) # Stop controller again self.assertTrue( self._controller_manager_interface.switch_controller( @@ -241,6 +253,7 @@ def test_activating_controller_with_running_passthrough_trajectory_controller_fa "joint_trajectory_controller", "forward_position_controller", "forward_velocity_controller", + "force_mode_controller", # tested in separate test ], ).ok ) @@ -285,3 +298,82 @@ def test_activating_controller_with_running_passthrough_trajectory_controller_fa ], ).ok ) + + def test_force_mode_and_trajectory_passthrough_controller_are_compatible(self): + # Deactivate all writing controllers + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + "passthrough_trajectory_controller", + "force_mode_controller", + ], + ).ok + ) + + time.sleep(3) + + # Start both together + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "passthrough_trajectory_controller", + "force_mode_controller", + ], + ).ok + ) + + # With passthrough traj controller running, start force mode controller + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "passthrough_trajectory_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + "force_mode_controller", + ], + ).ok + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "force_mode_controller", + ], + ).ok + ) + + # With start force mode controller running, passthrough traj controller + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "force_mode_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + "passthrough_trajectory_controller", + ], + ).ok + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "passthrough_trajectory_controller", + ], + ).ok + ) diff --git a/ur_robot_driver/test/integration_test_force_mode.py b/ur_robot_driver/test/integration_test_force_mode.py new file mode 100644 index 00000000..d7462f0c --- /dev/null +++ b/ur_robot_driver/test/integration_test_force_mode.py @@ -0,0 +1,524 @@ +#!/usr/bin/env python +# Copyright 2019, 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 os +import sys +import time +import unittest + +import pytest + +import launch_testing +import rclpy +from rclpy.node import Node + +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + +import std_msgs +from controller_manager_msgs.srv import SwitchController +from geometry_msgs.msg import ( + Pose, + PoseStamped, + Quaternion, + Point, + Twist, + Wrench, + Vector3, +) + +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ControllerManagerInterface, + DashboardInterface, + ForceModeInterface, + IoStatusInterface, + ConfigurationInterface, + generate_driver_test_description, +) + +TIMEOUT_EXECUTE_TRAJECTORY = 30 + + +@pytest.mark.launch_test +@launch_testing.parametrize( + "tf_prefix", + [(""), ("my_ur_")], +) +def generate_test_description(tf_prefix): + return generate_driver_test_description(tf_prefix=tf_prefix) + + +class RobotDriverTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context + rclpy.init() + cls.node = Node("robot_driver_test") + time.sleep(1) + cls.init_robot(cls) + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + cls.node.destroy_node() + rclpy.shutdown() + + 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._configuration_controller_interface = ConfigurationInterface(self.node) + + def setUp(self): + self._dashboard_interface.start_robot() + time.sleep(1) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self.node) + + def lookup_tcp_in_base(self, tf_prefix, timepoint): + trans = None + while not trans: + rclpy.spin_once(self.node) + try: + trans = self.tf_buffer.lookup_transform( + tf_prefix + "base", tf_prefix + "tool0", timepoint + ) + except TransformException: + pass + return trans + + def test_force_mode_controller(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "force_mode_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self._force_mode_controller_interface = ForceModeInterface(self.node) + + # Create task frame for force mode + point = Point(x=0.8, y=0.8, z=0.8) + orientation = Quaternion(x=0.7071, y=0.0, z=0.0, w=0.7071) + task_frame_pose = Pose() + task_frame_pose.position = point + task_frame_pose.orientation = orientation + header = std_msgs.msg.Header(seq=1, frame_id=tf_prefix + "base") + 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 = Wrench() + wrench.force = Vector3(x=0.0, y=0.0, z=5.0) + wrench.torque = Vector3(x=0.0, y=0.0, z=0.0) + + # Specify interpretation of task frame (no transform) + type_spec = 2 + + # Specify max speeds and deviations of force mode + speed_limits = Twist() + speed_limits.linear = Vector3(x=0.0, y=0.0, z=1.0) + speed_limits.angular = Vector3(x=0.0, y=0.0, z=1.0) + deviation_limits = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + + # specify damping and gain scaling + damping_factor = 0.1 + gain_scale = 0.8 + + trans_before = self.lookup_tcp_in_base(tf_prefix, rclpy.time.Time()) + + # 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, + type=type_spec, + speed_limits=speed_limits, + deviation_limits=deviation_limits, + damping_factor=damping_factor, + gain_scaling=gain_scale, + ) + self.assertTrue(res.success) + + time.sleep(5.0) + + trans_after = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now()) + + # task frame and wrench determines the expected motion + # In the example we used + # - a task frame rotated pi/2 deg around the base frame's x axis + # - a wrench with a positive z component for the force + # => we should expect a motion in negative y of the base frame + self.assertTrue(trans_after.transform.translation.y < trans_before.transform.translation.y) + self.assertAlmostEqual( + trans_after.transform.translation.x, + trans_before.transform.translation.x, + delta=0.001, + ) + self.assertAlmostEqual( + trans_after.transform.translation.z, + trans_before.transform.translation.z, + delta=0.001, + ) + self.assertAlmostEqual( + trans_after.transform.rotation.x, + trans_before.transform.rotation.x, + delta=0.01, + ) + self.assertAlmostEqual( + trans_after.transform.rotation.y, + trans_before.transform.rotation.y, + delta=0.01, + ) + self.assertAlmostEqual( + trans_after.transform.rotation.z, + trans_before.transform.rotation.z, + delta=0.01, + ) + self.assertAlmostEqual( + trans_after.transform.rotation.w, + trans_before.transform.rotation.w, + delta=0.01, + ) + + res = self._force_mode_controller_interface.stop_force_mode() + self.assertTrue(res.success) + + # Deactivate controller + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + deactivate_controllers=["force_mode_controller"], + ).ok + ) + + def test_illegal_force_mode_types(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "force_mode_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self._force_mode_controller_interface = ForceModeInterface(self.node) + + # 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=1.0) + task_frame_pose = Pose() + task_frame_pose.position = point + task_frame_pose.orientation = orientation + header = std_msgs.msg.Header(seq=1, frame_id=tf_prefix + "base") + header.stamp.sec = int(time.time()) + 1 + header.stamp.nanosec = 0 + frame_stamp = PoseStamped() + frame_stamp.header = header + frame_stamp.pose = task_frame_pose + + res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=0) + self.assertFalse(res.success) + res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=4) + self.assertFalse(res.success) + res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=1) + self.assertTrue(res.success) + res = self._force_mode_controller_interface.stop_force_mode() + res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=2) + self.assertTrue(res.success) + res = self._force_mode_controller_interface.stop_force_mode() + res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=3) + self.assertTrue(res.success) + res = self._force_mode_controller_interface.stop_force_mode() + + def test_illegal_task_frame(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "force_mode_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self._force_mode_controller_interface = ForceModeInterface(self.node) + + # 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=1.0) + task_frame_pose = Pose() + task_frame_pose.position = point + task_frame_pose.orientation = orientation + header = std_msgs.msg.Header(seq=1, frame_id=tf_prefix + "base") + header.stamp.sec = int(time.time()) + 1 + header.stamp.nanosec = 0 + frame_stamp = PoseStamped() + frame_stamp.header = header + frame_stamp.pose = task_frame_pose + + # Illegal frame name produces error + header.frame_id = "nonexisting6t54" + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, + ) + self.assertFalse(res.success) + header.frame_id = "base" + + # Illegal quaternion produces error + task_frame_pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=0.0) + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, + ) + self.assertFalse(res.success) + + def test_start_force_mode_on_inactive_controller_fails(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[], + deactivate_controllers=[ + "force_mode_controller", + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self._force_mode_controller_interface = ForceModeInterface(self.node) + + # 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=1.0) + task_frame_pose = Pose() + task_frame_pose.position = point + task_frame_pose.orientation = orientation + header = std_msgs.msg.Header(seq=1, frame_id=tf_prefix + "base") + header.stamp.sec = int(time.time()) + 1 + header.stamp.nanosec = 0 + frame_stamp = PoseStamped() + frame_stamp.header = header + frame_stamp.pose = task_frame_pose + + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, + ) + self.assertFalse(res.success) + + def test_deactivating_controller_stops_force_mode(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "force_mode_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self._force_mode_controller_interface = ForceModeInterface(self.node) + + # 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=1.0) + task_frame_pose = Pose() + task_frame_pose.position = point + task_frame_pose.orientation = orientation + header = std_msgs.msg.Header(seq=1, frame_id=tf_prefix + "base") + 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 = Wrench() + wrench.force = Vector3(x=0.0, y=0.0, z=5.0) + wrench.torque = Vector3(x=0.0, y=0.0, z=0.0) + + # Specify interpretation of task frame (no transform) + type_spec = 2 + + # Specify max speeds and deviations of force mode + speed_limits = Twist() + speed_limits.linear = Vector3(x=0.0, y=0.0, z=1.0) + speed_limits.angular = Vector3(x=0.0, y=0.0, z=1.0) + deviation_limits = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + + # specify damping and gain scaling + damping_factor = 0.1 + gain_scale = 0.8 + + 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, + type=type_spec, + speed_limits=speed_limits, + deviation_limits=deviation_limits, + damping_factor=damping_factor, + gain_scaling=gain_scale, + ) + self.assertTrue(res.success) + + time.sleep(0.5) + + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[], + deactivate_controllers=[ + "force_mode_controller", + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self._force_mode_controller_interface = ForceModeInterface(self.node) + + time.sleep(0.5) + trans_before_wait = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now()) + + # Make sure the robot didn't move anymore + time.sleep(0.5) + trans_after_wait = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now()) + + self.assertAlmostEqual( + trans_before_wait.transform.translation.z, trans_after_wait.transform.translation.z + ) + + def test_params_out_of_range_fails(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "force_mode_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self._force_mode_controller_interface = ForceModeInterface(self.node) + + # 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=1.0) + task_frame_pose = Pose() + task_frame_pose.position = point + task_frame_pose.orientation = orientation + header = std_msgs.msg.Header(seq=1, frame_id=tf_prefix + "base") + header.stamp.sec = int(time.time()) + 1 + header.stamp.nanosec = 0 + frame_stamp = PoseStamped() + frame_stamp.header = header + frame_stamp.pose = task_frame_pose + + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, gain_scaling=-0.1 + ) + self.assertFalse(res.success) + + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, gain_scaling=0.0 + ) + self.assertTrue(res.success) + res = self._force_mode_controller_interface.stop_force_mode() + self.assertTrue(res.success) + + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, gain_scaling=2.0 + ) + self.assertTrue(res.success) + res = self._force_mode_controller_interface.stop_force_mode() + self.assertTrue(res.success) + + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, gain_scaling=2.1 + ) + self.assertFalse(res.success) + + # damping factor + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, damping_factor=-0.1 + ) + self.assertFalse(res.success) + + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, damping_factor=0.0 + ) + self.assertTrue(res.success) + res = self._force_mode_controller_interface.stop_force_mode() + self.assertTrue(res.success) + + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, damping_factor=1.0 + ) + self.assertTrue(res.success) + res = self._force_mode_controller_interface.stop_force_mode() + self.assertTrue(res.success) + + res = self._force_mode_controller_interface.start_force_mode( + task_frame=frame_stamp, damping_factor=1.1 + ) + self.assertFalse(res.success) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 6350ea78..b0b2a4c8 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -52,19 +52,11 @@ IoStatusInterface, ConfigurationInterface, generate_driver_test_description, + ROBOT_JOINTS, ) TIMEOUT_EXECUTE_TRAJECTORY = 30 -ROBOT_JOINTS = [ - "shoulder_pan_joint", - "shoulder_lift_joint", - "elbow_joint", - "wrist_1_joint", - "wrist_2_joint", - "wrist_3_joint", -] - @pytest.mark.launch_test @launch_testing.parametrize( diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index a8a1a8ff..92754267 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -57,7 +57,7 @@ IsProgramRunning, Load, ) -from ur_msgs.srv import SetIO, GetRobotSoftwareVersion +from ur_msgs.srv import SetIO, GetRobotSoftwareVersion, 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. @@ -274,6 +274,15 @@ class ConfigurationInterface( pass +class ForceModeInterface( + _ServiceInterface, + namespace="/force_mode_controller", + initial_services={}, + services={"start_force_mode": SetForceMode, "stop_force_mode": Trigger}, +): + pass + + def _declare_launch_arguments(): declared_arguments = [] diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index d5df9b2b..35b0a8cb 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -229,6 +229,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +