Servo: Allow a negative joint margin

[AndyZe]

Servo has a joint_limit_margin parameter that provides a little padding around joint limits from the URDF. This is useful to make sure the robot has time to stop before the actual joint limits.

We have an unusual use case where one joint has very narrow joint limits and the encoders are pretty noisy. Also, there are hard stops, so it's no big deal if the software joint limit gets exceeded. For that, we want to allow a negative joint limit margin. Although we still throw a warning because this is so unusual.

[codecov]

Codecov Report

Merging #501 (feb43c9) into main (2ec3e9d) will not change coverage.
The diff coverage is n/a.

@@           Coverage Diff           @@
##             main     #501   +/-   ##
=======================================
  Coverage   46.65%   46.65%           
=======================================
  Files         183      183           
  Lines       19654    19654           
=======================================
  Hits         9168     9168           
  Misses      10486    10486           

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 2ec3e9d...feb43c9. Read the comment docs.

[AdamPettinger]

The idea seems OK to me, and I do think spitting out a warning is good

[destogl]

I can confirm the use-case :)

[vatanaksoytezer]

This also seems reasonable to me

[nbbrooks]

Some more context on why a servo specific solution is needed. Some smoothing algorithms appropriate for use with servo do not consider position limits and will "smooth" commands beyond them.

On the other hand, time parameterization algorithms used by move group, etc, handle pos as well as vel+acc+... limits directly.

Modifying joint_limits.yaml would impact both of these when we only want to adjust this for servoing. So this servo specific solution is needed.

[v4hn]

This parameter in general is just a way to simulate soft limits, which should be avoided, but can be moved out of. I would love to have support for them in MoveIt generally, but that's a bigger effort...