From 88275ccc2362f1eb6995a8d1bd5d3ee7ecf69f6c Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 28 Oct 2024 16:34:13 +0100 Subject: [PATCH] Allow setting the analog output domain when setting an analog output (backport of #1123) --- ur_controllers/doc/index.rst | 137 ++++++++++++++++++ .../ur_controllers/gpio_controller.hpp | 6 + ur_controllers/src/gpio_controller.cpp | 47 +++++- .../ur_robot_driver/hardware_interface.hpp | 1 + ur_robot_driver/src/hardware_interface.cpp | 12 +- 5 files changed, 201 insertions(+), 2 deletions(-) create mode 100644 ur_controllers/doc/index.rst diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst new file mode 100644 index 000000000..bc4d7c147 --- /dev/null +++ b/ur_controllers/doc/index.rst @@ -0,0 +1,137 @@ +ur_controllers +============== + +This package contains controllers and hardware interface for ``ros2_controllers`` that are special to the UR +robot family. Currently this contains: + + +* A **speed_scaling_state_broadcaster** that publishes the current execution speed as reported by + the robot to a topic interface. Values are floating points between 0 and 1. +* A **scaled_joint_trajectory_controller** that is similar to the *joint_trajectory_controller*\ , + but it uses the speed scaling reported to align progress of the trajectory between the robot and controller. +* A **io_and_status_controller** that allows setting I/O ports, controlling some UR-specific + functionality and publishes status information about the robot. + +About this package +------------------ + +This package contains controllers not being available in the default ``ros2_controllers`` set. They are +created to support more features offered by the UR robot family. Some of these controllers are +example implementations for certain features and are intended to be generalized and merged +into the default ``ros2_controllers`` controller set at some future point. + +Controller description +---------------------- + +This packages offers a couple of specific controllers that will be explained in the following +sections. + +.. _speed_scaling_state_broadcaster: + +ur_controllers/SpeedScalingStateBroadcaster +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller publishes the current actual execution speed as reported by the robot. Values are +floating points between 0 and 1. + +In the `ur_robot_driver +`_ +this is calculated by multiplying the two `RTDE +`_ data +fields ``speed_scaling`` (which should be equal to the value shown by the speed slider position on the +teach pendant) and ``target_speed_fraction`` (Which is the fraction to which execution gets slowed +down by the controller). + +.. _scaled_jtc: + +ur_controlers/ScaledJointTrajectoryController +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +These controllers work similar to the well-known +`joint_trajectory_controller `_. + +However, they are extended to handle the robot's execution speed specifically. Because the default +``joint_trajectory_controller`` would interpolate the trajectory with the configured time constraints (ie: always assume maximum velocity and acceleration supported by the robot), +this could lead to significant path deviation due to multiple reasons: + + +* The speed slider on the robot might not be at 100%, so motion commands sent from ROS would + effectively get scaled down resulting in a slower execution. +* The robot could scale down motions based on configured safety limits resulting in a slower motion + than expected and therefore not reaching the desired target in a control cycle. +* Motions might not be executed at all, e.g. because the robot is E-stopped or in a protective stop +* Motion commands sent to the robot might not be interpreted, e.g. because there is no + `external_control `_ + program node running on the robot controller. +* The program interpreting motion commands could be paused. + +The following plot illustrates the problem: + +.. image:: traj_without_speed_scaling.png + :target: traj_without_speed_scaling.png + :alt: Trajectory execution with default trajectory controller + + +The graph shows a trajectory with one joint being moved to a target point and back to its starting +point. As the joint's speed is limited to a very low setting on the teach pendant, speed scaling +(black line) activates and limits the joint speed (green line). As a result, the target +trajectory (light blue) doesn't get executed by the robot, but instead the pink trajectory is executed. +The vertical distance between the light blue line and the pink line is the path error in each +control cycle. We can see that the path deviation gets above 300 degrees at some point and the +target point at -6 radians never gets reached. + +All of the cases mentioned above are addressed by the scaled trajectory versions. Trajectory execution +can be transparently scaled down using the speed slider on the teach pendant without leading to +additional path deviations. Pausing the program or hitting the E-stop effectively leads to +``speed_scaling`` being 0 meaning the trajectory will not be continued until the program is continued. +This way, trajectory executions can be explicitly paused and continued. + +With the scaled version of the trajectory controller the example motion shown in the previous diagram becomes: + +.. image:: traj_with_speed_scaling.png + :target: traj_with_speed_scaling.png + :alt: Trajectory execution with scaled_joint_trajectory_controller + + +The deviation between trajectory interpolation on the ROS side and actual robot execution stays minimal and the +robot reaches the intermediate setpoint instead of returning "too early" as in the example above. + +Under the hood this is implemented by proceeding the trajectory not by a full time step but only by +the fraction determined by the current speed scaling. If speed scaling is currently at 50% then +interpolation of the current control cycle will start half a time step after the beginning of the +previous control cycle. + +.. _io_and_status_controller: + +ur_controllers/GPIOController +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller allows setting I/O ports, controlling some UR-specific functionality and publishes +status information about the robot. + +Published topics +"""""""""""""""" + +* ``~/io_states [ur_msgs/msg/IOStates]``: Status of all I/O ports +* ``~/robot_mode [ur_dashboard_msgs/msg/RobotMode]``: The current robot mode (e.g. ``POWER_OFF``, + ``IDLE``, ``RUNNING``) +* ``~/robot_program_running [std_msgs/msg/Bool]``: Publishes whether **the External Control + program** is running or not. If this is ``false`` no commands can be sent to the robot. +* ``~/safety_mode [ur_dashboard_msgs/msg/SafetyMode]``: The robot's current safety mode (e.g. + ``PROTECTIVE_STOP``, ``ROBOT_EMERGENCY_STOP``, ``NORMAL``) +* ``~/tool_data [ur_msgs/msg/ToolDataMsg]``: Information about the robot's tool configuration + +Advertised services +""""""""""""""""""" + +* ``~/hand_back_control [std_srvs/srv/Trigger]``: Calling this service will make the robot program + exit the *External Control* program node and continue with the rest of the program. +* ``~/resend_robot_program [std_srvs/srv/Trigger]``: When :ref:`headless_mode` is used, this + service can be used to restart the *External Control* program on the robot. +* ``~/set_io [ur_msgs/srv/SetIO]``: Set an output pin on the robot. +* ``~/set_analog_output [ur_msgs/srv/SetAnalogOutput]``: Set an analog output on the robot. This + also allows specifying the domain. +* ``~/set_payload [ur_msgs/srv/SetPayload]``: Change the robot's payload on-the-fly. +* ``~/set_speed_slider [ur_msgs/srv/SetSpeedSliderFraction]``: Set the value of the speed slider. +* ``~/zero_ftsensor [std_srvs/srv/Trigger]``: Zeroes the reported wrench of the force torque + sensor. diff --git a/ur_controllers/include/ur_controllers/gpio_controller.hpp b/ur_controllers/include/ur_controllers/gpio_controller.hpp index c20bd680e..b06b5139a 100644 --- a/ur_controllers/include/ur_controllers/gpio_controller.hpp +++ b/ur_controllers/include/ur_controllers/gpio_controller.hpp @@ -51,6 +51,7 @@ #include "ur_dashboard_msgs/msg/robot_mode.hpp" #include "ur_dashboard_msgs/msg/safety_mode.hpp" #include "ur_msgs/srv/set_io.hpp" +#include "ur_msgs/srv/set_analog_output.hpp" #include "ur_msgs/srv/set_speed_slider_fraction.hpp" #include "ur_msgs/srv/set_payload.hpp" #include "rclcpp/time.hpp" @@ -79,6 +80,7 @@ enum CommandInterfaces ZERO_FTSENSOR_ASYNC_SUCCESS = 32, HAND_BACK_CONTROL_CMD = 33, HAND_BACK_CONTROL_ASYNC_SUCCESS = 34, + ANALOG_OUTPUTS_DOMAIN = 35, }; enum StateInterfaces @@ -122,6 +124,9 @@ class GPIOController : public controller_interface::ControllerInterface private: bool setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs::srv::SetIO::Response::SharedPtr resp); + bool setAnalogOutput(ur_msgs::srv::SetAnalogOutput::Request::SharedPtr req, + ur_msgs::srv::SetAnalogOutput::Response::SharedPtr resp); + bool setSpeedSlider(ur_msgs::srv::SetSpeedSliderFraction::Request::SharedPtr req, ur_msgs::srv::SetSpeedSliderFraction::Response::SharedPtr resp); @@ -161,6 +166,7 @@ class GPIOController : public controller_interface::ControllerInterface rclcpp::Service::SharedPtr hand_back_control_srv_; rclcpp::Service::SharedPtr set_speed_slider_srv_; rclcpp::Service::SharedPtr set_io_srv_; + rclcpp::Service::SharedPtr set_analog_output_srv_; rclcpp::Service::SharedPtr set_payload_srv_; rclcpp::Service::SharedPtr tare_sensor_srv_; diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index d5e6d16ee..188542d0e 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -97,6 +97,8 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_cmd"); config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_async_success"); + config.names.emplace_back(tf_prefix + "gpio/analog_output_domain_cmd"); + return config; } @@ -293,6 +295,9 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre program_state_pub_ = get_node()->create_publisher("~/robot_program_running", qos_latched); set_io_srv_ = get_node()->create_service( "~/set_io", std::bind(&GPIOController::setIO, this, std::placeholders::_1, std::placeholders::_2)); + set_analog_output_srv_ = get_node()->create_service( + "~/set_analog_output", + std::bind(&GPIOController::setAnalogOutput, this, std::placeholders::_1, std::placeholders::_2)); set_speed_slider_srv_ = get_node()->create_service( "~/set_speed_slider", @@ -357,7 +362,7 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs: command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING); command_interfaces_[CommandInterfaces::ANALOG_OUTPUTS_CMD + req->pin].set_value(static_cast(req->state)); - RCLCPP_INFO(get_node()->get_logger(), "Setting analog output '%d' to state: '%1.0f'.", req->pin, req->state); + RCLCPP_INFO(get_node()->get_logger(), "Setting analog output '%d' to state: '%f'.", req->pin, req->state); if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value(); })) { RCLCPP_WARN(get_node()->get_logger(), "Could not verify that io was set. (This might happen when using the " @@ -385,6 +390,46 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs: } } +bool GPIOController::setAnalogOutput(ur_msgs::srv::SetAnalogOutput::Request::SharedPtr req, + ur_msgs::srv::SetAnalogOutput::Response::SharedPtr resp) +{ + std::string domain_string = "UNKNOWN"; + switch (req->data.domain) { + case ur_msgs::msg::Analog::CURRENT: + domain_string = "CURRENT"; + break; + case ur_msgs::msg::Analog::VOLTAGE: + domain_string = "VOLTAGE"; + break; + default: + RCLCPP_ERROR(get_node()->get_logger(), "Domain must be either 0 (CURRENT) or 1 (VOLTAGE)"); + resp->success = false; + return false; + } + + if (req->data.pin < 0 || req->data.pin > 1) { + RCLCPP_ERROR(get_node()->get_logger(), "Invalid pin selected. Only pins 0 and 1 are allowed."); + resp->success = false; + return false; + } + + command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING); + command_interfaces_[CommandInterfaces::ANALOG_OUTPUTS_CMD + req->data.pin].set_value( + static_cast(req->data.state)); + command_interfaces_[CommandInterfaces::ANALOG_OUTPUTS_DOMAIN].set_value(static_cast(req->data.domain)); + + RCLCPP_INFO(get_node()->get_logger(), "Setting analog output '%d' to state: '%f' in domain %s.", req->data.pin, + req->data.state, domain_string.c_str()); + + if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value(); })) { + RCLCPP_WARN(get_node()->get_logger(), "Could not verify that io was set. (This might happen when using the " + "mocked interface)"); + } + + resp->success = static_cast(command_interfaces_[IO_ASYNC_SUCCESS].get_value()); + return resp->success; +} + bool GPIOController::setSpeedSlider(ur_msgs::srv::SetSpeedSliderFraction::Request::SharedPtr req, ur_msgs::srv::SetSpeedSliderFraction::Response::SharedPtr resp) { 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 f7167e793..51d9d48e8 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -177,6 +177,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface // asynchronous commands std::array standard_dig_out_bits_cmd_; std::array standard_analog_output_cmd_; + double analog_output_domain_cmd_; double tool_voltage_cmd_; double io_async_success_; double target_speed_fraction_cmd_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 2852df1b4..f99bcc270 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -301,6 +301,8 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "gpio", "standard_analog_output_cmd_" + std::to_string(i), &standard_analog_output_cmd_[i])); } + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "gpio", "analog_output_domain_cmd", &analog_output_domain_cmd_)); command_interfaces.emplace_back( hardware_interface::CommandInterface(tf_prefix + "gpio", "tool_voltage_cmd", &tool_voltage_cmd_)); @@ -677,6 +679,8 @@ void URPositionHardwareInterface::initAsyncIO() standard_analog_output_cmd_[i] = NO_NEW_CMD_; } + analog_output_domain_cmd_ = NO_NEW_CMD_; + tool_voltage_cmd_ = NO_NEW_CMD_; payload_mass_ = NO_NEW_CMD_; @@ -706,7 +710,13 @@ void URPositionHardwareInterface::checkAsyncIO() for (size_t i = 0; i < 2; ++i) { if (!std::isnan(standard_analog_output_cmd_[i]) && ur_driver_ != nullptr) { - io_async_success_ = ur_driver_->getRTDEWriter().sendStandardAnalogOutput(i, standard_analog_output_cmd_[i]); + urcl::AnalogOutputType domain = urcl::AnalogOutputType::SET_ON_TEACH_PENDANT; + if (!std::isnan(analog_output_domain_cmd_) && ur_driver_ != nullptr) { + domain = static_cast(analog_output_domain_cmd_); + analog_output_domain_cmd_ = NO_NEW_CMD_; + } + io_async_success_ = + ur_driver_->getRTDEWriter().sendStandardAnalogOutput(i, standard_analog_output_cmd_[i], domain); standard_analog_output_cmd_[i] = NO_NEW_CMD_; } }