Huge incoherences when reading joint/link positions and velocities (probably Gazebo internal state handling error)

edit

Hi,

Some time ago I wrote a control plugin for a robot I am working on for my thesis. I had strange offset issues in the read states when testing my control algorithm which I wasn't able to solve because I couldn't find any errors in my code. Due to the length of the code, at first I thought that it had to be an error on my side, even if I couldn't find it. My robot model is a wheeled robot with a closed kinematic chains, of which I report a screen in Gazebo for clarity:

My control plugin reads the pointers of all links and joints and then uses the related methods to obtain measures of the various states and their derivatives. I omit here further details since they are not essential to understand the problem. Basically, when plotting the states and their derivatives in Matlab (I first save them to txt and then plot them) if I look at one variable and at its derivative, their are evidently not coherent. This could have been due to an error of mine inside my code.

However, I just performed a simple test by using directly the plotting utility of Gazebo. I plotted a joint position and its derivate. The result is the following You can clearly see that those plots are relative to the same joint (here it is not important which one is it). The problem is that the joint velocity is completely out w.r.t. the joint position. This can be easily seen since the position settles around a constant values towards the end of the simulation (which is expected due to how the controller works), while the velocity, which should be its derivative, settles to a constant value not equal to zero. Please tell me how this is possible. I believe this is some kind of error due to how Gazebo internally handles states (maybe). I also tested this behaviour, for example, with the pitch angle and the associated Y angular velocity of one of the links (this is legit because the controller keeps the motion in the plane x-z).

I really do not know why this happens.

Thanks in advance for the help

I forgot to tell that I am using Gazebo 11.

Small update. I tried to remove the plugin and let the system evolve freely (I also added some damping to the joint). The measurements seem to be coherent.

So it is the use of the plugin which is causing this strange behaviour (I have no idea why the GUI plotting utility is affected to).

I will try to reproduce the issue on a simpler system to help debugging.

I have also tried to completely remove any control input to the system and leave the plugin loaded into Gazebo. While the issue seems less evident, there is still incoherence between the readings. This time the only thing I am doing inside the plugin is reading the states using Gazebo API.