Fix trajectory unwind bug

[mechwiz]

Description

This PR fixes some unwinding/bounds issues for continuous joints.

1. If a trajectory that is being unwound from a reference-state starts (i.e. has an initial waypoint) where the continuous joint has a value above 2PI or below -2PI as could be the case when a trajectory from MTC is generated, I have noticed in a project that I was working on that the running_offset can be off by a multiple of 2PI leading to 2PI jumps in the command for that continous joint. This occurs because in the current implementation, the running_offset is not only being calculated using the offset between the reference state and the unwound reference state but also the offset between the initial waypoint (which may not be unwound) and the unwound current state which is not valid and is like comparing apples and oranges. To fix this, the initial waypoint of the trajectory also has its bounds enforced so that when it's being compared to the reference value, it's comparing apples to apples. Furthermore, I added logic to ensure that the running_offset takes into account when the unwound reference state and the unwound initial waypoint are not on the same side of PI by adding 2PI or subtracting 2PI accordingly.
2. When executeAndMonitor is called, unwinding is not handled properly if there are multiple trajectories within the plan as would happen when executing a task or a serial container from MTC. The way unwinding should work is that the current plan_component's trajectory (item i) should be unwound based on any group-matching previous plan_component's trajectory (item j). The current implementation for some unknown reason is starting at index zero and working up (which is not valid since there could be a matching group plan-component but it might not be the one that precedes component "i") instead of starting at the previous plan compoment and working down.

Fixes #1777

@AndyZe

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]

Codecov Report

Base: 50.32% // Head: 50.36% // Increases project coverage by +0.04% 🎉

Coverage data is based on head (c0bcb27) compared to base (fff361b).
Patch coverage: 86.67% of modified lines in pull request are covered.

❗ Current head c0bcb27 differs from pull request most recent head 6afb74f. Consider uploading reports for the commit 6afb74f to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #1772      +/-   ##
==========================================
+ Coverage   50.32%   50.36%   +0.04%     
==========================================
  Files         374      374              
  Lines       31329    31283      -46     
==========================================
- Hits        15762    15751      -11     
+ Misses      15567    15532      -35     

Impacted Files	Coverage Δ
...include/moveit/robot_trajectory/robot_trajectory.h	98.67% <ø> (ø)
...eit_core/robot_trajectory/src/robot_trajectory.cpp	66.48% <86.67%> (+9.83%)	⬆️
...rajectory_processing/src/ruckig_traj_smoothing.cpp	90.97% <0.00%> (-0.61%)	⬇️
...veit_core/robot_model/src/revolute_joint_model.cpp	90.08% <0.00%> (-0.57%)	⬇️
...ics_plugin_loader/src/kinematics_plugin_loader.cpp	82.76% <0.00%> (-0.57%)	⬇️
...bot_model/include/moveit/robot_model/joint_model.h	82.44% <0.00%> (-0.46%)	⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp	45.26% <0.00%> (-0.43%)	⬇️
...bot_model/include/moveit/robot_model/robot_model.h	90.39% <0.00%> (-0.35%)	⬇️
moveit_core/robot_model/src/joint_model_group.cpp	65.12% <0.00%> (-0.24%)	⬇️
...mpl_interface/src/model_based_planning_context.cpp	50.57% <0.00%> (ø)
... and 2 more

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

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[JafarAbdi]

Do you mind adding a test that reproduces the fixed issue?

[AndyZe]

I can write a couple tests, as part of reviewing this anyway.

[mechwiz]

Thanks Andy!
Sorry haven't gotten to the test writing part yet.

[AndyZe]

Started writing a test, got distracted by other bugs :/ Will come back to this. Or maybe we merge it as it is, then fix the other bugs and write tests.

[AndyZe]

I'm OK merging this without tests. unwind() clearly has some other bugs that need to be fixed, anyway. See #1778

We need to wait for one more review, though.

[mechwiz]

I'm cool with merging as is as well!

[mechwiz]

Okedoke, I think I fixed up the other unwind bugs and got the unwind test case to be successful

[sjahr]

@mechwiz Can you please rebase this branch, so we can merge it

[mechwiz]

Done