diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 00da049f5..74d54b4aa 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -324,6 +324,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface const std::string TOOL_CONTACT_GPIO = "tool_contact"; std::unordered_map> mode_compatibility_; + + bool use_currents_as_efforts_ = false; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/launch/ur_rsp.launch.py b/ur_robot_driver/launch/ur_rsp.launch.py index 225d251d7..36c1f8bc0 100644 --- a/ur_robot_driver/launch/ur_rsp.launch.py +++ b/ur_robot_driver/launch/ur_rsp.launch.py @@ -73,6 +73,7 @@ def generate_launch_description(): reverse_port = LaunchConfiguration("reverse_port") script_sender_port = LaunchConfiguration("script_sender_port") trajectory_port = LaunchConfiguration("trajectory_port") + use_currents_as_efforts = LaunchConfiguration("use_currents_as_efforts") script_filename = PathJoinSubstitution( [ @@ -184,6 +185,8 @@ def generate_launch_description(): "trajectory_port:=", trajectory_port, " ", + "use_currents_as_efforts:=", + use_currents_as_efforts, ] ) robot_description = { @@ -442,6 +445,13 @@ def generate_launch_description(): description="Port that will be opened for trajectory control.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "use_currents_as_efforts", + default_value="true", + description="Report motor currents as efforts. When set to false, the torques as reported from the robot are used. Note that this requires software 5.23.0 / 10.11.0.", + ) + ) return LaunchDescription( declared_arguments diff --git a/ur_robot_driver/resources/rtde_output_recipe.txt b/ur_robot_driver/resources/rtde_output_recipe.txt index c52197cb7..572dc3c41 100644 --- a/ur_robot_driver/resources/rtde_output_recipe.txt +++ b/ur_robot_driver/resources/rtde_output_recipe.txt @@ -26,4 +26,5 @@ safety_mode robot_status_bits safety_status_bits actual_current +actual_current_as_torque tcp_offset diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index d8f1c7205..b026d66e4 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -511,6 +511,11 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou // The driver will offer an interface to receive the program's URScript on this port. const int script_sender_port = stoi(info_.hardware_parameters["script_sender_port"]); + // Newer software version (5.23.0 / 10.11.0) support reporting the actual joint torques. On older + // versions fall back to the currents, instead. + use_currents_as_efforts_ = ((info_.hardware_parameters["use_currents_as_efforts"] == "true") || + (info_.hardware_parameters["use_currents_as_efforts"] == "True")); + // The ip address of the host the driver runs on std::string reverse_ip = info_.hardware_parameters["reverse_ip"]; if (reverse_ip == "0.0.0.0") { @@ -664,6 +669,17 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou get_robot_software_version_build_ = version_info_.build; get_robot_software_version_bugfix_ = version_info_.bugfix; + if (!use_currents_as_efforts_) { + if ((version_info_.major == 5 && version_info_.minor < 23) || + (version_info_.major == 10 && version_info_.minor < 11) || version_info_.major < 5) { + RCLCPP_ERROR(get_logger(), + "Driver configured to use actual torques as efforts, which is not supported by this software " + "version %s. Please use version 5.23.0 / 10.11.0 or newer for this feature.", + version_info_.toString().c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + } + async_thread_ = std::make_shared(&URPositionHardwareInterface::asyncThread, this); RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "System successfully started!"); @@ -774,7 +790,11 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: packet_read_ = true; readData(data_pkg, "actual_q", urcl_joint_positions_); readData(data_pkg, "actual_qd", urcl_joint_velocities_); - readData(data_pkg, "actual_current", urcl_joint_efforts_); + if (use_currents_as_efforts_) { + readData(data_pkg, "actual_current", urcl_joint_efforts_); + } else { + readData(data_pkg, "actual_current_as_torque", urcl_joint_efforts_); + } readData(data_pkg, "target_speed_fraction", target_speed_fraction_); readData(data_pkg, "speed_scaling", speed_scaling_); diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index b480434b0..927a36ff5 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -26,6 +26,7 @@ trajectory_port:=50003 non_blocking_read:=true robot_receive_timeout:=0.04 + use_currents_as_efforts:=true "> @@ -71,6 +72,7 @@ ${tool_device_name} ${tool_tcp_port} ${robot_receive_timeout} + ${use_currents_as_efforts} diff --git a/ur_robot_driver/urdf/ur.urdf.xacro b/ur_robot_driver/urdf/ur.urdf.xacro index e14476d0e..66f69721b 100644 --- a/ur_robot_driver/urdf/ur.urdf.xacro +++ b/ur_robot_driver/urdf/ur.urdf.xacro @@ -33,6 +33,8 @@ + + @@ -101,6 +103,7 @@ reverse_port="$(arg reverse_port)" script_sender_port="$(arg script_sender_port)" trajectory_port="$(arg trajectory_port)" + use_currents_as_efforts="$(arg use_currents_as_efforts)" />