Fix logic in CheckStartStateBounds adapter

[sea-bass]

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

[codecov-commenter]

⚠️ Please install the 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]

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

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]

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]

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]

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]

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]

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]

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]

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]

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!

[sjahr]

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

[sea-bass]

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]

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