diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index 16f07953d..f88777abb 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -87,6 +87,11 @@ generate_parameter_library( src/passthrough_trajectory_controller_parameters.yaml ) +generate_parameter_library( + twist_controller_parameters + src/twist_controller_parameters.yaml +) + generate_parameter_library( ur_configuration_controller_parameters src/ur_configuration_controller_parameters.yaml @@ -100,6 +105,7 @@ add_library(${PROJECT_NAME} SHARED src/freedrive_mode_controller.cpp src/gpio_controller.cpp src/passthrough_trajectory_controller.cpp + src/twist_controller.cpp src/ur_configuration_controller.cpp) target_include_directories(${PROJECT_NAME} PRIVATE @@ -114,6 +120,7 @@ target_link_libraries(${PROJECT_NAME} freedrive_mode_controller_parameters passthrough_trajectory_controller_parameters ur_configuration_controller_parameters + twist_controller_parameters ${geometry_msgs_TARGETS} ${lifecycle_msgs_TARGETS} ${std_msgs_TARGETS} diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index f4c916aa3..ff9c3f6d6 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -39,4 +39,9 @@ Controller to use the tool contact functionality of the robot. + + + Controller to move a manipulator's TCP using Cartesian velocities. + + diff --git a/ur_controllers/include/ur_controllers/twist_controller.hpp b/ur_controllers/include/ur_controllers/twist_controller.hpp new file mode 100644 index 000000000..6c7b3a110 --- /dev/null +++ b/ur_controllers/include/ur_controllers/twist_controller.hpp @@ -0,0 +1,75 @@ +// Copyright 2025, 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. + +#ifndef UR_CONTROLLERS__TWIST_CONTROLLER_HPP_ +#define UR_CONTROLLERS__TWIST_CONTROLLER_HPP_ + +#include + +#include +#include +#include + +#include "ur_controllers/twist_controller_parameters.hpp" + +namespace ur_controllers +{ +class TwistController : public controller_interface::ControllerInterface +{ +public: + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; + + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_init() override; + +private: + void reset(); + + using TwistStamped = geometry_msgs::msg::TwistStamped; + + bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr twist_command_subscriber_ = nullptr; + + realtime_tools::RealtimeThreadSafeBox received_twist_msg_; + TwistStamped command_msg_; + + std::shared_ptr param_listener_; + twist_controller::Params controller_params_; +}; +} // namespace ur_controllers + +#endif // UR_CONTROLLERS__TWIST_CONTROLLER_HPP_ diff --git a/ur_controllers/src/twist_controller.cpp b/ur_controllers/src/twist_controller.cpp new file mode 100644 index 000000000..9937ec130 --- /dev/null +++ b/ur_controllers/src/twist_controller.cpp @@ -0,0 +1,155 @@ +// Copyright 2025, 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 + +namespace ur_controllers +{ +controller_interface::CallbackReturn TwistController::on_init() +{ + param_listener_ = std::make_shared(get_node()); + controller_params_ = param_listener_->get_params(); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn TwistController::on_configure(const rclcpp_lifecycle::State& /* previous_state */) +{ + twist_command_subscriber_ = get_node()->create_subscription( + "~/twist", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) -> void { + if (!subscriber_is_active_) { + RCLCPP_WARN(get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) { + RCLCPP_WARN_ONCE(get_node()->get_logger(), "Received TwistStamped with zero timestamp, setting it to current " + "time, this message will only be shown once"); + msg->header.stamp = get_node()->now(); + } + + if (msg->header.frame_id != controller_params_.tf_prefix + "base") { + // TODO(urfeex): Support transforming the twist? + RCLCPP_WARN(get_node()->get_logger(), + "Received TwistStamped with frame_id '%s'. Currently, it is only supported to command twists in " + "'%s'. The command will be ignored.", + msg->header.frame_id.c_str(), (controller_params_.tf_prefix + "base").c_str()); + return; + } + + // TODO(urfeex): Check timestamp. Do not accept old commands. + + received_twist_msg_.set(*msg); + }); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration TwistController::command_interface_configuration() const +{ + // No command interfaces currently + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + const std::string tf_prefix = controller_params_.tf_prefix; + config.names.push_back(tf_prefix + "twist/linear_velocity_x"); + config.names.push_back(tf_prefix + "twist/linear_velocity_y"); + config.names.push_back(tf_prefix + "twist/linear_velocity_z"); + config.names.push_back(tf_prefix + "twist/angular_velocity_x"); + config.names.push_back(tf_prefix + "twist/angular_velocity_y"); + config.names.push_back(tf_prefix + "twist/angular_velocity_z"); + + return config; +} + +controller_interface::InterfaceConfiguration TwistController::state_interface_configuration() const +{ + // TODO(urfeex): Add state interfaces for tcp speed + + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::NONE; + + return config; +} + +controller_interface::CallbackReturn TwistController::on_activate(const rclcpp_lifecycle::State& state) +{ + reset(); + subscriber_is_active_ = true; + + return ControllerInterface::on_activate(state); +} + +controller_interface::CallbackReturn TwistController::on_deactivate(const rclcpp_lifecycle::State&) +{ + subscriber_is_active_ = false; + reset(); + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type TwistController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) +{ + // In case we can't read from the subscriber's buffer, we will not update the command_msg_. + auto current_cmd = received_twist_msg_.try_get(); + if (current_cmd.has_value()) { + command_msg_ = current_cmd.value(); + } + + // TODO(urfeex): Check if the command_msg_ is valid, e.g., if the timestamp is not too old. + + bool success = true; + success &= command_interfaces_[0].set_value(command_msg_.twist.linear.x); + success &= command_interfaces_[1].set_value(command_msg_.twist.linear.y); + success &= command_interfaces_[2].set_value(command_msg_.twist.linear.z); + success &= command_interfaces_[3].set_value(command_msg_.twist.angular.x); + success &= command_interfaces_[4].set_value(command_msg_.twist.angular.y); + success &= command_interfaces_[5].set_value(command_msg_.twist.angular.z); + + if (!success) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to write to command interfaces."); + return controller_interface::return_type::ERROR; + } + + return controller_interface::return_type::OK; +} + +void TwistController::reset() +{ + command_msg_.twist.linear.x = 0.0; + command_msg_.twist.linear.y = 0.0; + command_msg_.twist.linear.z = 0.0; + command_msg_.twist.angular.x = 0.0; + command_msg_.twist.angular.y = 0.0; + command_msg_.twist.angular.z = 0.0; + received_twist_msg_.set(command_msg_); +} +} // namespace ur_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(ur_controllers::TwistController, controller_interface::ControllerInterface) diff --git a/ur_controllers/src/twist_controller_parameters.yaml b/ur_controllers/src/twist_controller_parameters.yaml new file mode 100644 index 000000000..554de2381 --- /dev/null +++ b/ur_controllers/src/twist_controller_parameters.yaml @@ -0,0 +1,7 @@ +--- +twist_controller: + tf_prefix: { + type: string, + default_value: "", + description: "Urdf prefix of the corresponding arm" + } diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index c845dd759..70f4719e0 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -45,6 +45,9 @@ controller_manager: tool_contact_controller: type: ur_controllers/ToolContactController + twist_controller: + type: ur_controllers/TwistController + # The way this is currently implemented upstream doesn't really work for us. When using # position control, the robot will have a tracking error. However, limits will be enforced # from the currently reported position, effectively limiting the possible step size using the @@ -66,6 +69,10 @@ ur_configuration_controller: ros__parameters: tf_prefix: "$(var tf_prefix)" +twist_controller: + ros__parameters: + tf_prefix: "$(var tf_prefix)" + force_torque_sensor_broadcaster: ros__parameters: sensor_name: $(var tf_prefix)tcp_fts_sensor 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 00da049f5..df581e10d 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -83,6 +83,7 @@ enum StoppingInterface STOP_FREEDRIVE, STOP_TOOL_CONTACT, STOP_TORQUE, + STOP_TWIST, }; // We define our own quaternion to use it as a buffer, since we need to pass pointers to the state @@ -179,6 +180,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::vector6d_t urcl_position_commands_old_; urcl::vector6d_t urcl_velocity_commands_; urcl::vector6d_t urcl_torque_commands_; + urcl::vector6d_t urcl_twist_commands_; urcl::vector6d_t urcl_joint_positions_; urcl::vector6d_t urcl_joint_velocities_; urcl::vector6d_t urcl_joint_efforts_; @@ -260,6 +262,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::vector6d_t passthrough_trajectory_accelerations_; double passthrough_trajectory_time_from_start_; + bool twist_controller_running_; + // payload stuff urcl::vector3d_t payload_center_of_gravity_; double payload_mass_; @@ -322,6 +326,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface const std::string FORCE_MODE_GPIO = "force_mode"; const std::string FREEDRIVE_MODE_GPIO = "freedrive_mode"; const std::string TOOL_CONTACT_GPIO = "tool_contact"; + const std::string TWIST_GPIO = "twist"; std::unordered_map> mode_compatibility_; }; diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 0613fddac..dcf899cfc 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -205,6 +205,7 @@ def controller_spawner(controllers, active=True): "passthrough_trajectory_controller", "freedrive_mode_controller", "tool_contact_controller", + "twist_controller", ] if activate_joint_controller.perform(context) == "true": controllers_active.append(initial_joint_controller.perform(context)) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index d8f1c7205..6c01276f4 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -65,6 +65,7 @@ URPositionHardwareInterface::URPositionHardwareInterface() mode_compatibility_[hardware_interface::HW_IF_POSITION][PASSTHROUGH_GPIO] = false; mode_compatibility_[hardware_interface::HW_IF_POSITION][FREEDRIVE_MODE_GPIO] = false; mode_compatibility_[hardware_interface::HW_IF_POSITION][TOOL_CONTACT_GPIO] = true; + mode_compatibility_[hardware_interface::HW_IF_POSITION][TWIST_GPIO] = false; mode_compatibility_[hardware_interface::HW_IF_VELOCITY][hardware_interface::HW_IF_POSITION] = false; mode_compatibility_[hardware_interface::HW_IF_VELOCITY][hardware_interface::HW_IF_EFFORT] = false; @@ -72,6 +73,7 @@ URPositionHardwareInterface::URPositionHardwareInterface() mode_compatibility_[hardware_interface::HW_IF_VELOCITY][PASSTHROUGH_GPIO] = false; mode_compatibility_[hardware_interface::HW_IF_VELOCITY][FREEDRIVE_MODE_GPIO] = false; mode_compatibility_[hardware_interface::HW_IF_VELOCITY][TOOL_CONTACT_GPIO] = true; + mode_compatibility_[hardware_interface::HW_IF_VELOCITY][TWIST_GPIO] = true; mode_compatibility_[hardware_interface::HW_IF_EFFORT][hardware_interface::HW_IF_POSITION] = false; mode_compatibility_[hardware_interface::HW_IF_EFFORT][hardware_interface::HW_IF_VELOCITY] = false; @@ -86,6 +88,7 @@ URPositionHardwareInterface::URPositionHardwareInterface() mode_compatibility_[FORCE_MODE_GPIO][PASSTHROUGH_GPIO] = true; mode_compatibility_[FORCE_MODE_GPIO][FREEDRIVE_MODE_GPIO] = false; mode_compatibility_[FORCE_MODE_GPIO][TOOL_CONTACT_GPIO] = false; + mode_compatibility_[FORCE_MODE_GPIO][TWIST_GPIO] = false; mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_POSITION] = false; mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_VELOCITY] = false; @@ -93,6 +96,7 @@ URPositionHardwareInterface::URPositionHardwareInterface() mode_compatibility_[PASSTHROUGH_GPIO][FORCE_MODE_GPIO] = true; mode_compatibility_[PASSTHROUGH_GPIO][FREEDRIVE_MODE_GPIO] = false; mode_compatibility_[PASSTHROUGH_GPIO][TOOL_CONTACT_GPIO] = true; + mode_compatibility_[PASSTHROUGH_GPIO][TWIST_GPIO] = false; mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false; mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false; @@ -100,6 +104,7 @@ URPositionHardwareInterface::URPositionHardwareInterface() mode_compatibility_[FREEDRIVE_MODE_GPIO][FORCE_MODE_GPIO] = false; mode_compatibility_[FREEDRIVE_MODE_GPIO][PASSTHROUGH_GPIO] = false; mode_compatibility_[FREEDRIVE_MODE_GPIO][TOOL_CONTACT_GPIO] = false; + mode_compatibility_[FREEDRIVE_MODE_GPIO][TWIST_GPIO] = false; mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_POSITION] = true; mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_VELOCITY] = true; @@ -107,6 +112,14 @@ URPositionHardwareInterface::URPositionHardwareInterface() mode_compatibility_[TOOL_CONTACT_GPIO][FORCE_MODE_GPIO] = false; mode_compatibility_[TOOL_CONTACT_GPIO][PASSTHROUGH_GPIO] = true; mode_compatibility_[TOOL_CONTACT_GPIO][FREEDRIVE_MODE_GPIO] = false; + mode_compatibility_[TOOL_CONTACT_GPIO][TWIST_GPIO] = true; + + mode_compatibility_[TWIST_GPIO][hardware_interface::HW_IF_POSITION] = false; + mode_compatibility_[TWIST_GPIO][hardware_interface::HW_IF_VELOCITY] = false; + mode_compatibility_[TWIST_GPIO][FORCE_MODE_GPIO] = false; + mode_compatibility_[TWIST_GPIO][PASSTHROUGH_GPIO] = false; + mode_compatibility_[TWIST_GPIO][FREEDRIVE_MODE_GPIO] = false; + mode_compatibility_[TWIST_GPIO][TOOL_CONTACT_GPIO] = true; } URPositionHardwareInterface::~URPositionHardwareInterface() @@ -131,12 +144,14 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareComponent urcl_position_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; urcl_position_commands_old_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; + urcl_twist_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; position_controller_running_ = false; velocity_controller_running_ = false; torque_controller_running_ = false; freedrive_mode_controller_running_ = false; passthrough_trajectory_controller_running_ = false; tool_contact_controller_running_ = false; + twist_controller_running_ = false; runtime_state_ = static_cast(rtde::RUNTIME_STATE::STOPPED); pausing_state_ = PausingState::RUNNING; pausing_ramp_up_increment_ = 0.01; @@ -482,6 +497,19 @@ std::vector URPositionHardwareInterface::e &passthrough_trajectory_accelerations_[i])); } + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + TWIST_GPIO, "linear_velocity_x", &urcl_twist_commands_[0])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + TWIST_GPIO, "linear_velocity_y", &urcl_twist_commands_[1])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + TWIST_GPIO, "linear_velocity_z", &urcl_twist_commands_[2])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + TWIST_GPIO, "angular_velocity_x", &urcl_twist_commands_[3])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + TWIST_GPIO, "angular_velocity_y", &urcl_twist_commands_[4])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + TWIST_GPIO, "angular_velocity_z", &urcl_twist_commands_[5])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + TOOL_CONTACT_GPIO, "tool_contact_set_state", &tool_contact_set_state_)); @@ -884,6 +912,8 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, 0, urcl::RobotReceiveTimeout::millisec(1000 * 5.0 / static_cast(info_.rw_rate))); check_passthrough_trajectory_controller(); + } else if (twist_controller_running_) { + ur_driver_->writeJointCommand(urcl_twist_commands_, urcl::comm::ControlMode::MODE_SPEEDL, receive_timeout_); } else { ur_driver_->writeKeepalive(); } @@ -1170,6 +1200,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (tool_contact_controller_running_) { control_modes[i].push_back(TOOL_CONTACT_GPIO); } + if (twist_controller_running_) { + control_modes[i].push_back(TWIST_GPIO); + } } auto is_mode_compatible = [this](const std::string& mode, const std::vector& other_modes) { @@ -1197,7 +1230,8 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod { tf_prefix + FORCE_MODE_GPIO + "/type", FORCE_MODE_GPIO }, { tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i), PASSTHROUGH_GPIO }, { tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success", FREEDRIVE_MODE_GPIO }, - { tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state", TOOL_CONTACT_GPIO } + { tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state", TOOL_CONTACT_GPIO }, + { tf_prefix + TWIST_GPIO + "/linear_velocity_x", TWIST_GPIO } }; for (auto& item : start_modes_to_check) { @@ -1247,7 +1281,8 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod StoppingInterface::STOP_PASSTHROUGH }, { tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success", FREEDRIVE_MODE_GPIO, StoppingInterface::STOP_FREEDRIVE }, { tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state", TOOL_CONTACT_GPIO, - StoppingInterface::STOP_TOOL_CONTACT } + StoppingInterface::STOP_TOOL_CONTACT }, + { tf_prefix + TWIST_GPIO + "/linear_velocity_x", TWIST_GPIO, StoppingInterface::STOP_TWIST } }; for (auto& item : stop_modes_to_check) { if (key == std::get<0>(item)) { @@ -1312,6 +1347,7 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod freedrive_activated_ = false; freedrive_mode_abort_ = 1.0; } + if (stop_modes_.size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), StoppingInterface::STOP_TOOL_CONTACT) != stop_modes_[0].end()) { tool_contact_controller_running_ = false; @@ -1319,6 +1355,12 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod ur_driver_->endToolContact(); } + if (stop_modes_.size() != 0 && + std::find(stop_modes_[0].begin(), stop_modes_[0].end(), StoppingInterface::STOP_TWIST) != stop_modes_[0].end()) { + twist_controller_running_ = false; + urcl_twist_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; + } + if (start_modes_.size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), hardware_interface::HW_IF_POSITION) != start_modes_[0].end()) { velocity_controller_running_ = false; @@ -1366,6 +1408,12 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod std::find(start_modes_[0].begin(), start_modes_[0].end(), TOOL_CONTACT_GPIO) != start_modes_[0].end()) { tool_contact_controller_running_ = true; } + if (start_modes_[0].size() != 0 && + std::find(start_modes_[0].begin(), start_modes_[0].end(), TWIST_GPIO) != start_modes_[0].end()) { + velocity_controller_running_ = false; + position_controller_running_ = false; + twist_controller_running_ = true; + } start_modes_.clear(); stop_modes_.clear(); diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index b480434b0..0662ad15e 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -298,6 +298,14 @@ + + + + + + + +