Add Cartesian twist controller

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}

ur_controllers/controller_plugins.xml

@@ -39,4 +39,9 @@
       Controller to use the tool contact functionality of the robot.
     </description>
   </class>
+  <class name="ur_controllers/TwistController" type="ur_controllers::TwistController" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      Controller to move a manipulator's TCP using Cartesian velocities.
+    </description>
+  </class>
 </library>

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 <memory>
+
+#include <controller_interface/controller_interface.hpp>
+#include <realtime_tools/realtime_thread_safe_box.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+
+#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<TwistStamped>::SharedPtr twist_command_subscriber_ = nullptr;
+
+  realtime_tools::RealtimeThreadSafeBox<TwistStamped> received_twist_msg_;
+  TwistStamped command_msg_;
+
+  std::shared_ptr<twist_controller::ParamListener> param_listener_;
+  twist_controller::Params controller_params_;
+};
+}  // namespace ur_controllers
+
+#endif  // UR_CONTROLLERS__TWIST_CONTROLLER_HPP_

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 <ur_controllers/twist_controller.hpp>
+
+namespace ur_controllers
+{
+controller_interface::CallbackReturn TwistController::on_init()
+{
+  param_listener_ = std::make_shared<twist_controller::ParamListener>(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<TwistStamped>(
+      "~/twist", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr<TwistStamped> 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)

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"
+  }

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

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<std::string, std::unordered_map<std::string, bool>> mode_compatibility_;
 };

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))