Enforce singularity threshold when moving away from a singularity by nbbrooks · Pull Request #620 · moveit/moveit2

nbbrooks · 2021-08-18T18:45:08Z

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

Enforce singularity behavior regardless of whether they are moving into/away from singularity
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 · 2021-08-20T18:00:28Z

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 · 2021-08-20T18:02:16Z

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 · 2021-09-22T15:30:14Z

@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.

moveit_ros/moveit_servo/test/test_servo_singularity.cpp

…ingularity - Prevent uncontrolled behavior when servo starts close to a singularity and then servos away from it - Scale velocity at a different rate when approaching/leaving singularity - Add status code to distinguish between velocity scaling when moving towards/away from the singularity

codecov · 2021-11-08T17:17:13Z

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 · 2022-08-01T16:18:30Z

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 · 2022-08-05T22:00:19Z

@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.

moveit_ros/moveit_servo/config/panda_simulated_config.yaml

moveit_ros/moveit_servo/src/servo_parameters.cpp

AndyZe · 2022-08-10T20:13:46Z

Almost there.

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

IMO that's fine

Co-authored-by: AndyZe <andyz@utexas.edu>

…ved code block next to relevant singularity code

henrygerardmoore · 2022-08-12T21:26:30Z

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.

moveit_ros/moveit_servo/src/servo_parameters.cpp

henrygerardmoore · 2022-08-15T16:10:10Z

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

AndyZe · 2022-08-15T16:15:40Z

See #1499

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

* Enforce singularity threshold behavior even when moving away from a singularity - Prevent uncontrolled behavior when servo starts close to a singularity and then servos away from it - Scale velocity at a different rate when approaching/leaving singularity - Add status code to distinguish between velocity scaling when moving towards/away from the singularity * Work on expanding servo singularity tests * Pre-commit * removed duplicate input checking * added 2 other tests * undid changes to singularity test * Update moveit_ros/moveit_servo/src/servo_calcs.cpp with Nathan's suggestion Co-authored-by: Nathan Brooks <nbbrooks@gmail.com> * readability changes and additional servo parameter check * updating to newest design * added warning message * added missing semicolon * made optional parameter nicer * Remove outdated warning Co-authored-by: AndyZe <andyz@utexas.edu> * Removing inaccurate comment Co-authored-by: AndyZe <andyz@utexas.edu> * making Andy's suggested changes, added some comments and defaults, moved code block next to relevant singularity code * removed part of comment that does not apply any more * Mention "deprecation" in the warning Co-authored-by: Henry Moore <henrygerardmoore@gmail.com> Co-authored-by: Henry Moore <44307180+henrygerardmoore@users.noreply.github.com> Co-authored-by: AndyZe <zelenak@picknik.ai> Co-authored-by: AndyZe <andyz@utexas.edu> (cherry picked from commit d4b3294)

