VelocityJointInterface unstable due to huge delay

[miguelprada]

As suggested by @ThomasTimm, I'm trying to use the VelocityJointInterface in the driver instead of PositionJointInterface. However, when starting the robot with the velocity-based joint trajectory controller, the robot quickly goes unstable, even without sending any command at all (the robot should try to keep the initial position via the PID in velocity_controllers::JointTrajectoryController, see here). Note that I'm not using any custom code here, just launching

$ roslaunch ur_modern_driver ur10_ros_control.launch

I modified the UR driver to broadcast more information in the joint state message (see this specific revision of ur_hardware_interface.cpp in my fork for details). The additional data I'm publishing is:

• target joint positions/velocities/currents as reported by the UR feedback messages
• target joint velocity as produced by the joint trajectory controller

What I observe is that there's a huge delay between the command written by the joint trajectory controller and the target velocity reported by the UR, as shown in the figure below, where the red line is the output of the controller, the light blue line is the target joint velocity as reported by the UR (i.e. what's returned by RobotStateRT::getQdTarget()), and the dark blue line is the actual joint velocity as reported by the UR driver (i.e. what's returned by RobotStateRT::getQdActual()).

I've tried with different controllers and lower PID gains (see here for more info), but the robot always goes unstable eventually. Additionally, the delay seems to increase as the driver runs. Lowering the PID gains makes the system go unstable more slowly, and after 40-50 seconds without any command (i.e. just holding the position) the delay between the controller command and the target speed reported by the UR grows beyond 2 seconds, as shown in the figure below.

I'm posting this here to see if anyone has had the same issue or can think of something I might be missing, since this is driving me crazy. @ThomasTimm do you actually work using the ros_control flavour of the driver or do you use it in the sans-ros_control mode? Have you seen anything like this before?

[ThomasTimm]

I haven't seen this kind of behaviour before, no.
Seeing that the difference is growing unbounded, I would guess that either the rate of your controller is wrong (you should be publishing commands at 125 Hz or slower as the controller buffers velocity commands), or the speed have been lowered on the robot's teach pendant.

[miguelprada]

I'm pretty sure the controllers are updated at 125Hz. As I understand by reading the code, RosWrapper::rosControlLoop() will run synchronized with the messages received from the RT interface by checking the controller_updated_ flag. Is this right? In any case, the rate with which the joint state messages are published confirm this (i.e. they're published at 125Hz).

One thing that's bothering me is the time parameter in the speedj(qd,a,t) URScript command, which is underlying the ros_control velocity interface mode. Do you think it might be possible that by being set to 20ms, the calls to speedj might be piling up on the queue and creating the delay I observe between the controller command and the target velocity reported by the UR through the RT interface?

According to the script manual:

The time t is optional; if provided the function will return after time t, regardless of the target speed has been reached. If the time t is not provided, the function will return when the target speed is reached.

So, if a new speedj command is sent every 8ms, but each speedj command runs for 20ms, this could explain a lot.

However, I must say that I've tried to reduce the value to 8ms and the observed delay still seems to grow, with the added problem that the target speed reported by the controller sometimes drops down to zero (it would seem that's because there's no new speedj command to process after the current is finished, but that doesn't add up with the theory of a growing queue of commands).

I'm really running out of ideas to try, so any suggestion will be very much appreciated.

One detail that might be important is that our robots are running URControl version 3.2.18744.

[aaqibkhanunis]

MiguelPrada,

Hi,

I am having the same problems as you, and I am also using the same

$ roslaunch ur_modern_driver ur10_ros_control.launch

I just launch it and my UR10 goes crazy. I haven't been able to figure out what is going wrong. I have not been able to make any plots to detect any delays, but the situation is the same. Starting the velocity-based joint trajectory controller, the robot quickly goes unstable, and I haven't made any changes in any of the files, including "ur_hardware_interface.cpp". Now I cannot account for this weird behavior of the robot. do you think that, If I change the controller to position control, the behavior might change. I have not done it coz I am fearing something might go wrong and I don't want to damage the Robot. Can you think of any solution. Is this problem coming up with all of kind of controllers for the UR arms, or just this one.

ThomasTimm,

