fix memcpy bug in copyJointToOMPLState in ompl interface

[JeroenDM]

Description

A single line bug fix (that was punching far above its weight class in terms of causing debugging fun.)

Example of the issue in an online C++ shell.
Previous related discussion with @tylerjw, where he created the example that I use in the C++ shell link above.

What (I think) happens in this piece of code

The joint data for a specific joint is copied from MoveIt to OMPL, it looks something like this:

Robot model memory (array of doubles), the bold values are copied.
| x1 | x2 | ... | ... | y1 | y2 | y3| ... | ... | ... | z1 | .. |

Copies into OMPL state memory (also an array of doubles)
| x1 | x2 | y1 | y2 | y3 | z1 |

The | ... | represent joint values for fixed joints (or mimic joints?) that are not relevant for OMPL.

Test

I extended an existing test in test_state_space.cpp to make it fail without this fix. I mirrored the approach of the other tests. But maybe it is not the best way to test this. If anyone agrees, I can add another test that just copies fixed, known joint values.

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

Merging #2239 into master will decrease coverage by 0.35%.
The diff coverage is 100.00%.

@@            Coverage Diff             @@
##           master    #2239      +/-   ##
==========================================
- Coverage   57.76%   57.41%   -0.36%     
==========================================
  Files         326      326              
  Lines       25629    25629              
==========================================
- Hits        14805    14714      -91     
- Misses      10824    10915      +91     

Impacted Files	Coverage Δ
...e/src/parameterization/model_based_state_space.cpp	75.17% <100.00%> (+2.06%)	⬆️
...ls/include/moveit/py_bindings_tools/gil_releaser.h	0.00% <0.00%> (-100.00%)	⬇️
.../include/moveit/py_bindings_tools/py_conversions.h	0.00% <0.00%> (-86.67%)	⬇️
...s/include/moveit/py_bindings_tools/serialize_msg.h	0.00% <0.00%> (-80.00%)	⬇️
...ove_group_interface/src/wrap_python_move_group.cpp	31.97% <0.00%> (-10.98%)	⬇️
.../move_group_interface/src/move_group_interface.cpp	44.44% <0.00%> (-3.64%)	⬇️
...raint_samplers/src/default_constraint_samplers.cpp	82.90% <0.00%> (+0.36%)	⬆️
...meterization/work_space/pose_model_state_space.cpp	82.99% <0.00%> (+0.68%)	⬆️
...ipulation/pick_place/src/manipulation_pipeline.cpp	77.35% <0.00%> (+0.94%)	⬆️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 7d40d82...a59a8a3. Read the comment docs.

[tylerjw]

Thank you for figuring this out. Also, thank you so much for making a test for this. I left some minor comments on a few things if you want to clean up some stuff in the test. If not, you are welcome to ignore those comments as they all have to do with how the test was written in the first place and you are just following what was already there.

[tylerjw]

This is failing CI for unrelated issues with python.

[JeroenDM]

@tylerjw Agree to all :) (in the spirit of leaving things better than you found them). Thanks for the review!

Yeah, I went through the CI, but it's not ideal to go through at the moment... :)

[tylerjw]

lgtm, thank you

[tylerjw]

Thank you for the cleanup. I'll let someone else review this before we merge and hopefully by then we can fix the python test issue.

[mamoll]

Ouch, that's a nasty bug. Thanks for fixing this and writing a test.

[rhaschke]

Well spotted! Thanks.