Manage change of control mode in control board

[xela-95]

It is of utmost importance to manage correctly the switch from one control mode to another, to guarantee the smoothest transition, for example by properly setting new correct references to track.

[xela-95]

@traversaro if there are specifications (written somewhere or from experience on the robot) let me know, so that we can gather them here.

[traversaro]

@traversaro if there are specifications (written somewhere or from experience on the robot) let me know, so that we can gather them here.

I am not aware if there are any written specification, but what happens in most implementation is that the measured quantity is used to initialize the setpoint, see for example:

• https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoardDriverControlMode.cpp#L141
• https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoardDriverControlMode.cpp#L174

Interestingly, this logic seems not be used in velocity, but that is probably a bug:

• https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoardDriverControlMode.cpp#L157

[traversaro]

Interestingly, this logic seems not be used in velocity, but that is probably a bug:

Thinking a bit more about this, perhaps the logic is to do the safest possible thing in the change of control mode, i.e. ensure that the desired velocity is 0, so this could have sense.