universal robots package, servoj() at 125Hz

[Antoine Rennuit]

Hello all,

From reading the ur_driver.py code in branch indigo-devel of the universal robot package (https://github.com/ros-industrial/universal_robot/blob/indigo-devel/ur_driver/src/ur_driver/driver.py), it seems possible to send servoj() commands at 125Hz to the UR arm.

Has anyone actually already tried sending servoj() commands at 125Hz? For example in order to send a smooth path of desired joint angles calculated from a FollowJointTrajectory?

I am having a hell of a time trying to implement a bear (i.e. outside of ROS or ROS-I) TCP/IP client to connect to the URController and have the joints follow a smooth trajectory (and the UR support turns out to be... not supportive :( ). My resulting movement is smooth most of the time but there is a bad jerk every 2 seconds or so (videos here and here), hence I am looking for inspiration. And if you guys have managed to achieve a smooth movement at 125Hz (as it seems from the code), I should not reinvent the wheel.

But before using the ROS-I universal robot package rather than my code, I need to make sure this package can follow a joint trajectory at 125Hz smoothly... Let me know about your experience! I am also interested in knowing whether you have used any "trick" to have your communication working via the RT Client Interface on port 30003.

Thanks,

Antoine.

[Antoine Rennuit]

Also, do you know why there is a C-API version of the controller if you can already achieve 125Hz with URScript?

Thanks!

[Thomas Timm Andersen]

I've tried using the servoj() command at 125 Hz back in 2013, but without much success. I had the same issues as you notice in the video. I discussed the issue with Kelsey Hawkins at Roscon 2013, and his  development of the C-API version is a result of this.

 

I would recommend you take a look at a recent technical paper I’ve made with some students of mine called “UR10 Performance Analysis” (http://orbit.dtu.dk/en/publications/ur10-performance-analysis(0deaebd1-1593-4641-a971-e1ac0234ed88).html ) – despite its name, it is applicable to UR 5 and UR 10 alike, as it focuses on the software side. The paper also looks into some network related issues.

 

In short, I would recommend you use the speedj() command instead – in my experience, it is the only command that works reliably and consistently at 125 hz (except if you have an very old version of the ur5 with the 1st generation of the controller. In that case, no network control works reliably due to a memory leak in the network buffer. But that doesn’t seem to be your problem).

As joint speed control leads to positional drifting it requires that you make a feedback controller if you need accurate positional control, as the speedj command only takes… -well, joint speeds.

 

Regards

 

Thomas Timm Andersen PhD student DTU Electrical Engineering Technical University of Denmark Department of Electrical Engineering Elektrovej Building 326 DK - 2800 Kgs. Lyngby Denmark Direct +45 45253556 Mobile +45 20821983 tt...@elektro.dtu.dk www.elektro.dtu.dk/

--
You received this message because you are subscribed to the Google Groups "swri-ros-pkg-dev" group.
To unsubscribe from this group and stop receiving emails from it, send an email to swri-ros-pkg-d...@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Shaun Edwards]

Thomas,

Thanks for sharing the report.  It includes lots of useful information.  I also think the test method might be applied to other robot vendor equipment as well.

-Shaun

[Antoine Rennuit]

This is a very interesting reading. Thanks!

It seems that the report is all about the RT Client Interface (Matlab 125Hz). Do you know if the observations and conclusions you made can be extended to the Secondary Client Interface (on port 30002)?

Regards,

Antoine.

[Maarten de Vries]

On 18 May 2015 at 09:59, Thomas Timm Andersen <tt...@elektro.dtu.dk> wrote:

I've tried using the servoj() command at 125 Hz back in 2013, but without much success. I had the same issues as you notice in the video. I discussed the issue with Kelsey Hawkins at Roscon 2013, and his  development of the C-API version is a result of this.

 

I would recommend you take a look at a recent technical paper I’ve made with some students of mine called “UR10 Performance Analysis” (http://orbit.dtu.dk/en/publications/ur10-performance-analysis(0deaebd1-1593-4641-a971-e1ac0234ed88).html ) – despite its name, it is applicable to UR 5 and UR 10 alike, as it focuses on the software side. The paper also looks into some network related issues.

 

In short, I would recommend you use the speedj() command instead – in my experience, it is the only command that works reliably and consistently at 125 hz (except if you have an very old version of the ur5 with the 1st generation of the controller. In that case, no network control works reliably due to a memory leak in the network buffer. But that doesn’t seem to be your problem).

As joint speed control leads to positional drifting it requires that you make a feedback controller if you need accurate positional control, as the speedj command only takes… -well, joint speeds.

​

Very interesting and useful read. Thank you.

The bandwidth with which one can expect to control the robot is determined by the 24ms round-trip time.

Do you think it would be possible to reduce the feedback latency by streaming joint position setpoints to the arm (at hopefully 125 Hz)  and implementing the feedback loop on the arm itself in URScript with speedj commands?

Regards,

Maarten​

[G.A. vd. Hoorn - 3ME]

On 19/05/15 10:05, Maarten de Vries wrote:
> On 18 May 2015 at 09:59, Thomas Timm Andersen <tt...@elektro.dtu.dk> wrote:

[..]

> Very interesting and useful read. Thank you.
>
>> The bandwidth with which one can expect to control the robot is determined
>> by the 24ms round-trip time.
>>
>
> Do you think it would be possible to reduce the feedback latency by
> streaming joint position setpoints to the arm (at hopefully 125 Hz) and
> implementing the feedback loop on the arm itself in URScript with speedj
> commands?

I would expect that could work, but would need some buffering on the
controller itself due to the influences of Nagle on the TCP connection.

Perhaps UR should move to UDP for the 'rt' broadcasts on 30003.


Gijs

[Maarten de Vries]

On 19 May 2015 at 10:10, G.A. vd. Hoorn - 3ME <g.a.van...@tudelft.nl> wrote:

On 19/05/15 10:05, Maarten de Vries wrote:

On 18 May 2015 at 09:59, Thomas Timm Andersen <tt...@elektro.dtu.dk> wrot

​e:​

The bandwidth with which one can expect to control the robot is determined
by the 24ms round-trip time.

Do you think it would be possible to reduce the feedback latency by
streaming joint position setpoints to the arm (at hopefully 125 Hz)  and
implementing the feedback loop on the arm itself in URScript with speedj
commands?

I would expect that could work, but would need some buffering on the controller itself due to the influences of Nagle on the TCP connection.

Perhaps UR should move to UDP for the 'rt' broadcasts on 30003.

​Hmm, buffering would increase reaction time again, so that would not be very desirable. I do believe Nagle can be disabled on the sender side with the TCP_NODELAY socket option. That might be sufficient.

--

​ Maarten​

[G.A. vd. Hoorn - 3ME]

I think I was a bit too quick (should've read the rest first), but I
think the results in section 2.3 show that the 24ms is largely
influenced by internal (so not by the network) processes. It's not so
much frequency, but latency/time delays it seems.

[Thomas Timm Andersen]

As it is stated in the report, disabling NAGLE on the host computer is sufficient. It is also mentioned, although not very clearly, that replying to every messages will force the robot controller to stream data at 125 hz and not buffer anything. Just replying with a “\n” is sufficient.

 

I’ve submitted at paper for IROS on measuring delay on robot manipulators. As it is still being reviewed, I can’t disclose the results yet, but one thing is clear on the UR: The time from a motion is carried out, until this is reflected in the data the robot transmit, is less than 8 ms. That is, the joint angles seems to be sampled very close ( < 2ms) to the time the values are transmitted. (Surprisingly, this is not the case for all robots)

 

Maarten: Implementing the feedback in URScript on the controller is not a bad idea, but I am not sure if it is doable.

At first, let me explain why we have this “big” delay. It took me some month to realize, but it is actually difficult to make it much faster.

 

At t=0, the robot controller transmit its current status (position, speed, target etc.).

This is read by the host computer shortly after, let’s assume at t=1 ms

According to the report, you now have up to 6 ms to do your control stuff and transmit a new instruction. Let’s assume you are a bit faster than this and transmit a new target at t=4ms

At t=8ms, the robot reads this, and transmit a new status. In this status, it tells us it has a certain target. So it would have to move towards that target in the control period in the time span, t=8ms to t=16ms.

At t=16ms, the robot transmit yet another status. In this status, the target is whatever we transmitted at t=4ms.

The robot then starts actually executing whatever we transmitted, and in the status package transmitted at t=24ms the position is now what we commanded at t=4ms (Giving that it can be reached in 8ms).

 

It should be noted that since we don’t have access to the firmware, determining when one control cycle ends and another begins is purely guesswork, although looking at the C-API can give a hint. Any function that actually commands the joint servos is said to be blocking for the rest of the control cycle. But obviously the servos are already doing something, so what actually must happen is that a call to these functions will block, and then at the start of the next control cycle (when the C program and the thread that controls the servos synchronize) these values are written to the servo controller.

My guess is that pseudo-code for the controller is

 

Set joint vel to 0

Read internal state of joints

Send joint vel to servo controller

While (1) {

   Transmit internal state, and current target being pursued, to network socket

   Sleep(xx) to minimize blocking time

   Read instruction from network socket

   Parse instruction

   Read internal state of joints for position controller

   Calculate new joint vel

   Send new joint vel to servo controller and block

}

 

Where xx is a low value, like 0.5-1 ms. This would explain why sometimes transmitting a new instruction as soon as we received a status only yields an 8ms delay instead of the usual 16ms (The mentioned best-case scenario in section 2.3). We would have liked that delay to be a bit higher so we could reliably see an 8ms delay, but as there might be several instructions to parse (I/O, Modbus, configuration, etc.), I don’t blame the folks at UR for choosing this value conservatively. In the above timing explanation, I’ve assumed xx to be zero for simplicity.

 

 

The reason I doubt that it is possible to do it faster in URScript is because some of the functions must be blocking as well: I.e. how does the “t” parameter in the speedj-command work, how about reading from a socket (which would be in a separate thread) and reading the internal values. All these unknown makes it difficult to optimize the current solution, and the benefit will at most be 8 ms. Also remember that the 24 ms is the round-trip time. It only takes 16 ms from a command is sent until the robot start to execute it.

 

 

Thomas

--

[Maarten de Vries]

​Thanks for the additional explanation. If network latency indeed has little to no influence then there would be no point in implementing the feedback loop in URScript. Either way, I would be interested to see a closed loop controller with speedj commands.

-- Maarten

[jorri...@gmail.com]

I have read your report about the performance of the UR10 robot and since i want to use the robot for real-time control, i tried to replicate the experiments and also test the new commands that were changed with the latest firmware. The move and servo commands seem to perform a lot better, but i noticed some strange things in the delays of the robot and was wondering if you found those too.

1) with or without using TCPNODELAY, i get a package from the robot every 8ms

2) when sleeping after the connection to port 30003 is made, the robot starts buffering its broadcasted messages, so in the same way the commands send to the robot are never skipped the messages send by the robot are never skipped as well.

3) I still get the a package every 8ms, BUT the target velocity does only change after 40ms

both 2) and 3) are very undesirable behaviour. 2) causes delays to build up during a long iteration of speed commands. Because if at some time a package isn't delivered, or late, the speed command is also sent late. The next package however will come earlier, because it is buffered and two speed commands are sent in one 8ms period. This will cause the robot to buffer the speed command and delay the execution of coming speed commands. I have attached a few graphs of the problem. Did you encounter this as well? and is the only workaround to detect when this happens and skip sending the next speed command? Or is there any way i can turn this buffering of the broadcasting off?

