Make getJacobian simpler and faster

[marioprats]

Description

This change makes getJacobian a little bit more readable (hopefully!), while also making it 2x faster, by removing string comparisons where possible and other little optimizations.
The jacobian benchmark shows a ~2x improvement with respect to HEAD (>500ns), and ~4x with respect to the KDL Jacobian, for the panda robot:

------------------------------------------------------------
Benchmark                  Time             CPU   Iterations
------------------------------------------------------------
BM_MoveItJacobian        248 ns          249 ns      2867707
BM_KDLJacobian          1099 ns         1100 ns       634762

The recently added Jacobian tests for the different types of joints are also passing.
Also manually tested this with Servo, which I know uses the MoveIt! Jacobian.

Note: a_pose_b is a notation I have used in the past to denote a transformation matrix to convert points in b to points in a, e.g: point_a = a_pose_b * point_b. Not sure if there's another established notation if MoveIt!. Happy to change to whatever makes more sense.

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]

Very cool!

[codecov]

Codecov Report

Attention: 8 lines in your changes are missing coverage. Please review.

Comparison is base (cc75143) 50.74% compared to head (eee6ffe) 50.77%.
Report is 2 commits behind head on main.

❗ Current head eee6ffe differs from pull request most recent head 9da0948. Consider uploading reports for the commit 9da0948 to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2389      +/-   ##
==========================================
+ Coverage   50.74%   50.77%   +0.03%     
==========================================
  Files         386      386              
  Lines       31971    31972       +1     
==========================================
+ Hits        16221    16230       +9     
+ Misses      15750    15742       -8     

Files	Coverage Δ
...bot_state/include/moveit/robot_state/robot_state.h	90.27% <ø> (+0.14%)	⬆️
moveit_core/robot_state/src/robot_state.cpp	51.28% <84.32%> (+0.78%)	⬆️

... and 4 files with indirect coverage changes

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

[sjahr]

Do you mind adding some comments in front of the main blocks to explain what is happening there? I think that will help maintaining this in the future

[marioprats]

Thanks for the reviews.
While going again through the code adding comments I realized of an issue when the joint group origin doesn't match the URDF origin. Added a test for it and should be ok now.