Added an EffortJointInterface

Author: hsd-dev

ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h

@@ -228,6 +228,7 @@ class HardwareInterface : public hardware_interface::RobotHW
   hardware_interface::PositionJointInterface pj_interface_;
   ur_controllers::SpeedScalingInterface speedsc_interface_;
   hardware_interface::VelocityJointInterface vj_interface_;
+  hardware_interface::EffortJointInterface ej_interface_;
   ur_controllers::ScaledVelocityJointInterface svj_interface_;
   hardware_interface::ForceTorqueSensorInterface fts_interface_;
 
@@ -236,6 +237,7 @@ class HardwareInterface : public hardware_interface::RobotHW
 
   vector6d_t joint_position_command_;
   vector6d_t joint_velocity_command_;
+  vector6d_t joint_effort_command_;
   vector6d_t joint_positions_;
   vector6d_t joint_velocities_;
   vector6d_t joint_efforts_;

ur_robot_driver/src/ros/hardware_interface.cpp

@@ -298,10 +298,15 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
                                                                       &joint_velocities_[i], &joint_efforts_[i]));
 
     // Create joint position control interface
-    pj_interface_.registerHandle(
-        hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
-    vj_interface_.registerHandle(
-        hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i]));
+    hardware_interface::JointHandle joint_handle_position =
+        hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]);
+    pj_interface_.registerHandle(joint_handle_position);
+    hardware_interface::JointHandle joint_handle_velocity =
+        hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i]);
+    vj_interface_.registerHandle(joint_handle_velocity);
+    hardware_interface::JointHandle joint_handle_effort =
+        hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_effort_command_[i]);
+    ej_interface_.registerHandle(joint_handle_effort);
     spj_interface_.registerHandle(ur_controllers::ScaledJointHandle(
         js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i], &speed_scaling_combined_));
     svj_interface_.registerHandle(ur_controllers::ScaledJointHandle(
@@ -322,6 +327,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   registerInterface(&spj_interface_);
   registerInterface(&pj_interface_);
   registerInterface(&vj_interface_);
+  registerInterface(&ej_interface_);
   registerInterface(&svj_interface_);
   registerInterface(&speedsc_interface_);
   registerInterface(&fts_interface_);