Weird behavior after PID tuning update

[PeterBowman]

The recent PID tuning accomplished at #2 (comment) (4807410) has some interesting side effects:

teoman.mp4

There seems to be something wrong with the model and we currently suspect the COLLADA files (introduced in #1). @imontesino says this happened to him when working with some old STL models on Bullet, apparently some vertices/faces appeared twice and collisions between them were happening constantly. He solved it by uploading a cleaner version at roboticslab-uc3m/teo-bullet-models@b4d68ad.

I have attempted a similar operation on our COLLADA files with Meshlab and its handy filtering functions (5a6252e). There are in fact a few duplicates, but scarce and in just a bunch of files. It didn't seem to solve the problem in our case.

[PeterBowman]

It seems that a substantial decrease of both Ki saturation and maximum PID output along with a substantial increase of contact points and stiffness helped a lot: 1dfc764. Thanks to @Gerson-Martin for pointing me at https://classic.gazebosim.org/tutorials?tut=physics_params&cat=physics!

[PeterBowman]

The derivative term is to be blamed here. I have found that the derivative error (obtained via GetErrors) is several orders of magnitude bigger than the proportional and integral ones.

Add the following code after this line for debug output:

double ep, ei, ed;
pid.GetErrors(ep, ei, ed);
yCIDebug(GAZEBOCONTROLBOARD, m_jointNames[j]) << ep << ei << ed;