Add effort command interface to hardware interface (#1411)

* Add effort command interface to hardware interface

* Add documentation for hardware interface including effort control mode

* Add effort controller to controller switch test

* Ensure backwards compatibility

This ensures backwards compatibility in multiple ways:

- When using this with a description not containing the effort
interface, it will simply not be exported. Therefore, when trying to
start an effort controller that will not work, since the interfaces
aren't available.
- When using this with a robot software version not supporting torque
control this will reject activating effort-controllers.

(cherry picked from commit c1cb7f2)

# Conflicts:
#	ur_robot_driver/doc/index.rst

Author: urfeex

ur_robot_driver/config/ur_controllers.yaml

@@ -21,6 +21,9 @@ controller_manager:
     forward_velocity_controller:
       type: velocity_controllers/JointGroupVelocityController
 
+    forward_effort_controller:
+      type: effort_controllers/JointGroupEffortController
+
     forward_position_controller:
       type: position_controllers/JointGroupPositionController
 
@@ -154,6 +157,17 @@ forward_velocity_controller:
       - $(var tf_prefix)wrist_3_joint
     interface_name: velocity
 
+forward_effort_controller:
+  ros__parameters:
+    joints:
+      - $(var tf_prefix)shoulder_pan_joint
+      - $(var tf_prefix)shoulder_lift_joint
+      - $(var tf_prefix)elbow_joint
+      - $(var tf_prefix)wrist_1_joint
+      - $(var tf_prefix)wrist_2_joint
+      - $(var tf_prefix)wrist_3_joint
+    interface_name: effort
+
 forward_position_controller:
   ros__parameters:
     joints:

ur_robot_driver/doc/hardware_interface.rst

@@ -0,0 +1,52 @@
+:github_url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_robot_driver/doc/hardware_interface.rst
+
+UR Hardware interface
+=====================
+
+The UR hardware interface is the core piece of the ROS driver. It is responsible for communicating
+with the robot controller, sending commands and receiving status updates.
+
+The hardware interface is implemented using the ``ros2_control`` framework, which allows for modular
+and flexible control of the robot.
+
+.. note::
+   The hardware interface itself doesn't define how the robot's motion can be controlled through
+   ROS. For that, a **controller** is needed. There are many controllers to choose from, such as
+   the ``JointTrajectoryController`` or the ``ForceModeController``. See `ros2_controllers
+   <https://control.ros.org/rolling/doc/ros2_controllers/doc/controllers_index.html#controllers-for-manipulators-and-other-robots>`_
+   for "standard" controllers and :ref:`ur_controllers` for more information on UR-specific
+   controllers
+
+Supported control modes
+-----------------------
+
+The UR hardware interface supports the following control modes:
+
+- **Position control**: The robot's joints are controlled by specifying target positions.
+- **Velocity control**: The robot's joints are controlled by specifying target velocities.
+- **Effort control**: The robot's joints are controlled by specifying target efforts (torques).
+  (Only available when running PolyScope >= 5.23.0 / 10.10.0)
+- **Force control**: The robot's end-effector is controlled by specifying target forces
+  in Cartesian space.
+- **Freedrive mode**: The robot can be moved freely by the user without any active control.
+- **Passthrough Trajectory control**: Complete trajectory points are forwarded to the robot for
+  interpolation and execution.
+- **Tool contact mode**: The robot stops when the tool comes into contact with an object, allowing for
+  safe interaction with the environment.
+- **Speed scaling**: Speed scaling on the robot can be read and written through the hardware
+  interface.
+- **GPIO**: Digital and analog I/O pins can be read and written through the hardware interface.
+- **Payload**: Payload configuration can be changed during runtime through the hardware interface.
+- **Force torque sensor**: Force torque sensor data can be read through the hardware interface.
+  Zeroing the sensor is also supported.
+
+Interacting with the hardware interface
+---------------------------------------
+
+As stated above, motion control is done through controllers. However, the ros2_control framework
+provides a set of services to interact with the hardware interface directly. These services can be
+comfortably used through the ``ros2 control`` `command line tool
+<https://control.ros.org/rolling/doc/ros2_control/ros2controlcli/doc/userdoc.html>`_.
+
+E.g. ``ros2 control list_hardware_components`` will list all hardware components, including the UR
+hardware interface with its interfaces as listed above.

ur_robot_driver/doc/index.rst

@@ -14,8 +14,14 @@ Welcome to ur_robot_driver's documentation!
    installation/toc
    usage
    setup_tool_communication
+<<<<<<< HEAD
    ROS_INTERFACE
    generated/index
+=======
+   hardware_interface
+   hardware_interface_parameters
+   dashboard_client
+>>>>>>> c1cb7f2 (Add effort command interface to hardware interface (#1411))
    robot_state_helper
    controller_stopper
 

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -82,6 +82,7 @@ enum StoppingInterface
   STOP_FORCE_MODE,
   STOP_FREEDRIVE,
   STOP_TOOL_CONTACT,
+  STOP_TORQUE,
 };
 
 // We define our own quaternion to use it as a buffer, since we need to pass pointers to the state
@@ -177,6 +178,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   urcl::vector6d_t urcl_position_commands_;
   urcl::vector6d_t urcl_position_commands_old_;
   urcl::vector6d_t urcl_velocity_commands_;
+  urcl::vector6d_t urcl_torque_commands_;
   urcl::vector6d_t urcl_joint_positions_;
   urcl::vector6d_t urcl_joint_velocities_;
   urcl::vector6d_t urcl_joint_efforts_;
@@ -229,6 +231,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   bool initialized_;
   double system_interface_initialized_;
   std::atomic_bool async_thread_shutdown_;
+  urcl::VersionInformation version_info_;
   double get_robot_software_version_major_;
   double get_robot_software_version_minor_;
   double get_robot_software_version_bugfix_;
@@ -305,6 +308,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   std::vector<std::vector<std::string>> start_modes_;
   bool position_controller_running_;
   bool velocity_controller_running_;
+  bool torque_controller_running_;
   bool force_mode_controller_running_ = false;
 
   std::unique_ptr<urcl::UrDriver> ur_driver_;

ur_robot_driver/launch/ur_control.launch.py

@@ -359,6 +359,7 @@ def controller_spawner(controllers, active=True):
         "joint_trajectory_controller",
         "forward_velocity_controller",
         "forward_position_controller",
+        "forward_effort_controller",
         "force_mode_controller",
         "passthrough_trajectory_controller",
         "freedrive_mode_controller",