Various bug fixes

[AndyZe]

Just a straightforward port of this PR from gz_ros2_control:

ros-controls/gz_ros2_control#114

[gwalck]

@AndyZe I am sorry for digging this merged solution to ros-controls/gz_ros2_control#113 out, but I think the sagging issue is still there in gazebo classic after this back-ported fix #177.

In POSITION control method my robot sags under gravity, and even worse, its joints slowly dislocate ! I built all ros2_control packages with rolling and latest master branch of gazebo_ros2_control and used some kuka_experimental robots.

kuka_jtc_not_maintaining_pos.mp4

I was definitively sending a static command (with RQT JTC) to a JTC with position interface but the robot sags down. So its not the "robot not holding position" issue that is another recent issue of JTC it seems.

It reminded me of a well known problem from ROS and first use of gazebo 10 years ago to "enforce" a position that by-passes physics (= setting a pose or joint angle and not its effort).
We learnt the hard-way that one has to not only set position of a joint, but to also set its velocity to zero otherwise the non-zero velocity is still acting slowly overtime making the joint/object drift.

You might think, this is a bug, because at the next cycle the position is set back to the correct command, and I would agree, but it seems since gazebo 2 this was never fixed or is a "feature". I don't know.

In any case, the solution we used in the past and which also works here to sagging in position mode (without a PID controller converting it to a velocity) is to also set velocity to zero.
I successfully tested it and can provide a PR.

More generally, I am not sure how this back-ported fix #177 helped with regard to sagging robots. I understand it in gz sim because there seems to be a POSITION_PID implemented and some velocity could be overwritten, but not in gz_classic version of the plugin which has no such thing (yet).

old gazebo_ros_control in ROS had POSITION and POSITION_PID and VELOCITY and VELOCITY_PID etc... by the way. I don't know if an effort was made to simplify (retire PID versions) in gazebo_ros2_control.

[EDIT]
I just started to test my solution (setting vel to zero) in gz_sim as well (trying to remove the position PID sending velocities), when I stumbled upon the introduction of JointPositionReset in gz sim https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-gazebo/pull-requests/437/page/1 and the discussion highlighted something crucial.

I know that SetPosition (gz classic) and PositionReset (gz sim) by-pass the physics loop (happens before), and the discussion suggested to not use those commands for normal operation where physics is involved.
However, as mentioned in ros-controls/gz_ros2_control#87 in the case of a robot with unknown actuator/control-box characteristics, it is nice to be able to set the position no matter what the forces in sim. Joint reset would do so (and sagging is avoided by setting zero velocity).

If some algorithm should be tested on a robot sim that has physics interaction (and not only rendering and sensor measurement), then the user should use other control modes than position control of a robot anyway isn't it ?
[/EDIT]

[EDIT2]
I tested to revert the position PID of ros-controls/gz_ros2_control#29 and use JointPositionReset(position_cmd) + JointVelocityReset(0) to have the robot not sag, but this did not work for me. The robot was not moving at all. Maybe I messed up the implementation or did not understand how those function work or what they are supposed to do.

At least in gz_classic, the sagging is fixed for me. Are you interested by a PR ?
[/EDIT2]

[bmagyar]

That being said at least we could have merged this wiy a better name on this repo but oh well :D

[destogl]

Yes, please add the PR. This might fix UR simulation too

[gwalck]

Yes, please add the PR. This might fix UR simulation too

It does indeed fix UR sim. PR created

[christophfroehlich]

Am I missing something, or is the parameter position_proportional_gain_ not used anywhere?

[ahcorde]

yes, it was added as a parameter, but it's not used in the code

[christophfroehlich]

Should we then remove it again, or will it be used? ;)

[ahcorde]

@christophfroehlich #220