Torso lift of Tiago does not work in servo mode

[jbmouret]

See the videos:
https://filesender.renater.fr/?s=download&token=b4bc6451-75e2-4f18-bb2d-06c4955fc6fc

I thought it could be a limit problems, but even with very high limits the torso does not do the same as in velocity mode.

However, it seems to work fine with

robot->set_actuator_types("servo");
robot->set_position_enforced(false);

More info. The issue seems to be linked to the "lower" limit. If I put -0.02 instead of 0, we have the expected behavior:
<limit effort="2000" lower="-0.02" upper="0.35" velocity="0.07"/>

It could be that the issue is that joint starts at the lower limit (0). Things are better if we start a bit above it:
set_positions(Eigen::VectorXd::Ones(1) * 0.02, {"torso_lift_joint"});

... but after 13s, the "sticky behavior" appears again. In that case, the position is negative (-1.20979e-13), and I think it's in a unfeasible constraint. It could be that because of the dynamics (the weight of the robot), the actuator slightly overshoots and then is stuck there.