The problem you are having is that you are not also sending the positions. A difference between the last command position and measured positions is the cause of the error.Add the following line to the FRI C++:
robotCommand().setJointPosition(joint_positions_cmd);
To test safely, just send torques for the last link in a sinusoidal fashion and check that a motion is achieved.
Thank you for the reply. We actually set the joint position in FRI, as you can see after the for loop in the C++ code.
Are you using this kind of torque control and if you do are you adding a small constant on the joint positions or you just command the measured (epsilon = 0 in our C++ code)?