diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index e94be4f0..a494a3bf 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -61,6 +61,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 @@ -74,6 +79,7 @@ generate_parameter_library( add_library(${PROJECT_NAME} SHARED 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) @@ -85,6 +91,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 ) diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index c784b35d..95556ec7 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -14,6 +14,11 @@ This controller publishes the Tool IO. + + + 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/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp new file mode 100644 index 00000000..ab547237 --- /dev/null +++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp @@ -0,0 +1,137 @@ +// 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 + */ +//---------------------------------------------------------------------- +#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 +#include "freedrive_mode_controller_parameters.hpp" + +namespace ur_controllers +{ +enum CommandInterfaces +{ + FREEDRIVE_MODE_ASYNC_SUCCESS = 25, + FREEDRIVE_MODE_CMD = 26, +}; +enum StateInterfaces +{ + INITIALIZED_FLAG = 0u, +}; + +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_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_init() override; + +private: + // Command interfaces + std::optional> async_success_command_interface_; + std::optional> enable_command_interface_; + std::optional> abort_command_interface_; + + // Everything related to the RT action server + using FreedriveModeAction = ur_msgs::action::EnableFreedriveMode; + using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; + using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + + RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. + rclcpp::TimerBase::SharedPtr goal_handle_timer_; ///< Timer to frequently check on the running goal + realtime_tools::RealtimeBuffer> joint_trajectory_mapping_; + rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); + + rclcpp_action::Server::SharedPtr freedrive_mode_action_server_; + rclcpp_action::GoalResponse + goal_received_callback(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + + rclcpp_action::CancelResponse goal_cancelled_callback( + const std::shared_ptr> goal_handle); + + void goal_accepted_callback( + std::shared_ptr> goal_handle); + + std::shared_ptr freedrive_param_listener_; + freedrive_mode_controller::Params freedrive_params_; + + /* Start an action server with an action called: /freedrive_mode_controller/start_freedrive_mode. */ + void start_action_server(void); + + std::atomic freedrive_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 +#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..0741ba33 --- /dev/null +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -0,0 +1,332 @@ + +// 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 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; + + // 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) +{ + + start_action_server(); + + 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(); + + return ControllerInterface::on_configure(previous_state); +} + +void FreedriveModeController::start_action_server(void) +{ + freedrive_mode_action_server_ = rclcpp_action::create_server( + get_node(), std::string(get_node()->get_name()) + "/freedrive_mode", + std::bind(&FreedriveModeController::goal_received_callback, this, std::placeholders::_1, + std::placeholders::_2), + std::bind(&FreedriveModeController::goal_cancelled_callback, this, std::placeholders::_1), + std::bind(&FreedriveModeController::goal_accepted_callback, this, std::placeholders::_1)); + return; +} + +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(); + + { + 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_deactivate(const rclcpp_lifecycle::State&) +{ + abort_command_interface_->get().set_value(1.0); + + const auto active_goal = *rt_active_goal_.readFromRT(); + if (active_goal) { + std::shared_ptr result = + std::make_shared(); + active_goal->setAborted(result); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type ur_controllers::FreedriveModeController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) +{ + const auto active_goal = *rt_active_goal_.readFromRT(); + 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 action."); + std::shared_ptr result = + std::make_shared(); + active_goal->setAborted(result); + freedrive_active_ = false; + return controller_interface::return_type::OK; + } else { + + // 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 { + abort_command_interface_->get().set_value(1.0); + + async_success_command_interface_->get().set_value(ASYNC_WAITING); + async_state_ = ASYNC_WAITING; + } + change_requested_ = false; + } + return controller_interface::return_type::OK; +} + +rclcpp_action::GoalResponse FreedriveModeController::goal_received_callback( + const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr goal) +{ + RCLCPP_INFO(get_node()->get_logger(), "Received new request for freedrive mode activation."); + + // Precondition: Running controller + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + RCLCPP_ERROR(get_node()->get_logger(), "Can't enable freedrive mode. Freedrive mode controller is not running."); + return rclcpp_action::GoalResponse::REJECT; + } + + if (freedrive_active_) { + RCLCPP_ERROR(get_node()->get_logger(), "Freedrive mode is already enabled: ignoring new request."); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse FreedriveModeController::goal_cancelled_callback( + const std::shared_ptr> goal_handle) +{ + bool success; + + // Check that cancel request refers to currently active goal (if any) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == goal_handle) { + RCLCPP_INFO(get_node()->get_logger(), "Disabling freedrive mode requested."); + + freedrive_active_ = false; + change_requested_ = true; + + RCLCPP_INFO(get_node()->get_logger(), "Waiting for the freedrive 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)); + } + + success = async_state_ == 1.0; + if (success) { + RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully."); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Could not disable freedrive mode."); + } + + // Mark the current goal as canceled + auto result = std::make_shared(); + active_goal->setCanceled(result); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + return rclcpp_action::CancelResponse::ACCEPT; +} + +void FreedriveModeController::goal_accepted_callback( + std::shared_ptr> goal_handle) +{ + RCLCPP_INFO_STREAM(get_node()->get_logger(), "Starting freedrive mode."); + + bool success; + + freedrive_active_ = true; + change_requested_ = true; + + RCLCPP_INFO(get_node()->get_logger(), "Waiting for freedrive mode to be set."); + const auto maximum_retries = freedrive_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) { + success = false; + } + } + + // Check if the change was successful + success = async_state_ == 1.0; + + if (success) { + RCLCPP_INFO_STREAM(get_node()->get_logger(), "Freedrive mode has been set successfully."); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Could not set the freedrive mode."); + } + + // Action handling will be done from the timer callback to avoid those things in the realtime + // thread. First, we delete the existing (if any) timer by resetting the pointer and then create a new + // one. + RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); + rt_goal->execute(); + rt_active_goal_.writeFromNonRT(rt_goal); + goal_handle_timer_.reset(); + goal_handle_timer_ = get_node()->create_wall_timer(action_monitor_period_.to_chrono(), + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); + return; +} + +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) \ No newline at end of file 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..cd58c0cf --- /dev/null +++ b/ur_controllers/src/freedrive_mode_controller_parameters.yaml @@ -0,0 +1,12 @@ +--- +freedrive_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" + } \ No newline at end of file diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 182d6eac..515941ca 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -24,6 +24,9 @@ controller_manager: forward_position_controller: type: position_controllers/JointGroupPositionController + freedrive_mode_controller: + type: ur_controllers/FreedriveModeController + passthrough_trajectory_controller: type: ur_controllers/PassthroughTrajectoryController tcp_pose_broadcaster: @@ -151,6 +154,10 @@ forward_position_controller: - $(var tf_prefix)wrist_2_joint - $(var tf_prefix)wrist_3_joint +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/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 42845611..2742c846 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -78,6 +78,7 @@ enum StoppingInterface NONE, STOP_POSITION, STOP_VELOCITY, + STOP_FREEDRIVE, STOP_PASSTHROUGH }; @@ -225,6 +226,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_action_requested_; + 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_; @@ -233,6 +241,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_; @@ -277,6 +286,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20); + // Check if name is correct here + const std::string FREEDRIVE_MODE = "freedrive_mode"; const std::string PASSTHROUGH_GPIO = "trajectory_passthrough"; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/launch/ur10.launch.py b/ur_robot_driver/launch/ur10.launch.py index 2bfde778..da829645 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 18c90922..366204c4 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 82850e5e..cb539050 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 547c9293..1d190723 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 38de8f5f..ea9cc44d 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 95478caa..89f373e2 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 78c145a8..d84b6acc 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 c70a6bc3..8c0be7ae 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 433f5e70..97518ea4 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 9e1ef380..3cbeef41 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -175,6 +175,11 @@ def controller_spawner(controllers, active=True): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "freedrive_mode_controller", + ] + + controller_spawners = [controller_spawner(controllers_active)] + [ + controller_spawner(controllers_inactive, active=False) "passthrough_trajectory_controller", ] if activate_joint_controller.perform(context) == "true": @@ -321,6 +326,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 3fe6dbec..fc70cea7 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -86,6 +86,7 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys start_modes_ = {}; 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; @@ -95,6 +96,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(); @@ -338,6 +340,14 @@ 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, "async_success", &freedrive_mode_async_success_)); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + tf_prefix + FREEDRIVE_MODE, "enable", &freedrive_mode_enable_)); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + tf_prefix + FREEDRIVE_MODE, "abort", &freedrive_mode_abort_)); command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "transfer_state", &passthrough_trajectory_transfer_state_)); @@ -680,6 +690,8 @@ 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_; + freedrive_mode_abort_ = NO_NEW_CMD_; + freedrive_mode_enable_ = NO_NEW_CMD_; initialized_ = true; } @@ -708,6 +720,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_action_requested_) { + 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(); @@ -815,56 +830,71 @@ 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(get_logger(), "Starting freedrive mode."); + freedrive_mode_async_success_ = ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_START); + freedrive_mode_enable_ = NO_NEW_CMD_; + freedrive_action_requested_ = true; + } + + if (!std::isnan(freedrive_mode_abort_) && freedrive_mode_abort_ == 1.0 && freedrive_action_requested_ && + ur_driver_ != nullptr) { + RCLCPP_INFO(get_logger(), "Stopping freedrive mode."); + freedrive_mode_async_success_ = ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_STOP); + freedrive_action_requested_ = false; + freedrive_mode_abort_ = NO_NEW_CMD_; +} } void URPositionHardwareInterface::updateNonDoubleValues() { - for (size_t i = 0; i < 18; ++i) { - actual_dig_out_bits_copy_[i] = static_cast(actual_dig_out_bits_[i]); - actual_dig_in_bits_copy_[i] = static_cast(actual_dig_in_bits_[i]); - } +for (size_t i = 0; i < 18; ++i) { + actual_dig_out_bits_copy_[i] = static_cast(actual_dig_out_bits_[i]); + actual_dig_in_bits_copy_[i] = static_cast(actual_dig_in_bits_[i]); +} - for (size_t i = 0; i < 11; ++i) { - safety_status_bits_copy_[i] = static_cast(safety_status_bits_[i]); - } +for (size_t i = 0; i < 11; ++i) { + safety_status_bits_copy_[i] = static_cast(safety_status_bits_[i]); +} - for (size_t i = 0; i < 4; ++i) { - analog_io_types_copy_[i] = static_cast(analog_io_types_[i]); - robot_status_bits_copy_[i] = static_cast(robot_status_bits_[i]); - } +for (size_t i = 0; i < 4; ++i) { + analog_io_types_copy_[i] = static_cast(analog_io_types_[i]); + robot_status_bits_copy_[i] = static_cast(robot_status_bits_[i]); +} - for (size_t i = 0; i < 2; ++i) { - tool_analog_input_types_copy_[i] = static_cast(tool_analog_input_types_[i]); - } +for (size_t i = 0; i < 2; ++i) { + tool_analog_input_types_copy_[i] = static_cast(tool_analog_input_types_[i]); +} - tool_output_voltage_copy_ = static_cast(tool_output_voltage_); - robot_mode_copy_ = static_cast(robot_mode_); - safety_mode_copy_ = static_cast(safety_mode_); - tool_mode_copy_ = static_cast(tool_mode_); - system_interface_initialized_ = initialized_ ? 1.0 : 0.0; - robot_program_running_copy_ = robot_program_running_ ? 1.0 : 0.0; +tool_output_voltage_copy_ = static_cast(tool_output_voltage_); +robot_mode_copy_ = static_cast(robot_mode_); +safety_mode_copy_ = static_cast(safety_mode_); +tool_mode_copy_ = static_cast(tool_mode_); +system_interface_initialized_ = initialized_ ? 1.0 : 0.0; +robot_program_running_copy_ = robot_program_running_ ? 1.0 : 0.0; } void URPositionHardwareInterface::transformForceTorque() { - // imported from ROS1 driver - hardware_interface.cpp#L867-L876 - tcp_force_.setValue(urcl_ft_sensor_measurements_[0], urcl_ft_sensor_measurements_[1], - urcl_ft_sensor_measurements_[2]); - tcp_torque_.setValue(urcl_ft_sensor_measurements_[3], urcl_ft_sensor_measurements_[4], - urcl_ft_sensor_measurements_[5]); +// imported from ROS1 driver - hardware_interface.cpp#L867-L876 +tcp_force_.setValue(urcl_ft_sensor_measurements_[0], urcl_ft_sensor_measurements_[1], + urcl_ft_sensor_measurements_[2]); +tcp_torque_.setValue(urcl_ft_sensor_measurements_[3], urcl_ft_sensor_measurements_[4], + urcl_ft_sensor_measurements_[5]); tcp_force_ = tf2::quatRotate(tcp_rotation_quat_.inverse(), tcp_force_); tcp_torque_ = tf2::quatRotate(tcp_rotation_quat_.inverse(), tcp_torque_); - urcl_ft_sensor_measurements_ = { tcp_force_.x(), tcp_force_.y(), tcp_force_.z(), - tcp_torque_.x(), tcp_torque_.y(), tcp_torque_.z() }; +urcl_ft_sensor_measurements_ = { tcp_force_.x(), tcp_force_.y(), tcp_force_.z(), + tcp_torque_.x(), tcp_torque_.y(), tcp_torque_.z() }; } void URPositionHardwareInterface::extractToolPose() { - // imported from ROS1 driver hardware_interface.cpp#L911-L928 - double tcp_angle = - std::sqrt(std::pow(urcl_tcp_pose_[3], 2) + std::pow(urcl_tcp_pose_[4], 2) + std::pow(urcl_tcp_pose_[5], 2)); +// imported from ROS1 driver hardware_interface.cpp#L911-L928 +double tcp_angle = + std::sqrt(std::pow(urcl_tcp_pose_[3], 2) + std::pow(urcl_tcp_pose_[4], 2) + std::pow(urcl_tcp_pose_[5], 2)); tf2::Vector3 rotation_vec(urcl_tcp_pose_[3], urcl_tcp_pose_[4], urcl_tcp_pose_[5]); if (tcp_angle > 1e-16) { @@ -876,7 +906,7 @@ void URPositionHardwareInterface::extractToolPose() } hardware_interface::return_type URPositionHardwareInterface::prepare_command_mode_switch( - const std::vector& start_interfaces, const std::vector& stop_interfaces) + const std::vector& start_interfaces, const std::vector& stop_interfaces) { hardware_interface::return_type ret_val = hardware_interface::return_type::OK; @@ -925,8 +955,53 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod } start_modes_[i] = PASSTHROUGH_GPIO; } + if (key == tf_prefix + FREEDRIVE_MODE + "/async_success") { + start_modes_.push_back(FREEDRIVE_MODE); + } } } + // set new mode to all interfaces at the same time + if (start_modes_.size() != 0 && start_modes_.size() != 6) { + ret_val = hardware_interface::return_type::ERROR; + } + + if (position_controller_running_ && + std::none_of(stop_modes_.begin(), stop_modes_.end(), + [](auto item) { return item == StoppingInterface::STOP_POSITION; }) && + std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == FREEDRIVE_MODE); + })) { + RCLCPP_ERROR(get_logger(), "Start of velocity interface or freedrive mode requested while there is the position " + "interface running."); + ret_val = hardware_interface::return_type::ERROR; + } + + if (velocity_controller_running_ && + std::none_of(stop_modes_.begin(), stop_modes_.end(), + [](auto item) { return item == StoppingInterface::STOP_VELOCITY; }) && + std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_POSITION || item == FREEDRIVE_MODE); + })) { + RCLCPP_ERROR(get_logger(), "Start of position interface or freedrive mode requested while there is the velocity " + "interface running."); + ret_val = hardware_interface::return_type::ERROR; + } + + if (freedrive_mode_controller_running_ && + std::none_of(stop_modes_.begin(), stop_modes_.end(), + [](auto item) { return item == StoppingInterface::STOP_FREEDRIVE; }) && + std::any_of(start_modes_.begin(), start_modes_.end(), [](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + })) { + RCLCPP_ERROR(get_logger(), "Start of position or velocity interface requested while the freedrive controller " + "is running."); + ret_val = hardware_interface::return_type::ERROR; + } + + // all start interfaces must be the same - can't mix position and velocity control + if (start_modes_.size() != 0 && !std::equal(start_modes_.begin() + 1, start_modes_.end(), start_modes_.begin())) { + ret_val = hardware_interface::return_type::ERROR; + } // Stopping interfaces // add stop interface per joint in tmp var for later check @@ -1003,6 +1078,11 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_VELOCITY) != stop_modes_.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_FREEDRIVE) != stop_modes_.end()) { + freedrive_mode_controller_running_ = false; + freedrive_action_requested_ = false; + freedrive_mode_abort_ = 1.0; } else if (stop_modes_.size() != 0 && std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_PASSTHROUGH) != stop_modes_.end()) { passthrough_trajectory_controller_running_ = false; @@ -1025,6 +1105,12 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod 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(), FREEDRIVE_MODE) != start_modes_.end()) { + velocity_controller_running_ = false; + position_controller_running_ = false; + freedrive_mode_controller_running_ = true; + freedrive_action_requested_ = false; } else if (start_modes_.size() != 0 && std::find(start_modes_.begin(), start_modes_.end(), PASSTHROUGH_GPIO) != start_modes_.end()) { velocity_controller_running_ = false; diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index d5df9b2b..8b77ffb5 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -225,6 +225,12 @@ + + + + + +