Motion delay while point streaming for real-time control

[JSook]

I'm using an ES165RD-II with a DX200 to do force control with an ATI Omega force/moment sensor. The robot's motions are controlled in real-time based on measurements from the force sensor.

I was originally having a bunch of issues using the /joint_path_command topic for control of the robot with smooth motion. I was able to use #215 to perform point streaming through the /joint_command topic instead which seems to solve that problem. The current stream is running well at 50Hz.

The current issue I'm having is that there is a significant and repeatable delay from when a joint command is sent to when the robot responds to it, about 250ms. I believe this may be a similar issue to #219 and I've seen it referenced at http://wiki.ros.org/fs100_motoman. I updated to the MotoROS 1.8.1 binary on the DX200, but that hasn't seemed to improve anything. The in-motion flag does work properly though. The delays were originally larger, I reduced the sleeps in #215 joint_trajectory_streamer.cpp and that helped slightly.

Here's a plot of the B joint command vs robot response, while constantly streaming points. Don't mind the joint numbering, it's zero indexed in one message and one indexed on the other.

I also did a test where I didn't keep the stream open which seems to make the delay worse if you start, stop and restart the stream while the robot is still in motion - an additional 250ms each time I think.

Is there a way to reduce this delay? The length of the delay and the size are the arm could lead to some robo-carnage. I have a feeling that increasing the loop rate on the DX200 side of things may help, but I can't find an easy way to do that. The robot topics are being published at 40Hz (not sure how to change that) and I noticed that the MotoPlus code has a period of 10 (ms?) - but I don't have the SDK to recompile that with a shorter period. Any advice is appreciated.

[gavanderhoorn]

I'm not sure whether this is going to change anything, but there are two joint_trajectory_streamer.cpp files where a 250 ms delay is used:

• motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp
• motoman_driver/src/joint_trajectory_streamer.cpp

at least; in the original driver. I've not checked the changes introduced by #215.

The robot topics are being published at 40Hz (not sure how to change that) and I noticed that the MotoPlus code has a period of 10 (ms?)

actually: the robot state node's control flow is directly coupled to that of the MotoROS StateServer. And the StateServer has a period of 25 ms, not 10. That's where the 40 Hz comes from.

I have a feeling that increasing the loop rate on the DX200 side of things may help, but I can't find an easy way to do that.

Maybe, but I wouldn't be surprised if there is some fundamental lower limit in the controller's infrastructure that is contributing to the delays here.

That is what #219 also hints at.

Also note: the trajectory generator on the MotoROS side was not written with your use-case in mind. The way it's used in #215 (and other similar approaches) is essentially a hack/work-around.

What is described in #219 is the state-of-the-art as far as I know.

[JSook]

Good info.

I'm not sure whether this is going to change anything, but there are two joint_trajectory_streamer.cpp files where a 250 ms delay is used

I reduced the sleep in both places. This isn't sounding promising, any other suggestions?

[gavanderhoorn]

Not at this time. You could post a comment on #219 as that is the issue where we track low-latency control via MotoROS and see whether @ted-miller or @EricMarcil have some input, but as I wrote earlier, I don't believe there have been any significant changes since #219 was opened.

[ted-miller]

I have just sent an email to JSook directly, but I wanted to post my comments here also.

The delay for the robot's motion is normal and expected. MotoROS processes the incoming motion commands and breaks them down into an API called mpExRcsIncrementMove(..). This is the lowest level of control that MotoPlus has access to. After submitting to that API, it goes into a black-box where the data is fed into the robot controller’s motion planner. In there it is filtered, smoothed, and broken up before being submitted to the servo loop. There many factors that contribute to the exact time delay amount. But, I would consider 250ms to be a normal delay.

[gavanderhoorn]

It's a bit on the high side though.

@ted-miller: how are force-torque sensors integrated in newer Yaskawa controllers? Software interfacing those must have lower level access because otherwise they would not work.

[ted-miller]

In regard to the newer force-torque sensors (such as HC-10), they don't control the robot motion using this API. The software for that is tied into a lower layer in the servo-board. That functionality is not exposed to MotoPlus.

[gavanderhoorn]

Seeing as the general issue of delays is being discussed in #219 and we've seemed to have answered @JSook's initial questions, I'm going to close this one.

Feel free to keep commenting on it of course.

[jim-rees]

The sleep is bogus and should be removed. The loop should instead wait (with a timeout) on a condition variable.

Sleeps should be removed frorm MotomanJointTrajectoryStreamer::streamingThread(). There should be a timed_wait() (in one place) instead.

Condition variable should be get a notify_all() in JointTrajectoryStreamer::jointCommandCB().

You need a mutex to synchronize all this.

Without these changes you'll get an average delay of timeout/2 on your streamed points, and of course lots of jitter. With these changes your delay will be much smaller, with no jitter.

[jim-rees]

Actually I don't remember how much of this applies to the generic code as opposed to the joint streaming branch, it's been too long.