diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 333069294..51ed65438 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/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: 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 e6cc00c86..be2e40bb1 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/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> start_modes_; bool position_controller_running_; bool velocity_controller_running_; + bool torque_controller_running_; bool force_mode_controller_running_ = false; std::unique_ptr ur_driver_; diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 49f4f33f7..c5fabb9f9 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/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", diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index a051148b8..e1b614f5e 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -60,37 +60,50 @@ namespace ur_robot_driver URPositionHardwareInterface::URPositionHardwareInterface() { mode_compatibility_[hardware_interface::HW_IF_POSITION][hardware_interface::HW_IF_VELOCITY] = false; + mode_compatibility_[hardware_interface::HW_IF_POSITION][hardware_interface::HW_IF_EFFORT] = false; mode_compatibility_[hardware_interface::HW_IF_POSITION][FORCE_MODE_GPIO] = false; mode_compatibility_[hardware_interface::HW_IF_POSITION][PASSTHROUGH_GPIO] = false; mode_compatibility_[hardware_interface::HW_IF_POSITION][FREEDRIVE_MODE_GPIO] = false; mode_compatibility_[hardware_interface::HW_IF_POSITION][TOOL_CONTACT_GPIO] = true; mode_compatibility_[hardware_interface::HW_IF_VELOCITY][hardware_interface::HW_IF_POSITION] = false; + mode_compatibility_[hardware_interface::HW_IF_VELOCITY][hardware_interface::HW_IF_EFFORT] = false; mode_compatibility_[hardware_interface::HW_IF_VELOCITY][FORCE_MODE_GPIO] = false; mode_compatibility_[hardware_interface::HW_IF_VELOCITY][PASSTHROUGH_GPIO] = false; mode_compatibility_[hardware_interface::HW_IF_VELOCITY][FREEDRIVE_MODE_GPIO] = false; mode_compatibility_[hardware_interface::HW_IF_VELOCITY][TOOL_CONTACT_GPIO] = true; + mode_compatibility_[hardware_interface::HW_IF_EFFORT][hardware_interface::HW_IF_POSITION] = false; + mode_compatibility_[hardware_interface::HW_IF_EFFORT][hardware_interface::HW_IF_VELOCITY] = false; + mode_compatibility_[hardware_interface::HW_IF_EFFORT][FORCE_MODE_GPIO] = false; + mode_compatibility_[hardware_interface::HW_IF_EFFORT][PASSTHROUGH_GPIO] = false; + mode_compatibility_[hardware_interface::HW_IF_EFFORT][FREEDRIVE_MODE_GPIO] = false; + mode_compatibility_[hardware_interface::HW_IF_EFFORT][TOOL_CONTACT_GPIO] = true; + mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false; mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false; + mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_EFFORT] = false; mode_compatibility_[FORCE_MODE_GPIO][PASSTHROUGH_GPIO] = true; mode_compatibility_[FORCE_MODE_GPIO][FREEDRIVE_MODE_GPIO] = false; mode_compatibility_[FORCE_MODE_GPIO][TOOL_CONTACT_GPIO] = false; mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_POSITION] = false; mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_VELOCITY] = false; + mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_EFFORT] = false; mode_compatibility_[PASSTHROUGH_GPIO][FORCE_MODE_GPIO] = true; mode_compatibility_[PASSTHROUGH_GPIO][FREEDRIVE_MODE_GPIO] = false; mode_compatibility_[PASSTHROUGH_GPIO][TOOL_CONTACT_GPIO] = true; mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false; mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false; + mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_EFFORT] = false; mode_compatibility_[FREEDRIVE_MODE_GPIO][FORCE_MODE_GPIO] = false; mode_compatibility_[FREEDRIVE_MODE_GPIO][PASSTHROUGH_GPIO] = false; mode_compatibility_[FREEDRIVE_MODE_GPIO][TOOL_CONTACT_GPIO] = false; mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_POSITION] = true; mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_VELOCITY] = true; + mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_EFFORT] = true; mode_compatibility_[TOOL_CONTACT_GPIO][FORCE_MODE_GPIO] = false; mode_compatibility_[TOOL_CONTACT_GPIO][PASSTHROUGH_GPIO] = true; mode_compatibility_[TOOL_CONTACT_GPIO][FREEDRIVE_MODE_GPIO] = false; @@ -120,6 +133,7 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; position_controller_running_ = false; velocity_controller_running_ = false; + torque_controller_running_ = false; freedrive_mode_controller_running_ = false; passthrough_trajectory_controller_running_ = false; tool_contact_controller_running_ = false; @@ -143,51 +157,55 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys trajectory_joint_accelerations_.reserve(32768); for (const hardware_interface::ComponentInfo& joint : info_.joints) { - if (joint.command_interfaces.size() != 2) { + auto has_cmd_interface = [](const hardware_interface::ComponentInfo& joint, const std::string& interface_name) { + auto it = find_if( + joint.command_interfaces.begin(), joint.command_interfaces.end(), + [&interface_name](const hardware_interface::InterfaceInfo& obj) { return obj.name == interface_name; }); + return it != joint.command_interfaces.end(); + }; + + if (!has_cmd_interface(joint, hardware_interface::HW_IF_POSITION)) { RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), - "Joint '%s' has %zu command interfaces found. 2 expected.", joint.name.c_str(), - joint.command_interfaces.size()); + "Joint '%s' does not contain a '%s' command interface.", joint.name.c_str(), + hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } - if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + if (!has_cmd_interface(joint, hardware_interface::HW_IF_VELOCITY)) { RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), - "Joint '%s' have %s command interfaces found as first command interface. '%s' expected.", - joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + "Joint '%s' does not contain a '%s' command interface.", joint.name.c_str(), + hardware_interface::HW_IF_VELOCITY); return hardware_interface::CallbackReturn::ERROR; } - if (joint.command_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) { - RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), - "Joint '%s' have %s command interfaces found as second command interface. '%s' expected.", - joint.name.c_str(), joint.command_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); - return hardware_interface::CallbackReturn::ERROR; - } + // We treat the torque command interface as optional here for now. This should ensure backwards + // compatibility with older descriptions. - if (joint.state_interfaces.size() != 3) { - RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), "Joint '%s' has %zu state interface. 3 expected.", - joint.name.c_str(), joint.state_interfaces.size()); - return hardware_interface::CallbackReturn::ERROR; - } + auto has_state_interface = [](const hardware_interface::ComponentInfo& joint, const std::string& interface_name) { + auto it = find_if( + joint.state_interfaces.begin(), joint.state_interfaces.end(), + [&interface_name](const hardware_interface::InterfaceInfo& obj) { return obj.name == interface_name; }); + return it != joint.state_interfaces.end(); + }; - if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + if (!has_state_interface(joint, hardware_interface::HW_IF_POSITION)) { RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), - "Joint '%s' have %s state interface as first state interface. '%s' expected.", joint.name.c_str(), - joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + "Joint '%s' does not contain a '%s' state interface.", joint.name.c_str(), + hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } - if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) { + if (!has_state_interface(joint, hardware_interface::HW_IF_VELOCITY)) { RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), - "Joint '%s' have %s state interface as second state interface. '%s' expected.", joint.name.c_str(), - joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); + "Joint '%s' does not contain a '%s' state interface.", joint.name.c_str(), + hardware_interface::HW_IF_VELOCITY); return hardware_interface::CallbackReturn::ERROR; } - if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) { + if (!has_state_interface(joint, hardware_interface::HW_IF_EFFORT)) { RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), - "Joint '%s' have %s state interface as third state interface. '%s' expected.", joint.name.c_str(), - joint.state_interfaces[2].name.c_str(), hardware_interface::HW_IF_EFFORT); + "Joint '%s' does not contain a '%s' state interface.", joint.name.c_str(), + hardware_interface::HW_IF_EFFORT); return hardware_interface::CallbackReturn::ERROR; } } @@ -321,6 +339,12 @@ std::vector URPositionHardwareInterface::exp std::vector URPositionHardwareInterface::export_command_interfaces() { + auto has_cmd_interface = [](const hardware_interface::ComponentInfo& joint, const std::string& interface_name) { + auto it = + find_if(joint.command_interfaces.begin(), joint.command_interfaces.end(), + [&interface_name](const hardware_interface::InterfaceInfo& obj) { return obj.name == interface_name; }); + return it != joint.command_interfaces.end(); + }; std::vector command_interfaces; for (size_t i = 0; i < info_.joints.size(); ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( @@ -328,6 +352,11 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &urcl_velocity_commands_[i])); + + if (has_cmd_interface(info_.joints[i], hardware_interface::HW_IF_EFFORT)) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &urcl_torque_commands_[i])); + } } // Obtain the tf_prefix from the urdf so that we can have the general interface multiple times // NOTE using the tf_prefix at this point is some kind of workaround. One should actually go through the list of gpio @@ -626,11 +655,11 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou } // Export version information to state interfaces - urcl::VersionInformation version_info = ur_driver_->getVersion(); - get_robot_software_version_major_ = version_info.major; - get_robot_software_version_minor_ = version_info.minor; - get_robot_software_version_build_ = version_info.build; - get_robot_software_version_bugfix_ = version_info.bugfix; + version_info_ = ur_driver_->getVersion(); + get_robot_software_version_major_ = version_info_.major; + get_robot_software_version_minor_ = version_info_.minor; + get_robot_software_version_build_ = version_info_.build; + get_robot_software_version_bugfix_ = version_info_.bugfix; async_thread_ = std::make_shared(&URPositionHardwareInterface::asyncThread, this); @@ -807,6 +836,7 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: // initialize commands urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; + urcl_torque_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; target_speed_fraction_cmd_ = NO_NEW_CMD_; resend_robot_program_cmd_ = NO_NEW_CMD_; zero_ftsensor_cmd_ = NO_NEW_CMD_; @@ -841,7 +871,8 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: } else if (velocity_controller_running_) { ur_driver_->writeJointCommand(urcl_velocity_commands_, urcl::comm::ControlMode::MODE_SPEEDJ, receive_timeout_); - + } else if (torque_controller_running_) { + ur_driver_->writeJointCommand(urcl_torque_commands_, urcl::comm::ControlMode::MODE_TORQUE, receive_timeout_); } else if (freedrive_mode_controller_running_ && freedrive_activated_) { ur_driver_->writeFreedriveControlMessage(urcl::control::FreedriveControlMessage::FREEDRIVE_NOOP); @@ -1119,6 +1150,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (velocity_controller_running_) { control_modes[i] = { hardware_interface::HW_IF_VELOCITY }; } + if (torque_controller_running_) { + control_modes[i] = { hardware_interface::HW_IF_EFFORT }; + } if (force_mode_controller_running_) { control_modes[i].push_back(FORCE_MODE_GPIO); } @@ -1155,6 +1189,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod const std::vector> start_modes_to_check{ { info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION }, { info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_VELOCITY }, + { info_.joints[i].name + "/" + hardware_interface::HW_IF_EFFORT, hardware_interface::HW_IF_EFFORT }, { tf_prefix + FORCE_MODE_GPIO + "/type", FORCE_MODE_GPIO }, { tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i), PASSTHROUGH_GPIO }, { tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success", FREEDRIVE_MODE_GPIO }, @@ -1173,6 +1208,19 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod } } + // effort control is only available from 5.23.0 / 10.10.0 + if (std::any_of(start_modes_.begin(), start_modes_.end(), [&](const std::vector& modes) { + return std::any_of(modes.begin(), modes.end(), + [&](const std::string& mode) { return mode == hardware_interface::HW_IF_EFFORT; }); + })) { + if ((version_info_.major == 5 && version_info_.minor < 23) || + (version_info_.major == 10 && version_info_.minor < 10) || version_info_.major < 5) { + RCLCPP_ERROR(get_logger(), "Requested to use effort interface on a robot version that doesn't support it. Torque " + "control is available from robot software 5.23.0 / 10.10.0 on."); + return hardware_interface::return_type::ERROR; + } + } + if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(), [&](const std::vector& other) { return other == start_modes_[0]; })) { RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Start modes of all joints have to be the same."); @@ -1188,6 +1236,8 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod StoppingInterface::STOP_POSITION }, { info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_VELOCITY, StoppingInterface::STOP_VELOCITY }, + { info_.joints[i].name + "/" + hardware_interface::HW_IF_EFFORT, hardware_interface::HW_IF_EFFORT, + StoppingInterface::STOP_TORQUE }, { tf_prefix + FORCE_MODE_GPIO + "/disable_cmd", FORCE_MODE_GPIO, StoppingInterface::STOP_FORCE_MODE }, { tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i), PASSTHROUGH_GPIO, StoppingInterface::STOP_PASSTHROUGH }, @@ -1233,6 +1283,11 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod velocity_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; } + if (stop_modes_[0].size() != 0 && + std::find(stop_modes_[0].begin(), stop_modes_[0].end(), StoppingInterface::STOP_TORQUE) != stop_modes_[0].end()) { + torque_controller_running_ = false; + urcl_torque_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; + } if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), StoppingInterface::STOP_FORCE_MODE) != stop_modes_[0].end()) { force_mode_controller_running_ = false; @@ -1263,6 +1318,7 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod if (start_modes_.size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), hardware_interface::HW_IF_POSITION) != start_modes_[0].end()) { velocity_controller_running_ = false; + torque_controller_running_ = false; passthrough_trajectory_controller_running_ = false; urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_; position_controller_running_ = true; @@ -1270,9 +1326,17 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod } else if (start_modes_[0].size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), hardware_interface::HW_IF_VELOCITY) != start_modes_[0].end()) { position_controller_running_ = false; + torque_controller_running_ = false; passthrough_trajectory_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; velocity_controller_running_ = true; + } else if (start_modes_[0].size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), + hardware_interface::HW_IF_EFFORT) != start_modes_[0].end()) { + position_controller_running_ = false; + velocity_controller_running_ = false; + torque_controller_running_ = true; + passthrough_trajectory_controller_running_ = false; + urcl_torque_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; } if (start_modes_[0].size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), FORCE_MODE_GPIO) != start_modes_[0].end()) { @@ -1282,6 +1346,7 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod std::find(start_modes_[0].begin(), start_modes_[0].end(), PASSTHROUGH_GPIO) != start_modes_[0].end()) { velocity_controller_running_ = false; position_controller_running_ = false; + torque_controller_running_ = false; passthrough_trajectory_controller_running_ = true; passthrough_trajectory_abort_ = 0.0; } @@ -1289,6 +1354,7 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod std::find(start_modes_[0].begin(), start_modes_[0].end(), FREEDRIVE_MODE_GPIO) != start_modes_[0].end()) { velocity_controller_running_ = false; position_controller_running_ = false; + torque_controller_running_ = false; freedrive_mode_controller_running_ = true; freedrive_activated_ = false; } diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py index 8bfd8ac7b..9e66c510b 100644 --- a/ur_robot_driver/test/integration_test_controller_switch.py +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -219,6 +219,14 @@ def test_activating_controller_with_running_position_controller_fails(self): ], ).ok ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_effort_controller", + ], + ).ok + ) self.assertFalse( self._controller_manager_interface.switch_controller( strictness=SwitchController.Request.STRICT,