Bug in the generation of a new trajectory when the robot is position controlled

[GiulioRomualdi]

I was tuning the position control in Gazebo for iCubGazeboV3 and I noticed a strange behavior of the controller when a new reference is set. As soon as the new setpoint is set on the yarpmotorgui the moved joint oscillates.
This behavior is independent of the gains of the PID position controller.

Here you can see a gif that represents the described behavior

The same problem happens when I switch from position to position-direct (and vice-versa)

This is @traversaro's answer

I think there is probably some bug in the logic that resets the desired position/trajectory generation when we start a new trajectory: in cases in which there is a non-negligible static error when you reset the trajectory generation and the desired position, you should not reset it to the measured position, but rather to the previous desired position. In particular, the bug is in

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

Line 123 in c042735

m_trajectory_generator[j]->initTrajectory(m_positions[j],m_trajectoryGenerationReferencePosition[j],m_trajectoryGenerationReferenceSpeed[j]);

.