…) (#1649)

nbbrooks mentioned this pull request Aug 18, 2021

[moveit_servo] PoseTracking/Servo example implementation details moveit/moveit#2617

Closed

nbbrooks mentioned this pull request Sep 22, 2021

Publish singularity condition to topic #695

Merged

6 tasks

nbbrooks force-pushed the always-enforce-singularity-thresholds branch 2 times, most recently from 7c0382e to c539886 Compare September 22, 2021 15:22

nbbrooks changed the title ~~WIP: Enforce singularity threshold behavior even when moving away from a singularity~~ Enforce singularity threshold when moving away from a singularity Sep 22, 2021

nbbrooks requested review from AndyZe and felixvd September 22, 2021 15:27

tylerjw reviewed Sep 22, 2021

View reviewed changes

moveit_ros/moveit_servo/test/test_servo_singularity.cpp Show resolved Hide resolved

nbbrooks force-pushed the always-enforce-singularity-thresholds branch from 9af403e to 004d538 Compare September 24, 2021 23:12

henningkayser assigned nbbrooks Oct 22, 2021

nbbrooks added 2 commits November 7, 2021 20:25

Work on expanding servo singularity tests

fabc2e2

nbbrooks force-pushed the always-enforce-singularity-thresholds branch from 004d538 to fabc2e2 Compare November 8, 2021 14:30

Pre-commit

a9ecb19

nbbrooks changed the title ~~Enforce singularity threshold when moving away from a singularity~~ WIP: Enforce singularity threshold when moving away from a singularity Nov 8, 2021

nbbrooks marked this pull request as draft November 9, 2021 01:58

nbbrooks marked this pull request as ready for review November 9, 2021 02:01

henningkayser marked this pull request as draft February 3, 2022 17:12

henrygerardmoore added 5 commits June 28, 2022 15:10

Merge branch 'main' into always-enforce-singularity-thresholds

6874a30

removed duplicate input checking

c0161b0

added 2 other tests

0bfc1e8

undid changes to singularity test

4636f50

Merge branch 'main' into always-enforce-singularity-thresholds

fafe0a7

nbbrooks marked this pull request as ready for review June 29, 2022 19:38

nbbrooks changed the title ~~WIP: Enforce singularity threshold when moving away from a singularity~~ Enforce singularity threshold when moving away from a singularity Jun 29, 2022

nbbrooks mentioned this pull request Jul 20, 2022

Servo velocity near singularities can increase suddenly #1440

Closed

henrygerardmoore and others added 5 commits August 5, 2022 14:09

Merge branch 'main' into always-enforce-singularity-thresholds

a9700a3

updating to newest design

1381c48

added warning message

7219658

added missing semicolon

0b6e47a

made optional parameter nicer

179b4b0

Merge branch 'main' into always-enforce-singularity-thresholds

62893ad

AndyZe self-requested a review August 10, 2022 19:45

Merge branch 'main' into always-enforce-singularity-thresholds

71c9fca

AndyZe reviewed Aug 10, 2022

View reviewed changes

moveit_ros/moveit_servo/config/panda_simulated_config.yaml Outdated Show resolved Hide resolved

moveit_ros/moveit_servo/src/servo_parameters.cpp Outdated Show resolved Hide resolved

moveit_ros/moveit_servo/src/servo_parameters.cpp Outdated Show resolved Hide resolved

henrygerardmoore and others added 4 commits August 12, 2022 15:07

Remove outdated warning

085c39f

Co-authored-by: AndyZe <andyz@utexas.edu>

Removing inaccurate comment

4f0d4f9

Co-authored-by: AndyZe <andyz@utexas.edu>

making Andy's suggested changes, added some comments and defaults, mo…

de97459

…ved code block next to relevant singularity code

removed part of comment that does not apply any more

0f2a6ca

AndyZe approved these changes Aug 14, 2022

View reviewed changes

tylerjw approved these changes Aug 15, 2022

View reviewed changes

moveit_ros/moveit_servo/src/servo_parameters.cpp Show resolved Hide resolved

AndyZe reviewed Aug 15, 2022

View reviewed changes

moveit_ros/moveit_servo/src/servo_parameters.cpp Outdated Show resolved Hide resolved

Mention "deprecation" in the warning

52a77b7

AndyZe merged commit d4b3294 into moveit:main Aug 15, 2022

tylerjw added the backport-humble Mergify label that triggers a PR backport to Humble label Oct 28, 2022

mergify bot mentioned this pull request Oct 28, 2022

Enforce singularity threshold when moving away from a singularity (backport #620) #1649

Merged

tylerjw pushed a commit that referenced this pull request Oct 28, 2022

Enforce singularity threshold when moving away from a singularity (#620…

ad39107

…) (#1649)

This was referenced May 18, 2023

MoveIt servo doesnt move the robot moveit/moveit#3434

Closed

MoveIt Servo moving at vastly different cartesian speed than instructed #2191

Closed

Conversation

nbbrooks commented Aug 18, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Checklist

Uh oh!

AndyZe commented Aug 20, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

AndyZe commented Aug 20, 2021

Uh oh!

nbbrooks commented Sep 22, 2021

Uh oh!

Uh oh!

codecov bot commented Nov 8, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

nbbrooks commented Aug 1, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

henrygerardmoore commented Aug 5, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

AndyZe commented Aug 10, 2022

Uh oh!

henrygerardmoore commented Aug 12, 2022

Uh oh!

tylerjw left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

henrygerardmoore commented Aug 15, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

AndyZe commented Aug 15, 2022

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

nbbrooks commented Aug 18, 2021 •

edited

Loading

AndyZe commented Aug 20, 2021 •

edited

Loading

codecov bot commented Nov 8, 2021 •

edited

Loading

nbbrooks commented Aug 1, 2022 •

edited

Loading

henrygerardmoore commented Aug 5, 2022 •

edited

Loading

henrygerardmoore commented Aug 15, 2022 •

edited

Loading