Add force mode controller

ur_controllers/CMakeLists.txt

@@ -8,6 +8,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
 find_package(controller_interface REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(joint_trajectory_controller REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
@@ -16,6 +17,8 @@ find_package(rcutils REQUIRED)
 find_package(realtime_tools REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
 find_package(ur_dashboard_msgs REQUIRED)
 find_package(ur_msgs REQUIRED)
 find_package(generate_parameter_library REQUIRED)
@@ -27,6 +30,7 @@ find_package(action_msgs REQUIRED)
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   angles
   controller_interface
+  geometry_msgs
   joint_trajectory_controller
   lifecycle_msgs
   pluginlib
@@ -35,6 +39,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   realtime_tools
   std_msgs
   std_srvs
+  tf2_geometry_msgs
+  tf2_ros
   ur_dashboard_msgs
   ur_msgs
   generate_parameter_library
@@ -45,6 +51,10 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
 
 include_directories(include)
 
+generate_parameter_library(
+  force_mode_controller_parameters
+  src/force_mode_controller_parameters.yaml
+)
 
 generate_parameter_library(
   gpio_controller_parameters
@@ -72,6 +82,7 @@ generate_parameter_library(
 )
 
 add_library(${PROJECT_NAME} SHARED
+  src/force_mode_controller.cpp
   src/scaled_joint_trajectory_controller.cpp
   src/speed_scaling_state_broadcaster.cpp
   src/gpio_controller.cpp
@@ -82,6 +93,7 @@ target_include_directories(${PROJECT_NAME} PRIVATE
   include
 )
 target_link_libraries(${PROJECT_NAME}
+  force_mode_controller_parameters
   gpio_controller_parameters
   speed_scaling_state_broadcaster_parameters
   scaled_joint_trajectory_controller_parameters
@@ -124,4 +136,23 @@ ament_export_libraries(
   ${PROJECT_NAME}
 )
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(hardware_interface REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_force_mode_controller
+    test/test_load_force_mode_controller.cpp
+  )
+  target_link_libraries(test_load_force_mode_controller
+    ${PROJECT_NAME}
+  )
+  ament_target_dependencies(test_load_force_mode_controller
+    controller_manager
+    ros2_control_test_assets
+  )
+endif()
+
 ament_package()

ur_controllers/controller_plugins.xml

@@ -14,6 +14,11 @@
       This controller publishes the Tool IO.
     </description>
   </class>
+  <class name="ur_controllers/ForceModeController" type="ur_controllers::ForceModeController" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      Controller to use UR's force_mode.
+    </description>
+  </class>
   <class name="ur_controllers/PassthroughTrajectoryController" type="ur_controllers::PassthroughTrajectoryController" base_class_type="controller_interface::ControllerInterface">
     <description>
       This controller forwards a joint-based trajectory to the robot controller for interpolation.

ur_controllers/doc/index.rst

@@ -264,3 +264,79 @@ Implementation details / dataflow
 * When the hardware reports that execution has been aborted (The ``passthrough_trajectory_abort``
   command interface), the action will be aborted.
 * When the action is preempted, execution on the hardware is preempted.
+
+.. _force_mode_controller:
+
+ur_controllers/ForceModeController
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+This controller activates the robot's *Force Mode*. This allows direct force control running on the
+robot control box. This controller basically interfaces the URScript function ``force_mode(...)``.
+
+Force mode can be combined with (and only with) the :ref:`passthrough trajectory controller
+<passthrough_trajectory_controller>` in order to execute motions under a given force constraints.
+
+.. note::
+   This is not an admittance controller, as given force constraints in a certain Cartesian
+   dimension will overwrite the motion commands in that dimension. E.g. when specifying a certain
+   force in the base frame's ``z`` direction, any motion resulting from the move command in the
+   base frame's ``z`` axis will not be executed.
+
+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 force_mode was successful |
++----------------------------------+--------+---------------+---------------------------------------------------------------------+
+
+Service interface / usage
+"""""""""""""""""""""""""
+
+The controller provides two services: One for activating force_mode and one for leaving it. To use
+those services, the controller has to be in ``active`` state.
+
+* ``~/stop_force_mode [std_srvs/srv/Trigger]``: Stop force mode
+* ``~/start_force_mode [ur_msgs/srv/SetForceMode]``: Start force mode
+
+In ``ur_msgs/srv/SetForceMode`` the fields have the following meanings:
+
+task_frame
+   All information (selection vector, wrench, limits, etc) will be considered to be relative
+   to that pose. The pose's frame_id can be anything that is transformable to the robot's
+   ``base`` frame.
+selection_vector_<x,y,z,rx,ry,rz>
+   1 means that the robot will be compliant in the corresponding axis of the task frame.
+wrench
+   The forces/torques the robot will apply to its environment. The robot adjusts its position
+   along/about compliant axis in order to achieve the specified force/torque. Values have no effect for non-
+   compliant axes.
+   Actual wrench applied may be lower than requested due to joint safety limits.
+type
+   An integer [1;3] specifying how the robot interprets the force frame
+
+   1
+      The force frame is transformed in a way such that its y-axis is aligned with a vector pointing
+      from the robot tcp towards the origin of the force frame.
+   2
+      The force frame is not transformed.
+   3
+      The force frame is transformed in a way such that its x-axis is the projection of the robot tcp
+      velocity vector onto the x-y plane of the force frame.
+speed_limits
+   Maximum allowed tcp speed (relative to the task frame). This is **only relevant for axes marked as
+   compliant** in the selection_vector.
+deviation_limits
+   For **non-compliant axes**, these values are the maximum allowed deviation along/about an axis
+   between the actual tcp position and the one set by the program.
+damping_factor
+   Force mode damping factor. Sets the damping parameter in force mode. In range [0;1], default value is 0.025
+   A value of 1 is full damping, so the robot will decelerate quickly if no force is present. A value of 0
+   is no damping, here the robot will maintain the speed.
+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.

ur_controllers/include/ur_controllers/force_mode_controller.hpp

@@ -0,0 +1,147 @@
+// 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
+ */
+//----------------------------------------------------------------------
+
+#pragma once
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <realtime_tools/realtime_buffer.h>
+#include <array>
+#include <memory>
+
+#include <controller_interface/controller_interface.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <std_srvs/srv/trigger.hpp>
+#include <ur_msgs/srv/set_force_mode.hpp>
+
+#include "force_mode_controller_parameters.hpp"
+
+namespace ur_controllers
+{
+enum CommandInterfaces
+{
+  FORCE_MODE_TASK_FRAME_X = 0u,
+  FORCE_MODE_TASK_FRAME_Y = 1,
+  FORCE_MODE_TASK_FRAME_Z = 2,
+  FORCE_MODE_TASK_FRAME_RX = 3,
+  FORCE_MODE_TASK_FRAME_RY = 4,
+  FORCE_MODE_TASK_FRAME_RZ = 5,
+  FORCE_MODE_SELECTION_VECTOR_X = 6,
+  FORCE_MODE_SELECTION_VECTOR_Y = 7,
+  FORCE_MODE_SELECTION_VECTOR_Z = 8,
+  FORCE_MODE_SELECTION_VECTOR_RX = 9,
+  FORCE_MODE_SELECTION_VECTOR_RY = 10,
+  FORCE_MODE_SELECTION_VECTOR_RZ = 11,
+  FORCE_MODE_WRENCH_X = 12,
+  FORCE_MODE_WRENCH_Y = 13,
+  FORCE_MODE_WRENCH_Z = 14,
+  FORCE_MODE_WRENCH_RX = 15,
+  FORCE_MODE_WRENCH_RY = 16,
+  FORCE_MODE_WRENCH_RZ = 17,
+  FORCE_MODE_TYPE = 18,
+  FORCE_MODE_LIMITS_X = 19,
+  FORCE_MODE_LIMITS_Y = 20,
+  FORCE_MODE_LIMITS_Z = 21,
+  FORCE_MODE_LIMITS_RX = 22,
+  FORCE_MODE_LIMITS_RY = 23,
+  FORCE_MODE_LIMITS_RZ = 24,
+  FORCE_MODE_ASYNC_SUCCESS = 25,
+  FORCE_MODE_DISABLE_CMD = 26,
+  FORCE_MODE_DAMPING = 27,
+  FORCE_MODE_GAIN_SCALING = 28,
+};
+enum StateInterfaces
+{
+  INITIALIZED_FLAG = 0u,
+};
+
+struct ForceModeParameters
+{
+  std::array<double, 6> task_frame;
+  std::array<double, 6> selection_vec;
+  std::array<double, 6> limits;
+  geometry_msgs::msg::Wrench wrench;
+  double type;
+  double damping_factor;
+  double gain_scaling;
+};
+
+class ForceModeController : 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 setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req,
+                    ur_msgs::srv::SetForceMode::Response::SharedPtr resp);
+  bool disableForceMode(const std_srvs::srv::Trigger::Request::SharedPtr req,
+                        std_srvs::srv::Trigger::Response::SharedPtr resp);
+  rclcpp::Service<ur_msgs::srv::SetForceMode>::SharedPtr set_force_mode_srv_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr disable_force_mode_srv_;
+
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  std::shared_ptr<force_mode_controller::ParamListener> param_listener_;
+  force_mode_controller::Params params_;
+
+  realtime_tools::RealtimeBuffer<ForceModeParameters> force_mode_params_buffer_;
+  std::atomic<bool> force_mode_active_;
+  std::atomic<bool> change_requested_;
+  std::atomic<double> 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<double(void)> get_value);
+};
+}  // namespace ur_controllers

ur_controllers/package.xml

@@ -23,6 +23,7 @@
 
   <depend>angles</depend>
   <depend>controller_interface</depend>
+  <depend>geometry_msgs</depend>
   <depend>joint_trajectory_controller</depend>
   <depend>lifecycle_msgs</depend>
   <depend>pluginlib</depend>
@@ -31,6 +32,8 @@
   <depend>realtime_tools</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
   <depend>ur_dashboard_msgs</depend>
   <depend>ur_msgs</depend>
   <depend>control_msgs</depend>