IMU filter may produce continuous warnings

[S-Dafarra]

While testing the robot, I noticed the following warning message being printed continuously

Provided non-increasing time vector

that I tracked to

icub-main/src/libraries/ctrlLib/src/adaptWinPolyEstimator.cpp

Line 130 in 7945541

yWarning()<<"Provided non-increasing time vector";

.

By looking at which modules use that class, I found it used in the ImuFilter:

icub-main/src/libraries/icubmod/imuFilter/ImuFilter.cpp

Line 85 in 7945541

double magVel=yarp::math::norm(velEst.estimate(iCub::ctrl::AWPolyElement(mag_filt,stampBias.getTime())));

I don't know if some check can be implemented on stampBias.getTime() to avoid this warning to be printed.

@Nicogene @pattacini

[pattacini]

This warning message might be critical so I think it's important that got printed.
Let me investigate its nature before deciding how to proceed.

[S-Dafarra]

This warning message might be critical so I think it's important that got printed.
Let me investigate its nature before deciding how to proceed.

Yes, sure. From the log it is not straightforward to understand what is going on and it is most likely ignored. It is possible there is some hardware issue, but I could not understand why that is printed.

[Nicogene]

A side note: probably this warning was printed also before, but since in the past it was an executable nobody noticed it.
It has been "recently" converted to a device and it is launched by the yarprobotinterface then it is now visible in its log

[pattacini]

What you say @Nicogene can be true, but my hunch is that it intimately depends on the type of the IMU.
With Xsens I did not spot such a warning when I coded the module imuFilter.

We know that with the Bosch unit within the RFE we've been suffering from problems related to timing.

[pattacini]

With non-increasing time values, the computation of the derivative of the magnetic info cannot be performed (in correspondence of the time instant when the misbehavior happens) and this goes to the detriment of the integration process that estimates the gyro offsets.

It would be beneficial to dump directly the data provided by the IMU YARP port to inspect the profile time stamps.

@Nicogene do you know what is the name of such a port?

[S-Dafarra]

I don't know if it helpful, but on iCub3 (the robot where I had this issue), these are the corresponding lines https://github.com/robotology/robots-configuration/blob/3c71e095a7c1c6b5463518dcbe96746430e588b6/iCubGenova09/icub_wbd.xml#L86-L87

[Nicogene]

@Nicogene do you know what is the name of such a port?

The name of the port is /icub/head/inertials/measures:o for the raw IMU, and /imuFilter/measures:o should be the filtered one

[pattacini]

Thanks @Nicogene !

@S-Dafarra would you be willing to launch the following the next time you'll be by iCub 3?

$ yarpdatadumper --rxTime --txTime --connect /icub/head/inertials/measures:o

[pattacini]

@Nicogene I wonder if the data shown in #595 (comment) are relevant here.

The second plot displays cases where dt = 0, which actually would bring up the warning message.

Is the same IMU device mounted on iCubGenova04?

[S-Dafarra]

Thanks @Nicogene !

@S-Dafarra would you be willing to launch the following the next time you'll be by iCub 3?

$ yarpdatadumper --rxTime --txTime --connect /icub/head/inertials/measures:o

Sorry I missed this, I will try on Monday!

Is the same IMU device mounted on iCubGenova04?

I think that on iCubGenova04 we still have the old one connected with i2c: https://github.com/robotology/robots-configuration/blob/3c71e095a7c1c6b5463518dcbe96746430e588b6/iCubGenova04/hardware/inertials/head-inertial.xml#L5-L7 while we have the rfe board on iCub3: https://github.com/robotology/robots-configuration/blob/3c71e095a7c1c6b5463518dcbe96746430e588b6/iCubGenova09/hardware/inertials/head-inertial.xml#L45-L48

[S-Dafarra]

Thanks @Nicogene !
@S-Dafarra would you be willing to launch the following the next time you'll be by iCub 3?

$ yarpdatadumper --rxTime --txTime --connect /icub/head/inertials/measures:o

Sorry I missed this, I will try on Monday!

Here the data. I just run it for about 30s
dump.zip

