positionDirectControl delayed and slow pitch head movement

[kt10aan]

Sending commands to the positionDirectControl module (in a form of a [pitch,roll,yaw] Bottle), the roll and yaw movements are responding quickly and they are quite fast.

However, when sending a command that affects the pitch axis, the resulting movement:
a) starts after some delay and
b) is slower

This is even more pronounced when I am sending commands to change only the yaw and/or the roll (keeping the pitch constant) versus sending commands to change only the pitch (keeping the yaw and the roll constant).

Is the likely reason something in the actual hardware (e.g., some issue with the neck ?) Or is it fixable through changing some config files (the robot's presumably)?

[drdanz]

Looks like something similar here what happened at the summer school to the black iCub before the tendon broke and the head started spinning like crazy!
@randaz81 @vtikha @julijenv perhaps remember some details

[pattacini]

For Blacky there's an issue open on this: robotology/icub-tech-support#46.

[kt10aan]

Is there anything in software that I can try? Because sometimes different robots have same symptoms for different reasons :) Otherwise, I will just wait for the general solution (and hope the neck tendon does not snap!!!)

[pattacini]

Hi @kt10aan,

Would it be possible for you to do some acquisitions and report here the results?

Using a simple snippet code you could:

1. control the joints in position and log the reference and the feedback with yarpdatadumper. We should see minimum-jerk trajectories;
2. control the joints in position-direct and log the same data.

In principle, we should be able to see big discrepancies between the pitch reference and the relative encoder feedback when in position-direct mode, while on the other two joints things should be running smoothly.

You could also check via yarpmotorgui the actual PID gains (report them here too) to see whether they correspond to the values stored in the config files.

[kt10aan]

Thanks @pattacini. In yarpmotorgui the PID gains (current vs desired and in config files) are exactly the same for joints (0 1 2).
I will get down to acquire the other data (also, is there anything in the icub-tests repository that could be of use in this case?)

[pattacini]

/cc @randaz81 @apaikan for hints on icub-tests.

[drdanz]

@kt10aan After some testing, we find out that we have the same issue here, and that it might be caused by the slew rate limiter that was introduced at some point in the positionDirectControl module. Commenting out that block makes the robot move smoothly for us. On the other hand it might be unsafe because removing that, makes the joints not limited in velocity (even if there is still a filtering that checks that consecutive commands are not too away from each other).
Anyway, if you want to test if the issue is the same, just remove lines 91-97 in <icub-main>/src/modules/positionDirectControl/positionDirectThread.cpp, but do not make excessively fast movements! ;)
We are still investigating how to fix this properly (suggestions are welcome).

[pattacini]

@drdanz 👍

In my opinion, the position-direct mode should be used only by experienced users, who are supposed to take care of limiting position references with high frequency content. Any kind of safety procedure we can come up with cannot cover all the possible applications indeed.

However, the slew rate limiter could be kept and made configurable (even removed) while launching the module, so that the user is aware and able to tune it.

[kt10aan]

@drdanz, commenting out the slew rate limiter, did the trick. Thanks!
While being careful with the vertical head movements, it (at least) seems to be quite safe.

[lornat75]

I agree with @pattacini, this issue confirms that adding the limiter is confusing.

[randaz81]

Well, I do not agree at all: removing the limiter is very risky.
It's the ONLY thing that protects the joints if a wrong command is received.
If your command is too far away respect to the current position of the joint, the PID controller will inevitably become unstable.
For the icub neck a difference greater than 10 degrees has a good chance of being fatal.

[kt10aan]

Just my 2 cents: We are playing a lot the past two days with the positionDirect and the Oculus, even giving it to people that did some quite abrupt and sudden movements in the pitch axis: everything seems more than fine.

I am not saying that is not dangerous, I am just saying that (within reason), it's not that dangerous. Of course everything is a trade off between benefits and risk (and the risk is greater if you don't have IIT people within reach to fix stuff!)

[randaz81]

Please also remember that even if you are confident that your module is sending correct numbers, other hidden factors (e.g network delays, thread rates etc) can play an important role.
You really don't have the need to move infinitely fast. Just estimate the max speed of your task and put the limit above this value.
By the way, this limit was initially embedded in motionControl. And it will probably moved there again.

[drdanz]

I don't think that the issue here is whether to have the slew rate limiter or not, we should find out why the limiter is causing this issue instead. I suspect that there might be some delay somewhere and the value read by getEncoders() is an old value and that causes the delay and shaking.

[pattacini]

By the way, this limit was initially embedded in motionControl. And it will probably moved there again.

What are the numbers we are talking about? We want the robot move fast with the Cartesian Control. Is there the risk that these thresholds will be limiting our bandwidth? How are they selected?

@randaz81 I'd really like to keep those thresholds in the module rather than putting them in motionControl ... please 😄

[pattacini]

Btw, in the assumption that the source produces "good" position trajectories (with limited bandwidth), then network lags have no impacts on risk factors thanks to the intrinsic nature of position control.

Different considerations must be taken for velocities controls, for which indeed we have set a timeout.

[randaz81]

You are wrong. Due to the non-linear nature of our system we have that:
-the step response of the PID control from 0 to 1deg (for example) is stable. Our gains have been tuned under this assumption.
-the step response of the PID control from 0 to 10 is unstable.
So if some direct position commands of the sent trajectory are lost, you have a good chance to unstabilize the system...

[randaz81]

We have to define a better check. I'll open a feature request task on a separated thread in icub-main.

[pattacini]

Well, I've just added a note on network lags, which are a bit different from network drops. Anyway, at this point what scares me a lot is that the system is unstable when fed with a step of only 10 deg.

From a control standpoint, the system robustness would increase significantly if the PID could work within a range wider than just 1 deg.

Anyway, once more these considerations draw the attention on the PID tuning task we have scheduled to carry out soon. See also robotology/icub-main#140.

[pattacini]

The problem has been actually solved, closing the issue.
For any other related development, we will open a new ticket, as stated by @randaz81.

[lornat75]

If we keep the limiter we have to make sure that it is clear to the user when the limiter gets activated so the debug goes in the right direction (checking why the position direct commands are not smooth enough). I hope we all agree on this...