diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 858f4610a..c845dd759 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 @@ -164,6 +167,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/doc/hardware_interface.rst b/ur_robot_driver/doc/hardware_interface.rst new file mode 100644 index 000000000..9cac33f21 --- /dev/null +++ b/ur_robot_driver/doc/hardware_interface.rst @@ -0,0 +1,52 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_robot_driver/doc/hardware_interface.rst + +UR Hardware interface +===================== + +The UR hardware interface is the core piece of the ROS driver. It is responsible for communicating +with the robot controller, sending commands and receiving status updates. + +The hardware interface is implemented using the ``ros2_control`` framework, which allows for modular +and flexible control of the robot. + +.. note:: + The hardware interface itself doesn't define how the robot's motion can be controlled through + ROS. For that, a **controller** is needed. There are many controllers to choose from, such as + the ``JointTrajectoryController`` or the ``ForceModeController``. See `ros2_controllers + `_ + for "standard" controllers and :ref:`ur_controllers` for more information on UR-specific + controllers + +Supported control modes +----------------------- + +The UR hardware interface supports the following control modes: + +- **Position control**: The robot's joints are controlled by specifying target positions. +- **Velocity control**: The robot's joints are controlled by specifying target velocities. +- **Effort control**: The robot's joints are controlled by specifying target efforts (torques). + (Only available when running PolyScope >= 5.23.0 / 10.10.0) +- **Force control**: The robot's end-effector is controlled by specifying target forces + in Cartesian space. +- **Freedrive mode**: The robot can be moved freely by the user without any active control. +- **Passthrough Trajectory control**: Complete trajectory points are forwarded to the robot for + interpolation and execution. +- **Tool contact mode**: The robot stops when the tool comes into contact with an object, allowing for + safe interaction with the environment. +- **Speed scaling**: Speed scaling on the robot can be read and written through the hardware + interface. +- **GPIO**: Digital and analog I/O pins can be read and written through the hardware interface. +- **Payload**: Payload configuration can be changed during runtime through the hardware interface. +- **Force torque sensor**: Force torque sensor data can be read through the hardware interface. + Zeroing the sensor is also supported. + +Interacting with the hardware interface +--------------------------------------- + +As stated above, motion control is done through controllers. However, the ros2_control framework +provides a set of services to interact with the hardware interface directly. These services can be +comfortably used through the ``ros2 control`` `command line tool +`_. + +E.g. ``ros2 control list_hardware_components`` will list all hardware components, including the UR +hardware interface with its interfaces as listed above. diff --git a/ur_robot_driver/doc/index.rst b/ur_robot_driver/doc/index.rst index 60f8d94f4..30654270e 100644 --- a/ur_robot_driver/doc/index.rst +++ b/ur_robot_driver/doc/index.rst @@ -17,6 +17,7 @@ ur_robot_driver usage/toc operation_modes setup_tool_communication + hardware_interface hardware_interface_parameters dashboard_client robot_state_helper 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 c09cf363b..00da049f5 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 f5271597b..02a4da9b4 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -200,6 +200,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 a0cbbfef7..d8f1c7205 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::HardwareComponent 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::HardwareComponent 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 @@ -629,11 +658,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); @@ -810,6 +839,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_; @@ -844,7 +874,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); @@ -1124,6 +1155,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); } @@ -1159,6 +1193,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 }, @@ -1177,6 +1212,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(get_logger(), "Start modes of all joints have to be the same."); @@ -1192,6 +1240,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 }, @@ -1237,6 +1287,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; @@ -1267,6 +1322,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; @@ -1274,9 +1330,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()) { @@ -1286,6 +1350,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; } @@ -1293,6 +1358,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,