PID are not handled correctly for fingers joints in ergoCubGazeboV1

[xEnVrE]

While using one of the robots that rely on GYP, namely ergoCubGazeboV1, I noticed that when setting the PIDs online, e.g., using the yarpmotorgui, the settings are not updated as expected.

The following video shows an example:

test_pid_coupled-2023-09-08_15.26.50.mp4

I first command the index open/close joint r_index_oc, and it works as expected. Later on, I put the position P gain to 0.0 and I try to move the joint again. I expects it not to move, instead it does.

By testing the other joints, I realized that the r_index_add was not moving anymore. Hence, setting the P gain of r_index_oc has in fact changed the configuration of another joint.

Can be the fact that these are coupled joints the reason for this behavior - i.e., we are not handling PID setup correctly for these kind of joints?

cc @traversaro @Nicogene

[traversaro]

This is a bit of a mix of a problem in the plugin and due to how YARP interfaces are presented.

So, the way gazebo_yarp_controlboard currently works is that PID controller are closed on the (using the nomenclature used in https://github.com/orgs/robotology/discussions/609) physical joints quantities (see

gazebo-yarp-plugins/plugins/controlboard/src/ControlBoardDriver.cpp

Line 662 in 0fd9300

forceReference = pid.Update(convertUserToGazebo(j, m_motPositions[j]) - convertUserToGazebo(j, m_motReferencePositions[j]), stepTime);

), while the YARP interfaces operate on controlled axis quantities. So, all PID gains are referred to physical joints (see https://github.com/robotology/gazebo-yarp-plugins/blob/v4.8.0/plugins/controlboard/src/ControlBoardDriverPidControl.cpp), but in the yarpmotorgui they have the name of controlled axis. So, for sure this is broken for coupled joints. How do you imagine this should work? How it works on the real robot?

Perhaps the solution is just to close the PIDs on controlled axis quantities, or at least have an option to do so. In that case, setting the gains would work as intended.

[xEnVrE]

How do you imagine this should work? How it works on the real robot?

I would say that in this case the simulation has to depart from the real robot as in this specific scenario the real robot does not have this problem at all (as we cannot control all the physical joints separately on the real thing - talking about coupled joints).

Could we maybe set all PIDs of physical joints linked to a given controlled axis to the value associated to that controlled axis?

[traversaro]

Could we maybe set all PIDs of physical joints linked to a given controlled axis to the value associated to that controlled axis?

Yes, that could be a possible solution.

cc @S-Dafarra @EhsanRanjbari