Add effort command interface to hardware interface (backport #1411) (#1528)

Author: mergify[bot]

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

@@ -372,6 +372,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",

ur_robot_driver/package.xml

@@ -46,6 +46,7 @@
   <depend>ur_description</depend>
   <depend>ur_msgs</depend>
 
+  <exec_depend>effort_controllers</exec_depend>
   <exec_depend>force_torque_sensor_broadcaster</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>joint_state_publisher</exec_depend>