3) seems to be a delay caused by the NAGLE algorithm  between the controller and the robot, do you have any idea how i can disable this?

PS. I am working with python on an ubuntu system and the robot is a UR5 with a CB3 with 3.1 firmware.

PPS Attached:

Op dinsdag 19 mei 2015 03:04:22 UTC-7 schreef Thomas Timm Andersen:

[jorri...@gmail.com]

posted before finished:

PPS Attached:

delay.png: green line is the send speed profile, the green squares are the target velocity of the robot at the time they were received. This shows the delay of a package and the quick succession of buffered packages after.

delat_diff.png: this figure shows the time difference between two subsequent packages. The spike shows the delayed packages

delayresult.png: shows the result of this delay. The target velocity only changes 130ms after the speed command is sent.

Op zondag 25 oktober 2015 14:46:10 UTC-7 schreef jorri...@gmail.com:

[Thomas Timm Andersen]

1)      Yes, I have heard that Universal should have fixed the firmware so that TCP_NODELAY isn’t necessary any more. Note, though, that this is something handled at a pretty low level in the hardware stack and handling is probably driver dependent. Therefore I would always recommend using the TCP_NODELAY for timing critical applications.

2)      I’m not sure what you mean by sleeping? The robot doesn’t buffer its messages, they are still being sent. If you don’t read on your socket connecting the messages are buffered on the receiving computer (Try to use Wireshark to monitor the network, the robot will always send data when it has a connection to port 30003. If not, I would like to know how you made it sleep)

