Enforce singularity threshold when moving away from a singularity

[nbbrooks]

Description

I have updated this description to reflect the final behavior.

Currently the singularity threshold behavior is only enforced if the manipulator is moving further into singularity. If a different mode of operation is used (e.x. joint servoing) which puts the manipulator into/beyond the singularity region and then the user switches to Cartesian servoing, the velocity scaling/singularity halting behavior will not take effect if they are servoing away from the singularity (e.g. dot < 0). This can result in wild motion.

This PR makes 2 opt-in changes

1. Enforce singularity behavior regardless of whether they are moving into/away from singularity
2. Don't reduce the velocity as much for movement near, but moving away from a singularity (compared to if it was moving further into singularity)

The behavior of 2 uses a new parameter leaving_singularity_threshold_multiplier and is described in the below graphic. By default leaving_singularity_threshold_multiplier is 2.0

TODO:

• Default the new approaching_stop_singularity_threshold param to hard_stop_singularity_threshold so this doesn't break existing implementations (e.g. make this opt in)
• Add tests checking approach and hard stop conditions

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[AndyZe]

I like these things. Both very useful for debugging:

Publish singularity condition to topic servo_server/condition
Add status code to distinguish between velocity scaling when moving towards/away from the singularity

I don't like this. Several people have complained in the past when the robot could only move away from singularity slowly:

Enforce singularity behavior regardless of whether they are moving into/away from singularity

Instead of that, I like the idea you brought up privately:

Add a 3rd threshold that is inbetween the current two. This will be the "approaching_singularity_hard_stop". It won't let you move past that if you are moving into the singularity, but still lets you move away

[AndyZe]

If you want to reduce the scope of this PR so it only does this two things, I'd approve:

Publish singularity condition to topic servo_server/condition
Add status code to distinguish between velocity scaling when moving towards/away from the singularity

[nbbrooks]

@AndyZe I moved the singularity topic publishing to separate PR #695

I did not include the new status code for servoing away from singularity to that PR, as currently we don't enforce singularity when moving away so that status code would never be used.

[codecov]

Codecov Report

Merging #620 (de97459) into main (1d67b51) will increase coverage by 0.04%.
The diff coverage is 87.88%.

❗ Current head de97459 differs from pull request most recent head 52a77b7. Consider uploading reports for the commit 52a77b7 to get more accurate results

@@            Coverage Diff             @@
##             main     #620      +/-   ##
==========================================
+ Coverage   51.10%   51.13%   +0.04%     
==========================================
  Files         380      380              
  Lines       31737    31755      +18     
==========================================
+ Hits        16217    16236      +19     
+ Misses      15520    15519       -1     

Impacted Files	Coverage Δ
...veit_servo/include/moveit_servo/servo_parameters.h	100.00% <ø> (ø)
moveit_ros/moveit_servo/src/servo_parameters.cpp	86.53% <71.43%> (-0.78%)	⬇️
moveit_ros/moveit_servo/src/servo_calcs.cpp	68.25% <100.00%> (+0.81%)	⬆️
moveit_core/robot_state/src/robot_state.cpp	47.44% <0.00%> (+0.08%)	⬆️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

[nbbrooks]

That sounds fine. I'll admit, the parameter names were a little confusing (because you were preserving backward compatability)

Totally agree. Here is my proposal for the new behavior in a graphic we can clean up and use in documentation. If people like it I'll update PR description.

[henrygerardmoore]

@nbbrooks @AndyZe Just updated to the latest design, take a look when you have a chance and let me know if I should change anything (especially with the style of the deprecation warning and how I checked for the parameter's existence before getting it).
Servo tests still pass locally, but should we add a test to make sure the scaling is working as expected? Mathematically it makes sense to me, I was just wondering if that might be warranted.
Also, is linking to a PR in a comment bad style? I could maybe link directly to the diagram or just try to write an explanation if that would be better.

[AndyZe]

Almost there.

Also, is linking to a PR in a comment bad style?

IMO that's fine

[henrygerardmoore]

Thanks for the review, Andy! I think it should be good to go now. Made your changes and cleaned some stuff up as well as adding defaults for the panda .yaml files.

[tylerjw]

I like the approach of keeping old configs working and using a warning to inform them of new/better behavior they can take advantage of through a config change.

I would like to see logic moved out of sevo_calcs and into free functions that can be tested independently of all of the state in servo_calcs but will not block this PR for that.

[henrygerardmoore]

Is one of the new tests flaky? 52a77b7 failed CI while 0f2a6ca didn't even though the only change was the warning text 😓

[AndyZe]

See #1499

Feel free to debug that if you have time. That would be awesome.