Weird behavior of gravity compensation on icubGazeboSim

[gabrielenava]

I did a simple modification of wholeBodyImpedance:

I took the gravity compensation part. I removed the feedback + feedforward part. I substituted it with a block that can do a sweep in frequency on a sine function. I set the AMPLITUDE of the sine to zero to verify the effectiveness of gravity compensation with icubGazeboSim. This is the result (after 30 [s] of simulation):

It is possible to reproduce the same behavior using wholeBodyImpedance. I did the following modification:

it is the classical wholeBodyImedance controller but I added three GAIN blocks in which I set the gain to zero in order to remove the feedforward/feedback part. This is the result after 30[s]:

and these are the gravity torques:

I know this may be due to integration errors in Gazebo, still these results are really bad to me. It seems to me strange that gravity compensation is so bad. I cannot understand why. Furthermore, this is what happens if I try to control only 1 joint (in the picture: torso pitch, joint torque):

I cannot understand this at all. Why I can't control only the torso pitch in torque mode?

This is what is inside the gravity compensation block:

[traversaro]

I cannot understand this at all. Why I can't control only the torso pitch in torque mode?

Remember that if you control only one joint, the model will assume that all other joints are set to 0, so the gravity compensation will probably totally wrong.

[traversaro]

To debug the gravity compensation, I suggest to start by comparing the gravity compensation torques and the torque provided by the PID low level control, to see if they are ~consistent.

[traversaro]

Anyhow, consider that the icubGazeboSim model available in icub-gazebo has been manually modified a lot of time (when the eyes and hands have been added, etc etc). I would suggest to do this tests on the new iCubGazeboV2_5 introduced in https://github.com/robotology-playground/icub-model-generator/pull/71 .

[gabrielenava]

I cannot understand this at all. Why I can't control only the torso pitch in torque mode?

Remember that if you control only one joint, the model will assume that all other joints are set to 0, so the gravity compensation will probably totally wrong.

Ok thanks, this explains why it failed when I tried it only with torso pitch.

I actually tried the same test using Walkman model, and I got the same results. Maybe it is due to numerical integration errors?

[traversaro]

Mhh I think we should chat about this on skype/appear.

[gabrielenava]

I tested the gravity compensation only on the legs. I also compared the desired torques with the measured (are these torques the one provided by the PID low level control also in simulation? does it make sense to do this comparison in simulation?). This is the torque error:

it is almost always zero, but there's a strange peak and I don't know why, but I don't think is related to the gravity compensation issue. Indeed, the gravity compensation is way better on the legs (the derive is much slower in 30 [s]). See the picture below.

So I really think we're debugging numerical integration issue.

[traversaro]

numerical integration issue

Nitpick; in general consider that there are several points in which the classical multibody dynamic is "approximated" in the ODE physics engine (see http://robotics.cs.tamu.edu/dshell/papers/simpar2010ode.pdf), so it can be also a numerical simulation issue that is not related to integration algorithms.

[gabrielenava]

I tested the gravity compensator on real robot, it was working fine even without feedback terms. We can close this issue.