Force mode arguments

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,13 +17,16 @@ 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)
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   angles
   controller_interface
+  geometry_msgs
   joint_trajectory_controller
   lifecycle_msgs
   pluginlib
@@ -31,13 +35,19 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   realtime_tools
   std_msgs
   std_srvs
+  tf2_geometry_msgs
+  tf2_ros
   ur_dashboard_msgs
   ur_msgs
   generate_parameter_library
 )
 
 include_directories(include)
 
+generate_parameter_library(
+  force_mode_controller_parameters
+  src/force_mode_controller_parameters.yaml
+)
 
 generate_parameter_library(
   gpio_controller_parameters
@@ -55,6 +65,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)
@@ -63,6 +74,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

ur_controllers/controller_plugins.xml

@@ -14,4 +14,9 @@
       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>
 </library>

ur_controllers/include/ur_controllers/force_mode_controller.hpp

@@ -0,0 +1,124 @@
+// 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 [email protected]
+ * \date    2023-06-29
+ */
+//----------------------------------------------------------------------
+
+#pragma once
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <memory>
+
+#include <controller_interface/controller_interface.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <rclcpp/rclcpp.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,
+};
+
+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;
+
+private:
+  bool setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req,
+                    ur_msgs::srv::SetForceMode::Response::SharedPtr resp);
+  bool disableForceMode();
+  rclcpp::Service<ur_msgs::srv::SetForceMode>::SharedPtr set_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_;
+
+  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

@@ -22,6 +22,7 @@
 
   <depend>angles</depend>
   <depend>controller_interface</depend>
+  <depend>geometry_msgs</depend>
   <depend>joint_trajectory_controller</depend>
   <depend>lifecycle_msgs</depend>
   <depend>pluginlib</depend>
@@ -30,6 +31,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>