[pattacini]

Thanks heaps @S-Dafarra !
I'll have a look at the data then.

[pattacini]

Apparently, the timestamps are ok – both the tx and the rx – meaning that we don't have any time delta <=0.

@S-Dafarra did you get the error message popping up during the acquisition?

[S-Dafarra]

@S-Dafarra did you get the error message popping up during the acquisition?

I did not check. I also removed the imu filter from the robot yarprobotinterface to avoid filling the logger. Do you think it might be related to a temporary problem in reading the data?

[pattacini]

I did not check. I also removed the imu filter from the robot yarprobotinterface to avoid filling the logger

That's certainly ok but it would help to get it working again just for the sake of the test.
My goal is to inspect the data-trace I posted above and at the same time being sure that the error messages are spotted.

It'd be decisive to understand what's going on.

Do you think it might be related to a temporary problem in reading the data?

Some sort of quantum effect? 😉
Let's not exclude anything at this point.

[S-Dafarra]

Here some other data. This time I did see the message popping out, even if it was not continuous

dump.zip

[pattacini]

Thus, it doesn't seem that we have non-increasing timestamps. The plot above just highlights that there's been one packet lost.

@S-Dafarra just disable the imuFilter device while we'll be delving deeper.

Thanks again for the logs.

[pattacini]

I took the chance to craft a quick testing snippet in pattacini/icub-main-728 with the aim to check whether the library component is doing its job correctly.

The same exact library component is used within the module velocityObserver, which is the one I employed for the test.

The test itself generates a stream to which we can optionally attach an increasing timestamp. Also, we can yield a glitch each second, meaning that the timestamp is for just one step non-increasing.

Here's how to launch the test suite (you can run it in Gitpod):

$ test-728 --add-stamp [on|off] --add-glitch [on|off]
$ velocityObserver
$ yarp connect /test /velObs/pos:i

Results are that we spot the warning message only when we generate the glitch!
Therefore, the library component seems to be running as intended when fed with a correct flow of timestamps.

Given that we didn't observe from the logs anything weird, it might be that the bug lurks in the handling of the timestamps when they get received from the device driver, prior to being passed on to the library component.

cc @Nicogene

[pattacini]

@Nicogene can we discuss this a bit together when you'll be available?

[Nicogene]

Yes sure!

[pattacini]

After a quick debriefing w/ @Nicogene we agreed that the only test we can do is to check if the timestamp ts retrieved in

icub-main/src/libraries/icubmod/imuFilter/ImuFilter.cpp

Line 74 in 7945541

stampBias.update(ts);

is ok or not.

To do this, we're required to make a local hack to the imuFilter code.

@S-Dafarra, we can plan for this in the next sprint unless you're willing to insert the following snippet in the code and give it yet another whirl:

stampBias.update(ts);

static double t_{0.);
if (stampBias.getTime() <= t_) {
    yDebug() << "Got it!"; // or maybe we can send it out to a YARP port
}
t_ = stampBias.getTime();

double t0=Time::now();

[davidetome]

I see the message also on the iCub3

[S-Dafarra]

I see the message also on the iCub3

That's where I first noticed it.

[davidetome]

Thank you @S-Dafarra , it was only to say that I can run the test w/ the modified imuFilter code on it w/o using the greeny 👍

[davidetome]

@pattacini I ran the modified code for imuFilter.cppand got 

[pattacini]

So, this demonstrates that we do receive from the YARP I/F non-increasing timestamps: imuFilter is not the guilty guy.

That's kind of puzzling as this problem didn't come up from the logs we analyzed:

• did we log from the wrong port? cc @Nicogene
• was it that by chance the log didn't contain the problem itself? cc @S-Dafarra (I'd say that's unlikely)
• is there any possibility that the data on the net is ok and the YARP I/F is messing it up ahead of delivering it to the consumer? cc @Nicogene

[Nicogene]

did we log from the wrong port? cc @Nicogene

It seems the correct ones to me

is there any possibility that the data on the net is ok and the YARP I/F is messing it up ahead of delivering it to the consumer? cc @Nicogene

