diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index f310a32e..79a89acb 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -71,6 +71,11 @@ generate_parameter_library( src/scaled_joint_trajectory_controller_parameters.yaml ) +generate_parameter_library( + freedrive_mode_controller_parameters + src/freedrive_mode_controller_parameters.yaml +) + generate_parameter_library( passthrough_trajectory_controller_parameters src/passthrough_trajectory_controller_parameters.yaml @@ -85,6 +90,7 @@ add_library(${PROJECT_NAME} SHARED src/force_mode_controller.cpp src/scaled_joint_trajectory_controller.cpp src/speed_scaling_state_broadcaster.cpp + src/freedrive_mode_controller.cpp src/gpio_controller.cpp src/passthrough_trajectory_controller.cpp src/ur_configuration_controller.cpp) @@ -97,6 +103,7 @@ target_link_libraries(${PROJECT_NAME} gpio_controller_parameters speed_scaling_state_broadcaster_parameters scaled_joint_trajectory_controller_parameters + freedrive_mode_controller_parameters passthrough_trajectory_controller_parameters ur_configuration_controller_parameters ) @@ -153,6 +160,16 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) + ament_add_gmock(test_load_freedrive_mode_controller + test/test_load_freedrive_mode_controller.cpp + ) + target_link_libraries(test_load_freedrive_mode_controller + ${PROJECT_NAME} + ) + ament_target_dependencies(test_load_freedrive_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 f3365a26..ec15809a 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -19,6 +19,11 @@ Controller to use UR's force_mode. + + + This controller handles the activation of the freedrive 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 38211d08..f03632d5 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -340,3 +340,41 @@ damping_factor 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. + +.. _freedrive_mode_controller: + +ur_controllers/FreedriveModeController +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller activates the robot's *Freedrive Mode*, allowing to manually move the robot' joints. +This controller can't be combined with any other motion controller. + +Parameters +"""""""""" + ++----------------------+--------+---------------+---------------------------------------------------------------------------------------+ +| Parameter name | Type | Default value | Description | +| | | | | ++----------------------+--------+---------------+---------------------------------------------------------------------------------------+ +| ``tf_prefix`` | string | | Urdf prefix of the corresponding arm | ++----------------------+--------+---------------+---------------------------------------------------------------------------------------+ +| ``inactive_timeout`` | int | 1 | Time interval (in seconds) of message inactivity after which freedrive is deactivated | ++----------------------+--------+---------------+---------------------------------------------------------------------------------------+ + +Usage +""""" + +The controller provides the ``~/enable_freedrive_mode`` topic of type ``[std_msgs/msg/Bool]`` for handling activation and deactivation: + +* to start and keep freedrive active, you'll have to frequently publish a ``True`` msg on the indicated topic. + If no further messages are received by the controller within the ``inactive_timeout`` seconds, + freedrive mode will be deactivated. Hence, it is recommended to publish a ``True`` message at least every + ``inactive_timeout/2`` seconds. + + .. code-block:: + + ros2 topic pub --rate 2 /freedrive_mode_controller/enable_freedrive_mode std_msgs/msg/Bool "{data: true}" + +* to deactivate freedrive mode is enough to publish a ``False`` msg on the indicated topic or + to deactivate the controller or to stop publishing ``True`` on the enable topic and wait for the + controller timeout. diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp new file mode 100644 index 00000000..04e90805 --- /dev/null +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -0,0 +1,130 @@ +// Copyright 2024, 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 Vincenzo Di Pentima dipentima@fzi.de + * \date 2024-09-26 + */ +//---------------------------------------------------------------------- +#ifndef UR_CONTROLLERS__FREEDRIVE_MODE_CONTROLLER_HPP_ +#define UR_CONTROLLERS__FREEDRIVE_MODE_CONTROLLER_HPP_ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include "std_msgs/msg/bool.hpp" + +#include "freedrive_mode_controller_parameters.hpp" + +namespace ur_controllers +{ +enum CommandInterfaces +{ + FREEDRIVE_MODE_ASYNC_SUCCESS = 0u, + FREEDRIVE_MODE_ENABLE = 1, + FREEDRIVE_MODE_ABORT = 2, +}; + +using namespace std::chrono_literals; // NOLINT + +class FreedriveModeController : public controller_interface::ControllerInterface +{ +public: + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + // Change the input for the update function + 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_cleanup(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_init() override; + +private: + // Command interfaces: optional is used only to avoid adding reference initialization + std::optional> async_success_command_interface_; + std::optional> enable_command_interface_; + std::optional> abort_command_interface_; + + std::shared_ptr> enable_freedrive_mode_sub_; + + rclcpp::TimerBase::SharedPtr freedrive_sub_timer_; ///< Timer to check for timeout on input + mutable std::chrono::seconds timeout_interval_; + void freedrive_cmd_callback(const std_msgs::msg::Bool::SharedPtr msg); + + std::shared_ptr freedrive_param_listener_; + freedrive_mode_controller::Params freedrive_params_; + + std::atomic freedrive_active_; + std::atomic change_requested_; + std::atomic async_state_; + std::atomic first_log_; + std::atomic timer_started_; + + void start_timer(); + void timeout_callback(); + + std::thread logging_thread_; + std::atomic logging_thread_running_; + std::atomic logging_requested_; + std::condition_variable logging_condition_; + std::mutex log_mutex_; + void log_task(); + void start_logging_thread(); + void stop_logging_thread(); + + 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 +#endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp new file mode 100644 index 00000000..e1ae016e --- /dev/null +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -0,0 +1,337 @@ + +// Copyright 2024, 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 Vincenzo Di Pentima dipentima@fzi.de + * \date 2024-09-16 + */ +//---------------------------------------------------------------------- +#include +#include +#include +#include +#include + +namespace ur_controllers +{ +controller_interface::CallbackReturn FreedriveModeController::on_init() +{ + try { + // Create the parameter listener and get the parameters + freedrive_param_listener_ = std::make_shared(get_node()); + freedrive_params_ = freedrive_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 FreedriveModeController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + const std::string tf_prefix = freedrive_params_.tf_prefix; + timeout_interval_ = std::chrono::seconds(freedrive_params_.inactive_timeout); + + // Get the command interfaces needed for freedrive mode from the hardware interface + config.names.emplace_back(tf_prefix + "freedrive_mode/async_success"); + config.names.emplace_back(tf_prefix + "freedrive_mode/enable"); + config.names.emplace_back(tf_prefix + "freedrive_mode/abort"); + + return config; +} + +controller_interface::InterfaceConfiguration +ur_controllers::FreedriveModeController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::NONE; + + return config; +} + +controller_interface::CallbackReturn +ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::State& previous_state) +{ + // Subscriber definition + enable_freedrive_mode_sub_ = get_node()->create_subscription( + "~/enable_freedrive_mode", 10, + std::bind(&FreedriveModeController::freedrive_cmd_callback, this, std::placeholders::_1)); + + timer_started_ = false; + + const auto logger = get_node()->get_logger(); + + if (!freedrive_param_listener_) { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during configuration"); + return controller_interface::CallbackReturn::ERROR; + } + + // Update the dynamic map parameters + freedrive_param_listener_->refresh_dynamic_parameters(); + + // Get parameters from the listener in case they were updated + freedrive_params_ = freedrive_param_listener_->get_params(); + + start_logging_thread(); + + return ControllerInterface::on_configure(previous_state); +} + +controller_interface::CallbackReturn +ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::State& state) +{ + change_requested_ = false; + freedrive_active_ = false; + async_state_ = std::numeric_limits::quiet_NaN(); + + first_log_ = false; + logging_thread_running_ = true; + logging_requested_ = false; + + { + const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode/" + "async_success"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + async_success_command_interface_ = *it; + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + + { + const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode/" + "enable"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + enable_command_interface_ = *it; + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + + { + const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode/" + "abort"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + abort_command_interface_ = *it; + abort_command_interface_->get().set_value(0.0); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + + return ControllerInterface::on_activate(state); +} + +controller_interface::CallbackReturn +ur_controllers::FreedriveModeController::on_cleanup(const rclcpp_lifecycle::State& /*previous_state*/) +{ + abort_command_interface_->get().set_value(1.0); + + stop_logging_thread(); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::State&) +{ + freedrive_active_ = false; + + freedrive_sub_timer_.reset(); + timer_started_ = false; + + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type ur_controllers::FreedriveModeController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) +{ + async_state_ = async_success_command_interface_->get().get_value(); + + if (change_requested_) { + if (freedrive_active_) { + // Check if the freedrive mode has been aborted from the hardware interface. E.g. the robot was stopped on the + // teach pendant. + if (!std::isnan(abort_command_interface_->get().get_value()) && + abort_command_interface_->get().get_value() == 1.0) { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode aborted by hardware, aborting request."); + freedrive_active_ = false; + return controller_interface::return_type::OK; + } else { + RCLCPP_INFO(get_node()->get_logger(), "Received command to start Freedrive Mode."); + + // Set command interface to enable + enable_command_interface_->get().set_value(1.0); + + async_success_command_interface_->get().set_value(ASYNC_WAITING); + async_state_ = ASYNC_WAITING; + } + + } else { + RCLCPP_INFO(get_node()->get_logger(), "Received command to stop Freedrive Mode."); + + abort_command_interface_->get().set_value(1.0); + + async_success_command_interface_->get().set_value(ASYNC_WAITING); + async_state_ = ASYNC_WAITING; + } + first_log_ = true; + change_requested_ = false; + } + + if ((async_state_ == 1.0) && (first_log_)) { + first_log_ = false; + logging_requested_ = true; + + // Notify logging thread + logging_condition_.notify_one(); + } + return controller_interface::return_type::OK; +} + +void FreedriveModeController::freedrive_cmd_callback(const std_msgs::msg::Bool::SharedPtr msg) +{ + // Process the freedrive_mode command. + if (get_node()->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + if (msg->data) { + if ((!freedrive_active_) && (!change_requested_)) { + freedrive_active_ = true; + change_requested_ = true; + start_timer(); + } + } else { + if ((freedrive_active_) && (!change_requested_)) { + freedrive_active_ = false; + change_requested_ = true; + } + } + } + + if (freedrive_sub_timer_) { + freedrive_sub_timer_->reset(); + } +} + +void FreedriveModeController::start_timer() +{ + if (!timer_started_) { + // Start the timer only after the first message is received + freedrive_sub_timer_ = + get_node()->create_wall_timer(timeout_interval_, std::bind(&FreedriveModeController::timeout_callback, this)); + timer_started_ = true; + + RCLCPP_INFO(get_node()->get_logger(), "Timer started after receiving first command."); + } +} + +void FreedriveModeController::timeout_callback() +{ + if (timer_started_ && freedrive_active_) { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode will be deactivated since no new message received."); + + freedrive_active_ = false; + change_requested_ = true; + } + + timer_started_ = false; +} + +void FreedriveModeController::start_logging_thread() +{ + if (!logging_thread_running_) { + logging_thread_running_ = true; + logging_thread_ = std::thread(&FreedriveModeController::log_task, this); + } +} + +void FreedriveModeController::stop_logging_thread() +{ + logging_thread_running_ = false; + if (logging_thread_.joinable()) { + logging_thread_.join(); + } +} + +void FreedriveModeController::log_task() +{ + while (logging_thread_running_) { + std::unique_lock lock(log_mutex_); + + auto condition = [this] { return !logging_thread_running_ || logging_requested_; }; + + // Wait for the condition + logging_condition_.wait(lock, condition); + + if (!logging_thread_running_) + break; + + if (freedrive_active_) { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been enabled successfully."); + } else { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully."); + } + + // Reset to log only once + logging_requested_ = false; + } +} + +bool FreedriveModeController::waitForAsyncCommand(std::function get_value) +{ + const auto maximum_retries = freedrive_params_.check_io_successful_retries; + int retries = 0; + while (get_value() == ASYNC_WAITING) { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + retries++; + + if (retries > maximum_retries) + return false; + } + return true; +} +} // namespace ur_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(ur_controllers::FreedriveModeController, controller_interface::ControllerInterface) diff --git a/ur_controllers/src/freedrive_mode_controller_parameters.yaml b/ur_controllers/src/freedrive_mode_controller_parameters.yaml new file mode 100644 index 00000000..e50b4226 --- /dev/null +++ b/ur_controllers/src/freedrive_mode_controller_parameters.yaml @@ -0,0 +1,17 @@ +--- +freedrive_mode_controller: + tf_prefix: { + type: string, + default_value: "", + description: "Urdf prefix of the corresponding arm" + } + inactive_timeout: { + type: int, + default_value: 1, + description: "Time interval (in seconds) of message inactivity after which freedrive is deactivated" + } + 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/freedrive_mode_controller_params.yaml b/ur_controllers/test/freedrive_mode_controller_params.yaml new file mode 100644 index 00000000..417d385c --- /dev/null +++ b/ur_controllers/test/freedrive_mode_controller_params.yaml @@ -0,0 +1,5 @@ +--- +freedrive_mode_controller: + ros__parameters: + tf_prefix: "" + inactive_timeout: 10 diff --git a/ur_controllers/test/test_load_freedrive_mode_controller.cpp b/ur_controllers/test/test_load_freedrive_mode_controller.cpp new file mode 100644 index 00000000..0fd65c41 --- /dev/null +++ b/ur_controllers/test/test_load_freedrive_mode_controller.cpp @@ -0,0 +1,61 @@ +// 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(TestLoadFreedriveModeController, load_controller) +{ + std::shared_ptr executor = std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique(ros2_control_test_assets::minimal_robot_urdf), executor, + "test_controller_manager"); + + const std::string test_file_path = std::string{ TEST_FILES_DIRECTORY } + "/freedrive_mode_controller_params.yaml"; + cm.set_parameter({ "test_freedrive_mode_controller.params_file", test_file_path }); + + cm.set_parameter({ "test_freedrive_mode_controller.type", "ur_controllers/FreedriveModeController" }); + + ASSERT_NE(cm.load_controller("test_freedrive_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/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index b93bfb14..5e996bd2 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -27,6 +27,9 @@ controller_manager: force_mode_controller: type: ur_controllers/ForceModeController + freedrive_mode_controller: + type: ur_controllers/FreedriveModeController + passthrough_trajectory_controller: type: ur_controllers/PassthroughTrajectoryController @@ -162,6 +165,10 @@ force_mode_controller: ros__parameters: tf_prefix: "$(var tf_prefix)" +freedrive_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 new file mode 100644 index 00000000..67c3a492 --- /dev/null +++ b/ur_robot_driver/doc/usage/controllers.rst @@ -0,0 +1,127 @@ +Controllers +=========== + +This help page describes the different controllers available for the ``ur_robot_driver``. This +should help users finding the right controller for their specific use case. + +Where are controllers defined? +------------------------------ + +Controllers are defined in the ``config/ur_controllers.yaml`` file. + +How do controllers get loaded and started? +------------------------------------------ + +As this driver uses `ros2_control `_ all controllers are managed by the +controller_manager. During startup, a default set of running controllers is loaded and started, +another set is loaded in stopped mode. Stopped controllers won't be usable right away, but they +need to be started individually. + +Controllers that are actually writing to some command interfaces (e.g. joint positions) will claim +those interfaces. Only one controller claiming a certain interface can be active at one point. + +Controllers can be switched either through the controller_manager's service calls, through the +`rqt_controller_manager `_ gui or through the ``ros2 control`` verb from the package ``ros-${ROS_DISTRO}-ros2controlcli`` package. + +For example, to switch from the default ``scaled_joint_trajectory_controller`` to the +``forward_position_controller`` you can call + +.. code-block:: console + + $ ros2 control switch_controllers --deactivate scaled_joint_trajectory_controller \ + --activate forward_position_controller + [INFO 2024-09-23 20:32:04.373] [_ros2cli_1207798]: waiting for service /controller_manager/switch_controller to become available... + Successfully switched controllers + +Read-only broadcasters +---------------------- + +These broadcasters are read-only. They read states from the robot and publish them on a ROS topic. +As they are read-only, they don't claim any resources and can be combined freely. By default, they +are all started and running. Those controllers do not require the robot to have the +external_control script running. + +joint_state_broadcaster +^^^^^^^^^^^^^^^^^^^^^^^ + +Type: `joint_state_broadcaster/JointStateBroadcaster `_ + +Publishes all joints' positions, velocities, and motor currents as ``sensor_msgs/JointState`` on the ``joint_states`` topic. + +.. note:: + + The effort field contains the currents reported by the joints and not the actual efforts in a + physical sense. + +speed_scaling_state_broadcaster +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Type: :ref:`ur_controllers/SpeedScalingStateBroadcaster ` + +This broadcaster publishes the current actual execution speed as reported by the robot. Values are +floating points between 0 and 1. + +force_torque_sensor_broadcaster +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Type: `force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster `_ + +Publishes the robot's wrench as reported from the controller. + +Commanding controllers +---------------------- + +The commanding controllers control the robot's motions. Those controllers can't be combined +arbitrarily, as they will claim hardware resources. Only one controller can claim one hardware +resource at a time. + +scaled_joint_trajectory_controller (Default motion controller) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Type: :ref:`ur_controllers/ScaledJointTrajectoryController ` + +Scaled version of the +`joint_trajectory_controller +`_. +It uses the robot's speed scaling information and thereby the safety compliance features, like pause on safeguard stop. In addition, it also makes it possible to adjust execution speed using the speed slider on the teach pendant or set the program in pause and restart it again. +See it's linked documentation for details. + +io_and_status_controller +^^^^^^^^^^^^^^^^^^^^^^^^ + +Type: :ref:`ur_controllers/GPIOController ` + +Allows setting I/O ports, controlling some UR-specific functionality and publishes status information about the robot. + +forward_velocity_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Type: `velocity_controllers/JointGroupVelocityController `_ + +Allows setting target joint positions directly. The robot tries to reach the target position as +fast as possible. The user is therefore responsible for sending commands that are achievable. This +controller is particularly useful when doing servoing such as ``moveit_servo``. + +forward_position_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Type: `position_controllers/JointGroupPositionController `_ + +Allows setting target joint velocities directly. The user is responsible for sending commands that +are achievable. This controller is particularly useful when doing servoing such as +``moveit_servo``. + +force_mode_controller +^^^^^^^^^^^^^^^^^^^^^ + +Type: :ref:`ur_controllers/ForceModeController ` + +Allows utilizing the robot's builtin *Force Mode*. + +freedrive_mode_controller +^^^^^^^^^^^^^^^^^^^^^^^^^ + +Type: :ref:`ur_controllers/FreedriveModeController ` + +Allows utilizing the robot's *Freedrive mode*, making it possible to manually move the robot's +joints. 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 1f665a1b..4d525e92 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -79,7 +79,8 @@ enum StoppingInterface STOP_POSITION, STOP_VELOCITY, STOP_PASSTHROUGH, - STOP_FORCE_MODE + STOP_FORCE_MODE, + STOP_FREEDRIVE, }; // We define our own quaternion to use it as a buffer, since we need to pass pointers to the state @@ -228,6 +229,13 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double get_robot_software_version_bugfix_; double get_robot_software_version_build_; + // Freedrive mode controller interface values + bool freedrive_activated_; + bool freedrive_mode_controller_running_; + double freedrive_mode_async_success_; + double freedrive_mode_enable_; + double freedrive_mode_abort_; + // Passthrough trajectory controller interface values double passthrough_trajectory_transfer_state_; double passthrough_trajectory_abort_; @@ -236,6 +244,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::vector6d_t passthrough_trajectory_velocities_; urcl::vector6d_t passthrough_trajectory_accelerations_; double passthrough_trajectory_time_from_start_; + // payload stuff urcl::vector3d_t payload_center_of_gravity_; double payload_mass_; @@ -293,8 +302,9 @@ 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"; + const std::string FORCE_MODE_GPIO = "force_mode"; + const std::string FREEDRIVE_MODE_GPIO = "freedrive_mode"; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/launch/ur10.launch.py b/ur_robot_driver/launch/ur10.launch.py index 31c4fc82..db4c7df9 100644 --- a/ur_robot_driver/launch/ur10.launch.py +++ b/ur_robot_driver/launch/ur10.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur10e.launch.py b/ur_robot_driver/launch/ur10e.launch.py index 7009abfd..c2c4dd92 100644 --- a/ur_robot_driver/launch/ur10e.launch.py +++ b/ur_robot_driver/launch/ur10e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur16e.launch.py b/ur_robot_driver/launch/ur16e.launch.py index 02a56fd5..1e90e038 100644 --- a/ur_robot_driver/launch/ur16e.launch.py +++ b/ur_robot_driver/launch/ur16e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur20.launch.py b/ur_robot_driver/launch/ur20.launch.py index 3f9ad3a9..aaf26af1 100644 --- a/ur_robot_driver/launch/ur20.launch.py +++ b/ur_robot_driver/launch/ur20.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur3.launch.py b/ur_robot_driver/launch/ur3.launch.py index feaa54c5..ce2b1ae3 100644 --- a/ur_robot_driver/launch/ur3.launch.py +++ b/ur_robot_driver/launch/ur3.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur30.launch.py b/ur_robot_driver/launch/ur30.launch.py index 3822f5e2..e5d38e58 100644 --- a/ur_robot_driver/launch/ur30.launch.py +++ b/ur_robot_driver/launch/ur30.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur3e.launch.py b/ur_robot_driver/launch/ur3e.launch.py index cbb3b2a5..8c23de13 100644 --- a/ur_robot_driver/launch/ur3e.launch.py +++ b/ur_robot_driver/launch/ur3e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur5.launch.py b/ur_robot_driver/launch/ur5.launch.py index 6281bbf9..bc157faa 100644 --- a/ur_robot_driver/launch/ur5.launch.py +++ b/ur_robot_driver/launch/ur5.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur5e.launch.py b/ur_robot_driver/launch/ur5e.launch.py index 372bf2ef..0fbdeb62 100644 --- a/ur_robot_driver/launch/ur5e.launch.py +++ b/ur_robot_driver/launch/ur5e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 02fd59a0..1d1ab86b 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -349,6 +349,7 @@ def controller_spawner(controllers, active=True): "forward_position_controller", "force_mode_controller", "passthrough_trajectory_controller", + "freedrive_mode_controller", ] if activate_joint_controller.perform(context) == "true": controllers_active.append(initial_joint_controller.perform(context)) @@ -500,6 +501,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], description="Initially loaded robot controller.", diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 9cb3350e..157310e9 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -83,6 +83,7 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; position_controller_running_ = false; velocity_controller_running_ = false; + freedrive_mode_controller_running_ = false; passthrough_trajectory_controller_running_ = false; runtime_state_ = static_cast(rtde::RUNTIME_STATE::STOPPED); pausing_state_ = PausingState::RUNNING; @@ -92,6 +93,7 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys initialized_ = false; async_thread_shutdown_ = false; system_interface_initialized_ = 0.0; + freedrive_mode_abort_ = 0.0; passthrough_trajectory_transfer_state_ = 0.0; passthrough_trajectory_abort_ = 0.0; trajectory_joint_positions_.clear(); @@ -365,6 +367,15 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "zero_ftsensor", "zero_ftsensor_async_success", &zero_ftsensor_async_success_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + FREEDRIVE_MODE_GPIO, "async_success", + &freedrive_mode_async_success_)); + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + FREEDRIVE_MODE_GPIO, "enable", &freedrive_mode_enable_)); + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + FREEDRIVE_MODE_GPIO, "abort", &freedrive_mode_abort_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "transfer_state", &passthrough_trajectory_transfer_state_)); @@ -716,6 +727,8 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: zero_ftsensor_cmd_ = NO_NEW_CMD_; hand_back_control_cmd_ = NO_NEW_CMD_; force_mode_disable_cmd_ = NO_NEW_CMD_; + freedrive_mode_abort_ = NO_NEW_CMD_; + freedrive_mode_enable_ = NO_NEW_CMD_; initialized_ = true; } @@ -744,6 +757,9 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: } else if (velocity_controller_running_) { ur_driver_->writeJointCommand(urcl_velocity_commands_, urcl::comm::ControlMode::MODE_SPEEDJ, receive_timeout_); + } else if (freedrive_mode_controller_running_ && freedrive_activated_) { + ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_NOOP); + } else if (passthrough_trajectory_controller_running_) { ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); check_passthrough_trajectory_controller(); @@ -859,6 +875,23 @@ void URPositionHardwareInterface::checkAsyncIO() zero_ftsensor_async_success_ = ur_driver_->zeroFTSensor(); zero_ftsensor_cmd_ = NO_NEW_CMD_; } + + if (!std::isnan(freedrive_mode_enable_) && ur_driver_ != nullptr) { + RCLCPP_INFO(rclcpp::get_logger("URPosistionHardwareInterface"), "Starting freedrive mode."); + freedrive_mode_async_success_ = + ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_START); + freedrive_mode_enable_ = NO_NEW_CMD_; + freedrive_activated_ = true; + } + + if (!std::isnan(freedrive_mode_abort_) && freedrive_mode_abort_ == 1.0 && freedrive_activated_ && + ur_driver_ != nullptr) { + RCLCPP_INFO(rclcpp::get_logger("URPosistionHardwareInterface"), "Stopping freedrive mode."); + freedrive_mode_async_success_ = + ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_STOP); + freedrive_activated_ = false; + freedrive_mode_abort_ = NO_NEW_CMD_; + } } void URPositionHardwareInterface::updateNonDoubleValues() @@ -943,6 +976,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (passthrough_trajectory_controller_running_) { control_modes[i].push_back(PASSTHROUGH_GPIO); } + if (freedrive_mode_controller_running_) { + control_modes[i].push_back(FREEDRIVE_MODE_GPIO); + } } if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(), @@ -994,6 +1030,14 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod return hardware_interface::return_type::ERROR; } start_modes_[i].push_back(PASSTHROUGH_GPIO); + } else if (key == tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success") { + 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 || + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO; + })) { + return hardware_interface::return_type::ERROR; + } + start_modes_[i].push_back(FREEDRIVE_MODE_GPIO); } } } @@ -1028,38 +1072,67 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod [&](const std::string& item) { return item == PASSTHROUGH_GPIO; }), control_modes[i].end()); } + if (key == tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success") { + stop_modes_[i].push_back(StoppingInterface::STOP_FREEDRIVE); + control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(), + [&](const std::string& item) { return item == FREEDRIVE_MODE_GPIO; }), + control_modes[i].end()); + } } } // Do not start conflicting controllers + // Passthrough controller requested to start if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), [this](auto& item) { return (item == PASSTHROUGH_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); + [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == FREEDRIVE_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); + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == FREEDRIVE_MODE_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start passthrough_trajectory " - "control while there is either position or " - "velocity mode is running."); + RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start passthrough_trajectory " + "control while there is either position or " + "velocity or freedrive mode running."); ret_val = hardware_interface::return_type::ERROR; } + // Force mode requested to start 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); + [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == FREEDRIVE_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 == FORCE_MODE_GPIO); + item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start force mode control while " - "there is either position or " - "velocity mode running."); + RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start force mode control while " + "there is either position or " + "velocity mode running."); + ret_val = hardware_interface::return_type::ERROR; + } + + // Freedrive mode requested to start + if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(), + [this](auto& item) { return (item == FREEDRIVE_MODE_GPIO); }) && + (std::any_of(start_modes_[0].begin(), start_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); + }) || + 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(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start force mode control while " + "there is either position or " + "velocity mode running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1069,15 +1142,16 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod (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); + item == FORCE_MODE_GPIO || item == FREEDRIVE_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); + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start position control while there " - "is either trajectory passthrough or " - "velocity mode or force_mode running."); + RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start position control while there " + "is either trajectory passthrough or " + "velocity mode or force_mode or freedrive mode " + "running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1087,15 +1161,16 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod (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); + item == FORCE_MODE_GPIO || item == FREEDRIVE_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); + item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO); }))) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Attempting to start velosity control while there " - "is either trajectory passthrough or " - "position mode or force_mode running."); + RCLCPP_ERROR(rclcpp::get_logger("URPosistionHardwareInterface"), "Attempting to start velocity control while there " + "is either trajectory passthrough or " + "position mode or force_mode or freedrive mode " + "running."); ret_val = hardware_interface::return_type::ERROR; } @@ -1127,6 +1202,11 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod trajectory_joint_positions_.clear(); trajectory_joint_accelerations_.clear(); trajectory_joint_velocities_.clear(); + } else if (stop_modes_.size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_FREEDRIVE) != stop_modes_[0].end()) { + freedrive_mode_controller_running_ = false; + freedrive_activated_ = false; + freedrive_mode_abort_ = 1.0; } if (start_modes_.size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), @@ -1151,6 +1231,12 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod position_controller_running_ = false; passthrough_trajectory_controller_running_ = true; passthrough_trajectory_abort_ = 0.0; + } else if (start_modes_[0].size() != 0 && + std::find(start_modes_[0].begin(), start_modes_[0].end(), FREEDRIVE_MODE_GPIO) != start_modes_[0].end()) { + velocity_controller_running_ = false; + position_controller_running_ = false; + freedrive_mode_controller_running_ = true; + freedrive_activated_ = false; } start_modes_.clear(); diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py index bdc9a0ec..a9356ffc 100644 --- a/ur_robot_driver/test/integration_test_controller_switch.py +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -96,6 +96,7 @@ def test_activating_multiple_controllers_same_interface_fails(self): "forward_velocity_controller", "passthrough_trajectory_controller", "force_mode_controller", + "freedrive_mode_controller", ], ).ok ) @@ -132,6 +133,7 @@ def test_activating_multiple_controllers_different_interface_fails(self): "forward_velocity_controller", "force_mode_controller", "passthrough_trajectory_controller", + "freedrive_mode_controller", ], ).ok ) @@ -180,6 +182,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", + "freedrive_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 @@ -195,6 +206,7 @@ def test_activating_controller_with_running_position_controller_fails(self): "forward_position_controller", "forward_velocity_controller", "force_mode_controller", + "freedrive_mode_controller", "passthrough_trajectory_controller", ], ).ok @@ -215,6 +227,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=[ + "freedrive_mode_controller", + ], + ).ok + ) self.assertFalse( self._controller_manager_interface.switch_controller( strictness=SwitchController.Request.STRICT, @@ -289,6 +309,14 @@ def test_activating_controller_with_running_passthrough_trajectory_controller_fa ], ).ok ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "freedrive_mode_controller", + ], + ).ok + ) # Stop the controller again self.assertTrue( self._controller_manager_interface.switch_controller( diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro new file mode 100644 index 00000000..91e73a88 --- /dev/null +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -0,0 +1,303 @@ + + + + + + + + + + + + + + + + + + + + mock_components/GenericSystem + ${mock_sensor_commands} + 0.0 + true + + + ur_robot_driver/URPositionHardwareInterface + ${robot_ip} + ${script_filename} + ${output_recipe_filename} + ${input_recipe_filename} + ${headless_mode} + ${reverse_port} + ${script_sender_port} + ${reverse_ip} + ${script_command_port} + ${trajectory_port} + ${tf_prefix} + ${non_blocking_read} + 2000 + 0.03 + ${use_tool_communication} + ${kinematics_hash} + ${tool_voltage} + ${tool_parity} + ${tool_baud_rate} + ${tool_stop_bits} + ${tool_rx_idle_chars} + ${tool_tx_idle_chars} + ${tool_device_name} + ${tool_tcp_port} + ${robot_receive_timeout} + + + + + + + + + + + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +