Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix logic in CheckStartStateBounds adapter #3143

Merged
merged 8 commits into from
Dec 4, 2024
Merged

Conversation

sea-bass
Copy link
Contributor

@sea-bass sea-bass commented Nov 30, 2024

Description

This PR aims to fix the logic in the CheckStartStateBounds adapter, as @chama1176 pointed out.

It also adds some basic unit tests to prevent further breakages and maintain expected behavior.

Closes #2924

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

@sea-bass sea-bass added the backport-jazzy Mergify label that triggers a PR backport to Jazzy label Nov 30, 2024
@codecov-commenter
Copy link

codecov-commenter commented Nov 30, 2024

⚠️ Please install the 'codecov app svg image' to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 45.57%. Comparing base (e7872eb) to head (42ff99e).
Report is 1 commits behind head on main.

❗ Your organization needs to install the Codecov GitHub app to enable full functionality.

Additional details and impacted files
@@            Coverage Diff             @@
##             main    #3143      +/-   ##
==========================================
- Coverage   46.00%   45.57%   -0.43%     
==========================================
  Files         483      482       -1     
  Lines       40632    40431     -201     
==========================================
- Hits        18689    18421     -268     
- Misses      21943    22010      +67     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

@sea-bass
Copy link
Contributor Author

@sjahr I also want to check whether it's intentional that we're not checking the bounds for planar/floating joints at all?

Is this because URDF also doesn't specify limits for this joint type?

@mikeferguson
Copy link
Contributor

mikeferguson commented Nov 30, 2024

Posted this in the issue ticket - but:

The original logic was:

  • for each joint model:
    • if REVOLUTE & continuous, call enforceBounds() [OK]
    • else
      • if PLANAR - normalizeRotation [OK]
      • else FLOATING - normalizeRotation [OK]
  • read parameters
  • for each joint model:
    • apply the satisfiesBounds logic

I think we actually just need to pull the "default" case outside the switch statement and do it for all joints?

This is the original code - before being replaced: https://github.com/moveit/moveit2/blob/e064a8497a72b7e417f911fa2b4cb87d7c809a59/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp

@mikeferguson
Copy link
Contributor

The fact that this is broken, also makes me think we should really have a test for this... since it seems pretty important to not violate the joint limits in a motion planning software...

@sea-bass
Copy link
Contributor Author

The fact that this is broken, also makes me think we should really have a test for this... since it seems pretty important to not violate the joint limits in a motion planning software...

I also didn't look at this... and there are no tests for any of the adapters? Oof. Yeah, I'd like to add one.

@mikeferguson
Copy link
Contributor

Looking at the original adapter - that code is basically unchanged since 2013 other than formatting and then porting to ROS 2... guess it didn't need any tests back then!

@sea-bass
Copy link
Contributor Author

sea-bass commented Nov 30, 2024

Hah, yeah. It shouldn't be that bad to create a planning scene from e.g. a PR2 or Panda and add some quick tests for the "revolute joint out of bounds" case.

It's more the other paths like continuous joints that will likely require making more robot models. Though PR2 I assume has a planar joint available...

I'm internally debating how much to bite off here...

@mikeferguson
Copy link
Contributor

PR2 has continuous joints in the forearm/wrist roll and it looks like the SRDF in moveit_resources has a planar "world_joint"

@sea-bass
Copy link
Contributor Author

sea-bass commented Nov 30, 2024

I added some tests and the logic is... more broken than we thought.

@sjahr @mikeferguson do you guys mind lending me a hand w.r.t. what the expected behavior should be? Right now:

  1. If a joint is out of bounds, you get an error in the logger only, but the adapter still succeeds (as reported in the original issue).
  2. If the above is true, and fix_start_state parameter is true, the joint states are not actually fixed by moving them close to some bound. It seems like the old code even had a max error parameter that was removed since?

It appears that fix_start_state only pertains to the continuous/planar/revolute joints normalizing rotation, but not to actual joint variables being out of bounds.

@mikeferguson
Copy link
Contributor

mikeferguson commented Nov 30, 2024

So, my understanding (taking into account the notes in 2849) is that, the following should happen:

  • REVOLUTE:
    • continuous - make sure the value is inside the limits (usually +/-pi) - if fix_start_state then basically wrap it, else fail
    • non-continuous - make sure the value is inside the limits - never fix it though (this is the change in behavior from the old days)
  • PLANAR: make sure yaw is normalized - if fix_start_state then fix it, else fail.
  • FLOATING: make sure quaternion rotation is normalized - if fix_start_state then fix it, else fail.

So then, the logic really is:

  • first loop through all joints, check PLANAR, FLOATING, continuous REVOLUTE - figure out how to fix them if needed, then mark 'changed_req' (by the way, this used to be "change_req" before the refactor - not sure why the "d" was added - it's not really past tense when it is set...)
  • important note: when PLANAR, FLOATING or continuous REVOLUTE joints are "change_req" we actually update the start_state variable.
  • The call to start_state.satisifesBounds(jmodel) will then find non-continuous joints that are invalid (we already updated start_state with the fixes above). It looks like we need to add a proper exit here and fail if this isn't true. There is also a huge print statement here - we shouldn't duplicate that, so I think the logic should actually be:
if (!start_state.satisfiesBounds(jmodel) || (change_req && !params.fix_start_state))
{
  // print and fail
}

to catch the case that we need to fix the start state for PLANAR, FLOATING or continuous REVOLUTE.

  • Once we've made it past that now larger if statement, we can simply return success, we're all set...

@sea-bass
Copy link
Contributor Author

sea-bass commented Nov 30, 2024

That's about in line with what I was thinking. Meaning the doc in the parameters YAML also needs to be fixed.

Will wait til Sebastian confirms this behavior and will clean up / fill out the test suite as well.

Thanks!

Copy link
Contributor

@sjahr sjahr left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for fixing this (and sorry for breaking it 🙈). My comment's aren't blocking the merge. I think this looks good!

@sjahr sjahr enabled auto-merge December 4, 2024 07:44
@sjahr sjahr added this pull request to the merge queue Dec 4, 2024
@github-merge-queue github-merge-queue bot removed this pull request from the merge queue due to no response for status checks Dec 4, 2024
@sea-bass sea-bass added this pull request to the merge queue Dec 4, 2024
@github-merge-queue github-merge-queue bot removed this pull request from the merge queue due to failed status checks Dec 4, 2024
@sea-bass
Copy link
Contributor Author

sea-bass commented Dec 4, 2024

Uhh... why is a bunch of new stuff failing suddenly?

Will we ever get some peace in this repo? Lol

Weirdly enough, the checks pass here, just not in the merge queue.

@sea-bass sea-bass added this pull request to the merge queue Dec 4, 2024
@sea-bass
Copy link
Contributor Author

sea-bass commented Dec 4, 2024

I deleted some GHA caches and the merqe queue jobs seem to be working now. 🤷🏻

Merged via the queue into main with commit f82cdcd Dec 4, 2024
12 checks passed
@sea-bass sea-bass deleted the fix-start-state-bounds branch December 4, 2024 15:09
mergify bot pushed a commit that referenced this pull request Dec 4, 2024
* Fix logic in CheckStartStateBoundsAdapter

* Explicit fallthrough

* Fix switch statement and add basic test scaffolding

* Remove extra variable

* Fix logic and add tests for continuous joints

* Validate changed start state

* Appeasing our Lord and Savior clang-tidy

* Update status message

(cherry picked from commit f82cdcd)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
backport-jazzy Mergify label that triggers a PR backport to Jazzy
Projects
None yet
Development

Successfully merging this pull request may close these issues.

CheckStartStateBounds does not support REVOLUTE and NOT Continuous type joints
4 participants