Dangerous Robot Behavior When Resuming External Control After Manual Jogging

[ShahhhVihaan]

Affected ROS2 Driver version(s)

2.4.8

Used ROS distribution.

Rolling

Which combination of platform is the ROS driver running on.

Ubuntu Linux with standard kernel

How is the UR ROS2 Driver installed.

From binary packets

Which robot platform is the driver connected to.

UR CB3 robot

Robot SW / URSim version(s)

3.15.8

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

Summary

I’m using ROS2 24.04, ROS2 Jazzy, and the 2.4.8 driver version from binary packages to control two UR3 robots in the same environment. The setup involves using a single control manager to start all controllers, and the arms can be controlled successfully using MoveIt. Here's the draft PR I wrote for the tutorials repository: UniversalRobots/Universal_Robots_ROS2_Tutorials#3

However, I’m encountering a critical issue when pausing the external control program on the pendant, manually jogging the arm, and then pressing play again. The robot rapidly moves back to its previous position with the speed scaling set to 100%, which is extremely dangerous. To mitigate this, I’ve been setting the speed scaling to below 10% and am always ready to hit the emergency stop.

Occasionally, after starting the external program, the robot moves to an unexpected position, possibly due to leftover processes from the previous ROS2 control session not fully shutting down. For safety, I stop the driver launch before jogging the arm, then restart the driver afterward.

Steps to Reproduce

1. Use the draft pull request here with your choice of UR arms, IPs, and Ports.
2. Set the robot’s speed scaling to a low value (below 10%).
3. Pause the external control program on the pendant, manually jog the arm, and press play.
4. Observe the robot’s behavior when resuming the program.

Expected Behavior

The robot should either maintain its current position or move safely and smoothly to the new commanded position after jogging, regardless of speed scaling.

Actual Behavior

The robot moves rapidly to its previous position, with speed scaling set to 100%, creating a hazardous situation. On rare occasions, the robot moves to an incorrect position after restarting the external control program.

Request for Assistance

I’ve reviewed the launch files and parameters but cannot pinpoint the cause of this issue. I would appreciate any guidance or suggestions on how to resolve this dangerous behavior.

Relevant log output

No response

Accept Public visibility

• I agree to make this context public

[fmauch]

Hi, thank you for reporting this. And also, thank you for providing the tutorial. It is not forgotten, time resources are always rare...

The problem you described have been reported before, e.g. in #520 but I have never been able to really reproduce this.

From what I understand, that problem could come from the controller not being stopped when the external_control program on the robot is being stopped. What should happen is that as soon as you jog the robot on the TP, the external control program gets interrupted. This should be reported back to the robot driver which will publish on the /io_and_status_controller/robot_program_running (in your case the full path will contain alice or bob) which should trigger the controller_stopper to shut down all controllers except the ones in the persistent_controllers list. If this step doesn't happen, the controller will continue sending the hold position command. Once you turn external_control back on, this command will be sent and used by the robot.
When the controller is stopped correctly, it will read the robot's current configuration upon activation which will avoid moving back to the previous position.

In order to verify that this is the problem, you can

1. watch the robot_program_running topic when you start jogging and re-enable external_control
2. use ros2 control list_controllers to see all controllers with their state. The controller controlling the robot's motion should switch between active and inactive state accordingly.

We currently do have multiple attempts to avoid this:

• Add a check in the external_control program, and simply ignore "jumps" in the commands being sent (See Added check to ensure that the targets are reachable within the robot… Universal_Robots_Client_Library#184)
• Allow setting the hardware interface being set back to "configured" state from it's read method which will allow us to drop the hardware interface into a state where also the controllers will be shut down directly without the help of the controller_stopper. This is partly implemented but I don't recall the complete state where we are at the moment... As far as I remember, I implemented dropping the state in ros2_control, but I haven't updated the driver, yet, partly because of taking down the controllers / making sure that the correct controllers run in configured state was not working as expected.

I hope this helps clarifying the situation a bit.

[firesurfer]

I can confirm this issue. It can also happen with the resend_robot_program service call. My guess is that the hardware interface sends the last commanded position on "reconnect" - but I never debugged it.

I have solved it in our environment by having a node which monitors the system and disables all controllers that can move anything in the system in case certain conditions are met. (basically the same what the controller stopper does).

As a workaround btw. if you want to manually move the arm you could use the rqt joint trajectory controller plugin. This way you can avoid stopping the program on the UR.

[Hytac]

We encountered the exact same issue.

Take a look at the hardware_interface.cpp. Let’s focus on these three callbacks:

hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::Time& time, const rclcpp::Duration& period)
hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp::Time& time,const rclcpp::Duration& period)
hardware_interface::return_type URPositionHardwareInterface::perform_command_mode_switch(const std::vector<std::string>& start_interfaces, const std::vector<std::string>& stop_interfaces)

Read is periodically retrieving the status of the robot (positions, currents, IO, etc.).

Write is periodically writing the last urcl_position_commands_. It doesn't matter if you manually jog the UR; it will keep writing the last received urcl_position_commands_ from ro2control. That's why, as soon as you enable the external control, the robot moves back to the previous position, which is unexpected and risky for the end user.

The workaround

This is the perform_command_mode_switch callback:

hardware_interface::return_type URPositionHardwareInterface::perform_command_mode_switch(
    const std::vector<std::string>& start_interfaces, const std::vector<std::string>& stop_interfaces)
{
  hardware_interface::return_type ret_val = hardware_interface::return_type::OK;

  if (stop_modes_.size() != 0 &&
      std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_POSITION) != stop_modes_.end()) {
    position_controller_running_ = false;
    urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_;
  } else if (stop_modes_.size() != 0 &&
             std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_VELOCITY) != stop_modes_.end()) {
    velocity_controller_running_ = false;
    urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
  }

  if (start_modes_.size() != 0 &&
      std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_POSITION) != start_modes_.end()) {
    velocity_controller_running_ = false;
    urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_;
    position_controller_running_ = true;

  } else if (start_modes_.size() != 0 && std::find(start_modes_.begin(), start_modes_.end(),
                                                   hardware_interface::HW_IF_VELOCITY) != start_modes_.end()) {
    position_controller_running_ = false;
    urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
    velocity_controller_running_ = true;
  }

  start_modes_.clear();
  stop_modes_.clear();

  return ret_val;
}

This is what makes this workaround work:
urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_;

After any command switch, the last urcl_position_commands_ is set to urcl_joint_positions_, which is the last read (the jogging position). So, what we do is simply switch to inactive when working locally, and switch back to active when going back to external control. Our state machine is in charge of the behavior. Hope it helps.

It would be nice to have a formal solution for this from the driver developers.