[Servo] Fix Twist transformation

[ibrahiminfinite]

Description

Fixes #2150
Previous discussions on #2301

The problem had boiled down to:

How do we reconcile that some users (like myself) want the rotation-only part of the twist, whereas others (like those who reported #2150) want the actual translation+rotation portion of twisting the EE frame w.r.t. the base frame?

This PR addresses the above by adding the option for proper twist conversion while preserving the existing method of twist conversion (which is wrong).
The selection is made using the parameter apply_twist_commands_about_ee_frame.

@sea-bass
@AndyZe

[codecov]

Codecov Report

Patch coverage: 69.05% and project coverage change: +0.04% 🎉

Comparison is base (05f3501) 50.70% compared to head (11fdaf0) 50.73%.

❗ Current head 11fdaf0 differs from pull request most recent head be8860c. Consider uploading reports for the commit be8860c to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2311      +/-   ##
==========================================
+ Coverage   50.70%   50.73%   +0.04%     
==========================================
  Files         386      386              
  Lines       31922    31958      +36     
==========================================
+ Hits        16183    16212      +29     
- Misses      15739    15746       +7     

Files Changed	Coverage Δ
moveit_ros/moveit_servo/src/servo.cpp	63.88% <45.84%> (-2.24%)	⬇️
moveit_ros/moveit_servo/tests/test_integration.cpp	100.00% <100.00%> (ø)

... and 2 files with indirect coverage changes

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

[sea-bass]

Looks reasonable.

Got an unused variable failing CI here, but other small comments on naming and ... comments.

[AndyZe]

The code looks great and I agree with @sea-bass's comments. Will test it now.

[AndyZe]

Here's a video with a twist command in panda_link8 frame, planning frame is panda_link0. There is a combo of rotation and translation, so that's good. I have to say that the translation is in the opposite direction of what I expected!?

I made this test by modifying a few lines in servo_keyboard_input.cpp

[ibrahiminfinite]

I have to say that the translation is in the opposite direction of what I expected!?

The transformation pulled from TF2 was the wrong one ,T_command_planning instead of T_planning_command.

Also restored servo_keyboard_input demo with the option to switch between the planning and end effector frames.

[AndyZe]

It looks exactly correct now. Nice work @ibrahiminfinite !