I am having an intuition that the UR-10 arms we have, they might no have been configured to receive the commands from a ROS interface. do we ave to configure anything viz the teach pendant..??

Please do update as my work is stuck.

[miguelprada]

@aaqibkhanunis So far I'm getting better results by switching to position controllers, yes, although then I have the overshoot problem as stated in #47. The overshoot is not very big when moving at slow-ish velocities, but increasing the speed degrades performance quite a lot. You can try this by switching vel_based_pos_traj_controller for pos_based_pos_traj_controller, and viceversa, in ur10_ros_control.launch.

With respect to the velocity interface, it seems that the behaviour is best when removing the optional t parameter in the speedj command in ur_realtime_communication.cpp. I still need to run some more proper tests to confirm this.

In any case, it really looks like this is due to some change in the UR controller, rather than some bug in the driver, don't you think @ThomasTimm? Some modified behaviour in SW version 3.2, maybe?

[ThomasTimm]

@aaqibkhanunis Unlike other arms, like the Motoman, the UR doesn't need any special setup, as it only relies on URScript commands. So as long as your network is setup properly, you should be good to go.

Yes, the driver's frequency is controlled by the UR's controller, as they probably doesn't agree 100% as to how long 8ms is. I was more curious about the ros_controller and what was in your urX_controller.yaml file (there are several places that should read 125hz, has this been changed while tweaking the controller/changing between position and velocity control?). Maybe try lowering this value to 120hz?
You can also use the rostopic hz-command to see how often ros_control sends a new command.

When I initially developed this improved driver, me and some students of mine tested all the available URScript commands to find which could be used for external control.
Furthermore, I extensively tested the speedj and how the t-parameter affected performance. I concluded that whenever the robot receives a new command while executing another command, it stops the execution and starts handling the new command. Thus, the value of t didn't really matter, as long as it was higher than the time between sending new commands (as you have noticed, if t is 8ms or smaller, it might stop moving in-between receiving new commands).
This was all done on a robot running firmware 1.8; unfortunately I don't have access to a robot with a controller capable of being upgraded to firmware 3.2, so I can't definitively say whether they have changed anything in 3.2. But it would surprise me if they have changed this fundamental behavior.

The way I tested it, and I would recommend that you @miguelprada do your test, is to write a simple python script that sends a new speedj command whenever it receives a status message from the robot. Make sure that the speedj value changes each time you send it (i.e. a sine-wave formed velocity profile). Then, try different values of t and plot the velocity you send, the target velocity form the robot and the actual velocity.

[miguelprada]

I'm afraid you should be surprised then @ThomasTimm, just as I am 😉.

I've run a couple of tests following your suggestion and the results are quite conclusive: the value of t in the speedj command does matter (in version 3.2.18744, at least).

The two figures below show the result of commanding a sinusoidal velocity profile (red line in the figures) with t = 0.02 as in the default version of your driver, and with no value of t provided. The velocity commands are reproduced (blue line in the figures) with a growing delay in the first case and quite precisely on the second. I'll prepare a PR with a small fix ASAP.

Of course, it was this delay which was making the PID controlled version unstable (as shown in the original comment), just as the theory predicts.

[gavanderhoorn]

I quickly checked the release notes for all v3.2 releases, but can't find this mentioned anywhere.

[miguelprada]

@gavanderhoorn Search for speedj in these release notes.

You should find the following text:

• *Updated the functionality of the speedj and speedl script command to:
  • Accelerate linearly in joint space and continue with constant joint speed.
  • The time t is optional; if provided the function will return after time t, regardless of the target speed has been reached.
    If the time t is not provided, the function will return when the target speed is reached.
  • Fixed the stopj and stopl script command to stop with the specified acceleration.
  • Fixed the speedl and stopl script command so the acceleration of the orientation and position speed are synchronized

The asterisk in the first bullet means these changes were introduced in SW 3.1.

[gavanderhoorn]

Right. Seems I missed that then. Would've been nice if that had been more prominently documented, it's quite a change from all previous behaviour.

Thanks for the reference.

[ThomasTimm]

Thanks for sorting this out @miguelprada !

[miguelprada]

My pleasure, @ThomasTimm.