Adding reset revolution counter service (#325)

[cambel]

Hi,

I am trying to implement the service to reset the revolution counter of the robot's wrist_3_link. However, I am having a problem with the behavior of the script.

If I send the script as a secondary program:

sec resetRevolutionCounter():
  reset_revolution_counter()
end

It does not work as intended, the wrist_3_link gives a full rotation at full speed, which is not desirable at all, but on top of that, the counter is not reset.

On the other hand, if I send the script just as:

reset_revolution_counter()

It does work as intended with the caveat that the driver's program is stopped and has to be reconnected in order to send any new command.

I think that the best solution would be to be able to reset the counter without disconnecting the main driver's program. Similarly to the behavior of the zero_ftsensor service.

I would appreciate any suggestions on what I may be doing wrong.

[fmauch]

First of all, thanks for the PR!

Given your description the script function is indeed able to be run in a secondary function which would definitely be a good approach.

The problem is that the hardware interface sends the current joint command to the robot each control cycle. If that joint happens to be a full revolution off the current command, it will do a full rotation to get to the old configuration.

So, in order to prevent this, one could modify the joint_position_command_ buffer to reflect the new configuration. This has multiple caveats:

• As the control loop runs in a different thread than the service handler you'll have to deal with thread synchronization. And the control thread should not have to wait for anything...
• Timing will be an issue here. When you send the reset_revolution_counter() to the robot you don't know exactly when this will actually happen. Any misalignment between setting the joint_position_command_ buffer and the joint actually reporting being reset will result in a jerk in that joint.
• What happens, if there is actually a command (trajectory goal) running when that happens. Modifying the output buffer will be overwritten by the trajectory almost instantaneously.

While the first two problems could probably be mitigated by doing numerous workarounds, the third one would actually remain a problem, but can also be seen as a hint to another possible solution.

If we shutdown all controllers actually moving the robot, then reset the counter and then restart the controller that has been running (maybe with a short delay to let the reset command do its job) the system will

• not move due to the reset, as no joint commands will be sent, basically, we will end up in this line inside the write() function.
• Any trajectories running at that point will be aborted and new trajectories will be rejected until the controller is running again.

The easiest way to achieve this would be to use the already running controller stopper. This could be done by simply publish a message to the robot_program_running topic as done here. Problem with that: This is meant to be only triggered from one source which is the driver's ReverseInterface that will check whether the corresponding URScript is active on the robot. As it will still remain active at that point, we are hijacking something not meant that way. We would have to find a way to clean that up properly.

However, as a proof of concept, this should work. While the reset is taking place, all "controlling" controllers (All except those) should get stopped temporarily. The control_mode variable on the teach panel should become 0 during the operation and then go back to the value it was before.

[cambel]

Thank you for the explanation!

I tried the approach that you mentioned, stopping the controllers > sending the script > restarting the controllers. I tested it and it worked. I had to add a delay after sending the script to allow the command to be processed before the controllers were back running.

[fmauch]

OK, great! So now we need to find a way to make this less hacky and prune to errors.

My main concern is that this behavior is also controlled by HardwareInterface::handleRobotProgramState.

One way around this might be to call this function directly instead of copying the contents and add a mutex to that function. This is necessary, as the service callback will be triggered by the async spinner while the changing program state will be triggered by the Reverse interface watchdog (which runs in a separate thread).

Oh, and maybe it's about time to format your code according to the style guidelines :-) We use clang-format in order to to that automatically. I usually use this script to format a complete repo:

#!/bin/bash
# This script uses clang-format to format all *.cpp, *.hpp and *.h files recursively

clang_format_exec="clang-format"

find . -name "*.cpp" -exec sh -c "echo 'Formatting {}' && $clang_format_exec -i {}" \;
find . -name "*.hpp" -exec sh -c "echo 'Formatting {}' && $clang_format_exec -i {}" \;
find . -name "*.h" -exec sh -c "echo 'Formatting {}' && $clang_format_exec -i {}" \;

[cambel]

Thank you for the formatting script!

I made the suggested changes, I am not sure if I added the mutex as intended though. I tested it and it works.

This is the behavior of the function when tested:

• If the angle of the wrist3 is bigger than 2PI or less than -2PI: The wrist3 moves slightly to some position and the revolution counter is reset. Though I am not sure why it moves.
• If the angle of the wrist3 is within [-2PI, 2PI]: The wrist3 does not move and the revolution counter does not change.

[urrsk]

* If the angle of the wrist3 is bigger than 2_PI or less than -2_PI: The wrist3 moves slightly to some position and the revolution counter is reset. Though I am not sure why it moves.

Thanks for that insight, I will try to get it verified

[urrsk]

Just want to clarify for the future reader that it is only the UR3 that will benefits from this.

Suggested change

	Calling this service will reset the revolution counter for the robot's wrist_3_link. Note: On e-Series robots this will only work when the robot is in remote-control mode.
	The UR3 and UR3e's wrist 3 can do infinite rotations and are therefore not limited to ±2Pi.
	Calling this service will reset the revolution counter for the robot's wrist_3_link. Note: On e-Series robots this will only work when the robot is in remote-control mode.
	This is handy to avoid rotating back after e.g. a screwing task, and thereby avoid compensating for this in the program.

[gavanderhoorn]

Might be nice to make this even stronger and have the service fail if the robot is not a UR3.

[cambel]

I suppose something like this:

bool HardwareInterface::resetRevolutionCounter(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res)
{
  if (ur_driver_->getName() == 'UR3e' || ur_driver_->getName() == 'UR3')
  {
    std::mutex mtx;
    handleRobotProgramState(false);
    mtx.lock();
    res.success = this->ur_driver_->sendScript(R"(sec resetRevolutionCounter():
    reset_revolution_counter()
  end
  )");
    mtx.unlock();
    handleRobotProgramState(true);
  }
  else
  {
    std::stringstream ss;
    ss << "Resetting the revolution counter is only available for UR3/e robots. This robot is "
       << ur_driver_->getName();
    ROS_ERROR_STREAM(ss.str());
    res.message = ss.str();
    res.success = false;
  }
  return true;
}

Though, I don't know where to get the robot's name...

[fmauch]

Out of my head I am not sure whether there's something predefined to get the robot model through the API. But that would have been my next point on the list.

Edit: Currently, this is not available through the API. Receiving it through the dashboard client would be a workaround to not further postpone this PR. Alternatively it could be received through the primary interface, but that is currently not implemented.

@gavanderhoorn wrote:

Might be nice to make this even stronger and have the service fail if the robot is not a UR3.

I would even have supposed to advertise it only for UR3(e) robots. But that might be more confusing than simply letting the service fail...

[fmauch]

• If the angle of the wrist3 is bigger than 2_PI or less than -2_PI: The wrist3 moves slightly to some position and the revolution counter is reset. Though I am not sure why it moves.

Thanks for that insight, I will try to get it verified

@cambel Just to make sure: Is this independent of using with the ROS driver?

[cambel]

I am not sure. I've only tested it using the ROS driver.
I will do some tests next week using just plain sockets connection.
What I know is that the slight motion of the wrist3 is not always happening, though today it happened all the time.

[cambel]

Okay, I did some more tests.

Using the URScript function resetRevolutionCounter() do not move the robot, it only resets the revolution counter when the angle of the wrist3 is bigger than 2_PI or less than -2_PI.

Edit: The revolution counter is reset when the wrist3 angle is outside the range [0, 2_PI]

The slight motion that I have been observing may be due to the slight delay added to the function ros::Duration(0.1).sleep();. I supposed that the controllers are not stopped before the URscript is called so the revolution counter is reset, then the controller tries to go to its latest know configuration for a slipt second before the controllers are stopped and resumed.

The problem is as @fmauch explained above, about the timing of the threads. I tried to implement the mutex but I've been unsuccessful to make it work properly (I have no experience doing that). No matter what I do, it does not seem to lock the resources so that the URscript function is executed only when the controllers are stopped and before they are resumed.

I'd appreciate some guidance on this part.

[fmauch]

I'd like to nail things down to the actual problem:

Could you add sleeps (as in 1 sec) before [exclusive] or after calling the URScript function? Is the problem that the controllers aren't going down fast enough or coming up too early?

The problem with our current setup is that we have a (kind of hacky) fire-and-forget interface to stopping & starting the controllers. If we can nail down the problem, we can think of a more proper solution.

[cambel]

@fmauch I will do more tests to try to understand how the driver is trying to execute the stopping & starting the controllers.

However, last time I did already try adding sleep of 1 second before/after calling the URScript, the result was the spin behavior I mentioned in the first commend of this thread. I even try only stopping the controller and then executing the URScript, but it goes back to working sometimes only when the functions sync properly I guess. After that, the controllers were running again without explicitly starting them again.

[urrsk]

Maybe the best solution to do this is to subtract an angle offset in the driver, instead of trying to make a async. call sync. Or add it to the sync protocol.

[github-actions]

This PR hasn't made any progress for quite some time and will be closed soon. Please comment if it is still relevant.