3)      That is strange – like I’ve tried to explain earlier, the delay shouldn’t be more than ~24ms. I am pretty sure that it is not due to delay between the controller and the robot, as the controller (which sends the data) is well aware of the target speed.

 

Your figures are a bit difficult to interpret. Axis labeling and legends would help a lot!

It is still not clear to me what you want to achieve, except “real time control”. But why do you need it, and do you mean “real time” in classical CS sense (guaranteed deterministic periods) or “real time” in a more practical way as “real time” is often used in robotics (really fast reaction to changes).

I usually use speedj with a ‘t’ parameter of 0.02, but still send speedj commands at 125 Hz. If you do this without synchronizing with the robot, you would still get smooth execution, even if you have late packages.

I would guess that the reason you sometimes gets late packages is because you don’t use TCP_NODELAY?

 

/Thomas

[Maarten de Vries]

On 26 October 2015 at 09:54, Thomas Timm Andersen <tt...@elektro.dtu.dk> wrote:

 

 

From: jorri...@gmail.com [mailto:jorri...@gmail.com]
Sent: 25. oktober 2015 22:54
To: swri-ros-pkg-dev
Cc: Thomas Timm Andersen; jorri...@gmail.com

Subject: Re: [ROS-Industrial] Re: universal robots package, servoj() at 125Hz

 

