Exit loop when position can't change. (backport #2307)

[mergify]

This is an automatic backport of pull request #2307 done by Mergify.
Cherry-pick of 7c95367 has failed:

On branch mergify/bp/humble/pr-2307
Your branch is up to date with 'origin/humble'.

You are currently cherry-picking commit 7c95367a2.
  (fix conflicts and run "git cherry-pick --continue")
  (use "git cherry-pick --skip" to skip this patch)
  (use "git cherry-pick --abort" to cancel the cherry-pick operation)

Changes to be committed:
	modified:   moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp

Unmerged paths:
  (use "git add <file>..." to mark resolution)
	both modified:   moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp

To fix up this pull request, you can check it out locally. See documentation: https://docs.github.com/en/github/collaborating-with-pull-requests/reviewing-changes-in-pull-requests/checking-out-pull-requests-locally

---

Mergify commands and options

More conditions and actions can be found in the documentation.

You can also trigger Mergify actions by commenting on this pull request:

• @Mergifyio refresh will re-evaluate the rules
• @Mergifyio rebase will rebase this PR on its base branch
• @Mergifyio update will merge the base branch into this PR
• @Mergifyio backport <destination> will backport this PR on <destination> branch

Additionally, on Mergify dashboard you can:

• look at your merge queues
• generate the Mergify configuration with the config editor.

Finally, you can contact us on https://mergify.com

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Comparison is base (3144e6e) 50.95% compared to head (ea79739) 50.96%.

Additional details and impacted files

@@            Coverage Diff             @@
##           humble    #2309      +/-   ##
==========================================
+ Coverage   50.95%   50.96%   +0.02%     
==========================================
  Files         382      382              
  Lines       31894    31902       +8     
==========================================
+ Hits        16247    16256       +9     
+ Misses      15647    15646       -1     

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

[henningkayser]

I don't quite get why we are running into failing tests for humble, while main and iron worked well. @uavster would you mind taking a quick look at what might be causing this?

[uavster]

I don't quite get why we are running into failing tests for humble, while main and iron worked well. @uavster would you mind taking a quick look at what might be causing this?

The failing test is testSingleDofDiscontinuity, added by @AndyZe in #2084 . None of the code paths I added in #2307 are being run in that test (or their corresponding error messages would show up in the console like in the next tests), so I don't see the connection. @AndyZe do you know of any change in humble that would make your test fail?

[ RUN      ] time_optimal_trajectory_generation.testSingleDofDiscontinuity
    /home/runner/work/moveit2/moveit2/.work/target_ws/src/moveit2/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp:537: Failure
    The difference between trajectory.getAcceleration(time)[0] and max_accelerations[0] is 28, which exceeds 1e-3, where
    trajectory.getAcceleration(time)[0] evaluates to 0,
    max_accelerations[0] evaluates to 28, and
    1e-3 evaluates to 0.001.
    Time: 0
    /home/runner/work/moveit2/moveit2/.work/target_ws/src/moveit2/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp:537: Failure
    The difference between trajectory.getAcceleration(time)[0] and max_accelerations[0] is 4, which exceeds 1e-3, where
    trajectory.getAcceleration(time)[0] evaluates to 32,
    max_accelerations[0] evaluates to 28, and
    1e-3 evaluates to 0.001.
    Time: 0.01
    /home/runner/work/moveit2/moveit2/.work/target_ws/src/moveit2/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp:537: Failure
    The difference between trajectory.getAcceleration(time)[0] and max_accelerations[0] is 28, which exceeds 1e-3, where
    trajectory.getAcceleration(time)[0] evaluates to 0,
    max_accelerations[0] evaluates to 28, and
    1e-3 evaluates to 0.001.
    Time: 0.02
    [  FAILED  ] time_optimal_trajectory_generation.testSingleDofDiscontinuity (0 ms)

[aditya2592]

Thank you for creating this backport. Is there a chance that it can be merged soon? We were seeing the issue of TOTG getting stuck without any logging to indicate whats happening and were wondering if this is the root cause.

[aditya2592]

@uavster @AndyZe I wanted to check again if this PR could be merged to Humble. It is hard to use TOTG on our robot fleet with this bug on Humble

[uavster]

@uavster @AndyZe I wanted to check again if this PR could be merged to Humble. It is hard to use TOTG on our robot fleet with this bug on Humble

To unblock @aditya2592, we could remove testSingleDofDiscontinuity, which is unrelated to the changes in this PR. Does that sound ok to you @AndyZe ?

[AndyZe]

Sure.

[uavster]

Sorry I missed this. New backport without the failing test in #2646.