Friction model controller

ur_controllers/CMakeLists.txt

@@ -62,6 +62,11 @@ generate_parameter_library(
   src/force_mode_controller_parameters.yaml
 )
 
+generate_parameter_library(
+  friction_model_controller_parameters
+  src/friction_model_controller_parameters.yaml
+)
+
 generate_parameter_library(
   gpio_controller_parameters
   src/gpio_controller_parameters.yaml
@@ -95,6 +100,7 @@ generate_parameter_library(
 add_library(${PROJECT_NAME} SHARED
   src/tool_contact_controller.cpp
   src/force_mode_controller.cpp
+  src/friction_model_controller.cpp
   src/scaled_joint_trajectory_controller.cpp
   src/speed_scaling_state_broadcaster.cpp
   src/freedrive_mode_controller.cpp
@@ -108,6 +114,7 @@ target_include_directories(${PROJECT_NAME} PRIVATE
 target_link_libraries(${PROJECT_NAME}
   tool_contact_controller_parameters
   force_mode_controller_parameters
+  friction_model_controller_parameters
   gpio_controller_parameters
   speed_scaling_state_broadcaster_parameters
   scaled_joint_trajectory_controller_parameters
@@ -191,6 +198,14 @@ if(BUILD_TESTING)
     controller_manager::controller_manager
     ros2_control_test_assets::ros2_control_test_assets
   )
+  ament_add_gmock(test_load_friction_model_controller
+    test/test_load_friction_model_controller.cpp
+  )
+  target_link_libraries(test_load_friction_model_controller
+    ${PROJECT_NAME}
+    controller_manager::controller_manager
+    ros2_control_test_assets::ros2_control_test_assets
+  )
 endif()
 
 ament_package()

ur_controllers/controller_plugins.xml

@@ -19,6 +19,11 @@
       Controller to use UR's force_mode.
     </description>
   </class>
+  <class name="ur_controllers/FrictionModelController" type="ur_controllers::FrictionModelController" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      Controller to set per-joint viscous and Coulomb friction scale factors for direct torque control.
+    </description>
+  </class>
   <class name="ur_controllers/FreedriveModeController" type="ur_controllers::FreedriveModeController" base_class_type="controller_interface::ControllerInterface">
     <description>
       This controller handles the activation of the freedrive mode.

ur_controllers/doc/index.rst

@@ -349,6 +349,38 @@ 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.
 
+.. _friction_model_controller:
+
+ur_controllers/FrictionModelController
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+This controller allows setting per-joint viscous and Coulomb friction scale factors for direct
+torque control. The friction model scales control how much internal friction compensation the robot
+applies during direct torque control mode.
+
+Parameters
+""""""""""
+
++------------------------------------+--------+---------------+----------------------------------------------------------------------+
+| Parameter name                     | Type   | Default value | Description                                                          |
+|                                    |        |               |                                                                      |
++------------------------------------+--------+---------------+----------------------------------------------------------------------+
+| ``tf_prefix``                      | string | <empty>       | Urdf prefix of the corresponding arm                                 |
++------------------------------------+--------+---------------+----------------------------------------------------------------------+
+| ``check_io_successful_retries``    | int    | 10            | Amount of retries for checking if setting friction scales was        |
+|                                    |        |               | successful                                                           |
++------------------------------------+--------+---------------+----------------------------------------------------------------------+
+
+Service interface / usage
+"""""""""""""""""""""""""
+
+The controller provides a service for setting friction scale factors. To use this service, the
+controller has to be in ``active`` state.
+
+* ``~/set_friction_model_parameters [ur_msgs/srv/SetFrictionModelParameters]``: Set per-joint viscous and Coulomb
+  friction scale factors for direct torque control. Each array must contain exactly one value for each joint in
+  range [0-1] where 0 means no compensation and 1 means full compensation.
+
 .. _freedrive_mode_controller:
 
 ur_controllers/FreedriveModeController

ur_controllers/include/ur_controllers/friction_model_controller.hpp

@@ -0,0 +1,111 @@
+// Copyright 2026, 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  Rune Søe-Knudsen rsk@universal-robots.com
+ * \date    2026-03-02
+ *
+ */
+//----------------------------------------------------------------------
+
+#pragma once
+#include <array>
+
+#include <controller_interface/controller_interface.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <realtime_tools/realtime_thread_safe_box.hpp>
+#include <ur_msgs/srv/set_friction_model_parameters.hpp>
+
+#include "ur_controllers/friction_model_controller_parameters.hpp"
+
+namespace ur_controllers
+{
+namespace friction_model
+{
+enum CommandInterfaces
+{
+  FRICTION_MODEL_VISCOUS_0 = 0u,
+  FRICTION_MODEL_VISCOUS_1 = 1,
+  FRICTION_MODEL_VISCOUS_2 = 2,
+  FRICTION_MODEL_VISCOUS_3 = 3,
+  FRICTION_MODEL_VISCOUS_4 = 4,
+  FRICTION_MODEL_VISCOUS_5 = 5,
+  FRICTION_MODEL_COULOMB_0 = 6,
+  FRICTION_MODEL_COULOMB_1 = 7,
+  FRICTION_MODEL_COULOMB_2 = 8,
+  FRICTION_MODEL_COULOMB_3 = 9,
+  FRICTION_MODEL_COULOMB_4 = 10,
+  FRICTION_MODEL_COULOMB_5 = 11,
+  FRICTION_MODEL_ASYNC_SUCCESS = 12,
+};
+}  // namespace friction_model
+
+struct FrictionModelParameters
+{
+  std::array<double, 6> viscous_scale;
+  std::array<double, 6> coulomb_scale;
+};
+
+class FrictionModelController : public controller_interface::ControllerInterface
+{
+public:
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+  CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+
+  CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+  CallbackReturn on_init() override;
+
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override;
+
+private:
+  bool setFrictionModelParameters(const ur_msgs::srv::SetFrictionModelParameters::Request::SharedPtr req,
+                                  ur_msgs::srv::SetFrictionModelParameters::Response::SharedPtr resp);
+
+  rclcpp::Service<ur_msgs::srv::SetFrictionModelParameters>::SharedPtr set_friction_model_parameters_srv_;
+
+  std::shared_ptr<friction_model_controller::ParamListener> param_listener_;
+  friction_model_controller::Params params_;
+
+  realtime_tools::RealtimeThreadSafeBox<FrictionModelParameters> friction_model_params_buffer_;
+  std::atomic<bool> change_requested_;
+  std::atomic<double> async_state_;
+
+  static constexpr double ASYNC_WAITING = 2.0;
+  bool waitForAsyncCommand(std::function<double(void)> get_value);
+};
+}  // namespace ur_controllers