1) with or without using TCPNODELAY, i get a package from the robot every 8ms

​Keep in mind that setting TCP_NODELAY only affects the packets you send. It has no influence on local buffering by the other side of the connection. So it is recommendable to set TCP_NODELAY but it won't change the behaviour of the UR controller.

-- Maarten

[Thomas Timm Andersen]

Ahh yes, I just went with what Antoine wrote (although I just noticed I made the same mistake in my new UR driver - so thanks for reminding me of what we wrote in our earlier report).

 

Whenever you receive a package through port 30003, you should set the TCP_QUICKACK flag on that port, not TCP_NODELAY. That way, you should get a steady 8ms between each package. Again, to emphasize, you need to set this flag after each time you call read on your socket. Although if you transmit something each time you receive a package, the previous package would be ACK'ed at that time, so this shouldn't cause the delays you see. Except perhaps if you process the received data before sending your command, and that data processing takes to long (more than a few milliseconds - not unrealistic in python) - so for good measured, always set the TCP_QUICKACK flag.

When you create the socket, setting the TCP_NODELAY flag is also a good idea though, as that prevent your own transmission (write) from being buffered. But this should only be done once when you create the socket.

 

/Thomas

[jorri...@gmail.com]

Thanks both of you for the quick responses and sorry for the bad figures, i'll include some better ones soon. In reply to your first response Thomas:
1) ok that's what i am seeing too indeed.