If I remember correctly we already discussed about the timestamps set by multipleanalogsensorsserver/client, maybe @traversaro remember about it, I cannot find it.

The chain of the ts should be:

icub-head:multipleanalogsensorserver -> icubsrv:mutlipleanalogsensorsclient(ref) -> icubsrv:multipleanalogsensorsserver

The imuFilter should take the ts from MAS server of icub-head, what I suspect is that MASserver if it does not have new data give you the latest in cache, this is why we have data with the same timestamps

The imuFilter has a period of 0 seconds see

icub-main/src/libraries/icubmod/imuFilter/ImuFilter.cpp

Line 35 in 7945541

PassThroughInertial(), PeriodicThread(0.0)

That's why you get continuously warnings

The strange thing that this

icub-main/src/libraries/icubmod/imuFilter/ImuFilter.cpp

Line 145 in 26c7435

this->setPeriod(m_period);

should set the correct period(10 ms by default)

In any case, in iCubGenova04 the period should be set correctly(https://github.com/robotology/robots-configuration/blob/f3fef4277717ae87d825a9a46d994ccd21fef3ec/iCubGenova04/wrappers/inertials/head-imuFilter.xml#L6)

[pattacini]

Thanks @Nicogene 👍🏻

The imuFilter has a period of 0 seconds

Nope, as you have correctly noticed, we use setPeriod.

With this respect, the following line is conceptually wrong

icub-main/src/libraries/icubmod/imuFilter/ImuFilter.cpp

Line 139 in a8e24e8

m_period=config.check("period",Value(0.01)).asDouble()/1000.0;

as m_period needs to be given in seconds (as per the default value 0.01) while there's a weird manipulation because the device driver expects the period in ms.

Probably, the best would be to change the XML files from ms to s and update the values from 10 to 0.02 or even more (i.e. a bit more of the streaming rate of the IMU).
Finally, the imuFilter line above needs to be updated accordingly.

Would you be willing to do the fix @Nicogene ?

⚠ The original imuFilter codebase was running in sync with the incoming data.

[pattacini]

Probably, the best would be to change the XML files from ms to s and update the values from 10 to 0.02 or even more (i.e. a bit more of the streaming rate of the IMU).
Finally, the imuFilter line above needs to be updated accordingly.

Would you be willing to do the fix @Nicogene ?

I've changed my mind! I believe it's better to stick to ms as of now, in sight of a major overhaul when all the device driver will switch to the use of seconds at once.

I'll do the job then @Nicogene!
I'm on it.

[pattacini]

PR's opened up:

• Fix imuFilter sampling of incoming data #745
• Fix imuFilter sampling of incoming data  robots-configuration#255

[pattacini]

@Nicogene we underestimated the lack of being in sync w/ the incoming data during the major overhaul done in #592.

Increasing the period to prevent warnings is ok as the device is not crucial; however, we're basically subsampling the signals this way, making imuFilter not useful at all.

I think the imuFilter device should really restore the in-sync functionality.
Would you be willing to take on this task?

I'm opening a new issue for that (see #746).

[Nicogene]

Increasing the period to prevent warnings is ok as the device is not crucial; however, we're basically subsampling the signals this way, making imuFilter not useful at all.

So we merge #745 and robotology/robots-configuration#255 or we await the proper fix?
Because I suspect that after those changes the imuFilter would work worse than the raw imu

I think the imuFilter device should really restore the in-sync functionality.

About this we should attach to a callback of the masclient that I suspect does not exist, the alternative is to roll-back to the read of the port, but we have to parse the mas message.
We could use the blocking read as synchronizer, and then we retrieve the data with through the mas client.
@traversaro do you know if exist a cleaner way to do that?

[pattacini]

So we merge #745 and robotology/robots-configuration#255 or we await the proper fix?

I would proceed anyway, as those warnings might be as dangerous as going at 20 ms.

[pattacini]

@traversaro do you know if exist a cleaner way to do that?

He has already a plan! 😀
See #746 (comment).

[pattacini]

Let's move the discussion to #746.