Fix sagging robots in position control

[gwalck]

Following up with my remarks on #177 (comment) and a request for a PR.

summary: in the discussion of the reset pos/vel commands it is said that setting position/velocity for joints is overriding physics, and should generally not be used,

I know from past experience with enforcing pose of an object at each time step was not sufficient to hold it in place (drift), velocity had to be set to zero as well. I immediately related the joint sagging issue to that pose drift.

Solution is to set zero velocity as well for that time step. Setting velocity to zero after fixing position is another override that stabilizes the physics and is unrelated to any robot physics.

here is a before and after on 2 robots

ur_broken.mp4

ur_fixed.mp4

kuka_broken.mp4

kuka_fixed.mp4

Other solutions have used max efforts or adding high damping (which probably essentially set velocity to zero)

I believe this PR should therefore fix the following issues without requiring dynamics parameters.

#90
#73
UniversalRobots/Universal_Robots_ROS2_Gazebo_Simulation#19

[ahcorde]

https://github.com/Mergifyio backport humble

[mergify]

backport humble

✅ Backports have been created

• #214 Fix sagging robots in position control (backport #213) has been created for branch humble