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 885660117..9a0976158 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -400,6 +400,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface std::unique_ptr data_package_buffer_; std::function get_data_package; + + 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 41b9a7bfd..fb7913eb1 100644 --- a/ur_robot_driver/launch/ur_rsp.launch.py +++ b/ur_robot_driver/launch/ur_rsp.launch.py @@ -74,6 +74,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( [ @@ -191,6 +192,8 @@ def generate_launch_description(): "trajectory_port:=", trajectory_port, " ", + "use_currents_as_efforts:=", + use_currents_as_efforts, ] ) robot_description = { @@ -457,6 +460,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/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index c658e82e5..297f5a4d8 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -676,6 +676,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") { @@ -773,12 +778,21 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Initializing driver..."); try { + auto input_recipe = urcl::rtde_interface::RTDEClient::readRecipe(input_recipe_filename); + auto output_recipe = urcl::rtde_interface::RTDEClient::readRecipe(output_recipe_filename); + + if (!use_currents_as_efforts_) { + if (std::find(output_recipe.begin(), output_recipe.end(), "actual_current_as_torque") == output_recipe.end()) { + output_recipe.push_back("actual_current_as_torque"); + } + } + rtde_comm_has_been_started_ = false; urcl::UrDriverConfiguration driver_config; driver_config.robot_ip = robot_ip; driver_config.script_file = script_filename; - driver_config.output_recipe_file = output_recipe_filename; - driver_config.input_recipe_file = input_recipe_filename; + driver_config.output_recipe = output_recipe; + driver_config.input_recipe = input_recipe; driver_config.headless_mode = headless_mode; driver_config.reverse_port = static_cast(reverse_port); driver_config.script_sender_port = static_cast(script_sender_port); @@ -793,7 +807,7 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou std::bind(&URPositionHardwareInterface::handleRobotProgramState, this, std::placeholders::_1); ur_driver_ = std::make_shared(driver_config); if (ur_driver_->getControlFrequency() != info_.rw_rate) { - ur_driver_->resetRTDEClient(output_recipe_filename, input_recipe_filename, info_.rw_rate); + ur_driver_->resetRTDEClient(output_recipe, input_recipe, info_.rw_rate); } data_package_buffer_ = std::make_unique(ur_driver_->getRTDEOutputRecipe()); } catch (urcl::ToolCommNotAvailable& e) { @@ -858,6 +872,17 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Initializing InstructionExecutor"); instruction_executor_ = std::make_shared(ur_driver_); + 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); // Start async thread for sending motion primitives @@ -971,7 +996,11 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: packet_read_ = true; readData(data_package_buffer_, "actual_q", urcl_joint_positions_); readData(data_package_buffer_, "actual_qd", urcl_joint_velocities_); - readData(data_package_buffer_, "actual_current", urcl_joint_efforts_); + if (use_currents_as_efforts_) { + readData(data_package_buffer_, "actual_current", urcl_joint_efforts_); + } else { + readData(data_package_buffer_, "actual_current_as_torque", urcl_joint_efforts_); + } readData(data_package_buffer_, "target_speed_fraction", target_speed_fraction_); readData(data_package_buffer_, "speed_scaling", speed_scaling_); readData(data_package_buffer_, "runtime_state", runtime_state_); diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index e8404741d..36e5847a2 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -29,6 +29,7 @@ robot_receive_timeout:=0.04 rw_rate:=0 verify_robot_model:=true + use_currents_as_efforts:=true "> @@ -77,6 +78,7 @@ ${tool_tcp_port} ${robot_receive_timeout} ${verify_robot_model} + ${use_currents_as_efforts} diff --git a/ur_robot_driver/urdf/ur.urdf.xacro b/ur_robot_driver/urdf/ur.urdf.xacro index 8e5fd4b18..ebccd77f4 100644 --- a/ur_robot_driver/urdf/ur.urdf.xacro +++ b/ur_robot_driver/urdf/ur.urdf.xacro @@ -34,6 +34,8 @@ + + @@ -104,6 +106,7 @@ trajectory_port="$(arg trajectory_port)" ur_type="$(arg ur_type)" verify_robot_model="$(arg verify_robot_model)" + use_currents_as_efforts="$(arg use_currents_as_efforts)" />