Avoid setting control mode at each control loop iteration

[traversaro]

cc @francesco-romano

[traversaro]

The problem is that the setTorques block is setting the control mode at each control loop, see https://github.com/robotology-playground/WBI-Toolbox/blob/new_wbi_ID/libraries/wbInterface/src/wbInterface.cpp#L1362 . Setting the control mode is a costly RPC operation, and should be done only once.

A sketch of the cost of every part of the functions is given in this debug output:

Time for set control mode portion of set reference block: 0.031909
Time for multiple joint set control reference: 0.000761 
Time for set reference block: 0.032707

[traversaro]

The reason why this was not a problem back in v0.1 is this issue: robotology-legacy/yarp-wholebodyinterface#9 .
Basically in v0.1 the yarpWbi was sending the rpc command only when the control mode (for the wbi) changed. In v0.2 ( see https://github.com/robotology-playground/yarp-wholebodyinterface/blob/72451957ff31e95df707c80499e3643f5d94f6b9/src/yarpWholeBodyActuators.cpp#L346 ) I changed this behaviour because I felt it was privating the user of the wbi of a feature (setting a given control mode in the underlyng controlboard).

[traversaro]

The same test after robotology-legacy/yarp-wholebodyinterface@6086e55 :

Time for set control mode portion of set reference block: 0.000001
Time for multiple joint set control reference: 0.000707 
Time for set reference block: 0.000719

[traversaro]

Leaving this issue open because in the long term we should try to avoid to redo setControlMode calls at each control loop.

[traversaro]

cc @francesco-romano @DanielePucci , on this topic check also robotology-legacy/yarp-wholebodyinterface#9

[jeljaik]

It isn't completely clear to me if this issue was solved at the yarp-wholeBodyInterface level. I remember @traversaro saying that in v0.2 once it's set future calls for the same control mode are ignored. Am I correct here?

[traversaro]

Yes, that is the behaviour that I have implemented to solve this issue but I really don't like it, but solving this is low priority I guess.