2) by sleeping i mean the time.sleep() command in python. And indeed what i noticed was that the messages started buffering in my side of the socket. i have a limited understanding of sockets and i hoped that every new message from the robot would replace the old one, but of course that's not the case. Thanks for the explanation.

3) I will upload new figures as soon as i have the chance, but for now i'll try to elaborate some more: 

 - if i send a speed command to the robot, i get a message ever 8ms about the target velocity of the robot. So, the communication between the controller and the computer seems to be fine. However, only after 5 samples after sending the speed change command i can actually see it in the target velocity of the robot.

- Also, sometimes the time between two messages is more than 16ms, or after reading your second point, the time between my computer reading two message from the socket is more than 16 ms. This adds two messages from the robot to my buffer, so the next read of the socket is immediate (well almost), then an immediate speed command send, another immediate read of the socket and another immediate speed command. The robot can't handle the speed commands this quickly, and as you present in your paper, buffers them. So the robot keeps on moving after the program stops. In other words every speed command sent after these speed commands are executed with a delay. This can build up if speed commands are sent every 8ms, which i intend to do.

Now on to what i would like to achieve: i want the robot to move in a certain way, depending on the external forces. So i have a controller which reads the torque and outputs a desired speed. At the moment i am looking at how to get the highest bandwidth, that's why a low delay is important. The guaranteed deterministic periods are not that important and the more practical sense of real time seems more appropriate.

I looked at the gain and the time parameter, setting this higher indeed smoothens the trajectory, but i am not having problems with that. I tried setting it as low as possible, to check if that would fix my delay problems, but it didn't

I use TCP_NODELAY after creating the socket and TCP_QUICKACK after every read. Maybe it's time to start looking at C instead of python, to see if that makes any difference. This is the way i set tcp_quickack

s.setsockopt(socket.IPPROTO_TCP, socket.TCP_QUICKACK, 1)

Again, thanks for all the help

Op maandag 26 oktober 2015 10:38:59 UTC-7 schreef Thomas Timm Andersen:

[jorri...@gmail.com]

made graphs

Op maandag 26 oktober 2015 12:12:53 UTC-7 schreef jorri...@gmail.com:

[Thomas Timm Andersen]

Thanks for the elaborate explanation, it makes much more sense now :)

 

It is still very strange that you see a 5 sample delay, it should only be 2-3. I would recommend that you try capturing some data with Wireshark to verify that this is indeed the case, and not a problem with your code. Alternatively, try to move your robot and your computer on to your own network to rule out that it is not an issue with a lot of traffic on the local network.

 

To deal with the problem of buffering data, I would recommend you to read data in one thread and then do your control loop in a separate thread. That way you are sure nothing is buffered on your end, and your controller always works with the newest data.

You could also have a look at my new modern universal robot driver ( https://github.com/ThomasTimm/ur_modern_driver ), it exposes a topic for sending joint speed commands to the robot. That way you just need to make a controller and not worry about the robot communication part.

Another thing that can cause delays are logging data. Especially if you try to print something can take quite some time in python. I would recommend allocating a large array and the write your data into that, and then at the end of your file write the content of the arrays to the screen or a file.

 

I think moving to c++ would be a good idea if you are looking to shave off a millisecond or two of your control loop execution time... But you set the TCP_QUICKACK flag correctly :-)

 

I'm not sure I get your point about the gain and time parameter, the speedj command doesn't have a gain parameter. Are you using the servoj command instead?

[johnsh...@gmail.com]

Hi Thomas,

I am trying to use speedj commands to control a UR3 (without using ROS).

My question is: What type of trajectory are you generating and what is your feedback controller?

I wanted to use cubic polynomials and give it's velocity directly to speedj but I don't know how the (default) acceleration of speedj interferes with the command.It is always very off.

Regarding the controller: When using a trajectory like the polynomial and updating the speedj velocity every 8 ms according to the first derivative, how does the actual current position play into this?

Because I am not using ROS I unfortunately can't make use of your modern UR driver.