Use kinematics plugin instead of inverse Jacobian for servo IK

[wyattrees]

Description

This changes servo to utilize a kinematics plugin if one is defined. If one is not present or doesn't support the group, default to inverse Jacobian

Also updated servo_example launch file to use MoveItConfigsBuilder.

To test, the current moveit_servo tutorial can be used as is. The modification of the servo_example launch file in this repository also includes a reference to kinematics.yaml for the servo node.

The kinematics plugin used by the servo node is defined via ROS parameters, which may be loaded from a yaml file. If using MoveItConfigsBuilder in your servo launch file (as is done in servo_example.launch.py in this PR), you can pass the necessary parameters as such:

moveit_config = (
        MoveItConfigsBuilder("moveit_resources_panda")
        .robot_description(file_path="config/panda.urdf.xacro")
        .to_moveit_configs()
    )

servo_node = Node(
        package="moveit_servo",
        executable="servo_node_main",
        parameters=[
            servo_params,
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics, # here is where kinematics plugin parameters are passed
        ],

In the above example, the kinematics.yaml file is taken from the moveit_resources repository in the workspace, specifically moveit_resources/panda_moveit_config/config/kinematics.yaml. By default, this yaml file defines the KDL plugin to be used. To use bio_ik as the solver, first clone the ros2 branch of the repository into your workspace. Then edit the above yaml file to contain the following:

panda_arm:
  kinematics_solver: bio_ik/BioIKKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05 # this timeout parameter is actually ignored by servo
  kinematics_solver_attempts: 1.0
  mode: gd_c # see bio_ik for other modes, but gd_c (gradient descent) has the best performance

The timeout parameter is ignored for servo, as it uses a fraction of the publish period instead.

The actual ROS parameter names that get passed by loading the yaml file are of the form robot_description_kinematics.<group_name>.<param_name>, e.g. robot_description_kinematics.panda_arm.kinematics_solver

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 #1434 (7bb054b) into main (2de48a7) will increase coverage by 0.02%.
The diff coverage is 82.06%.

❗ Current head 7bb054b differs from pull request most recent head 7435399. Consider uploading reports for the commit 7435399 to get more accurate results

@@            Coverage Diff             @@
##             main    #1434      +/-   ##
==========================================
+ Coverage   50.87%   50.88%   +0.02%     
==========================================
  Files         381      381              
  Lines       31740    31778      +38     
==========================================
+ Hits        16143    16168      +25     
- Misses      15597    15610      +13     

Impacted Files	Coverage Δ
moveit_ros/moveit_servo/src/servo_calcs.cpp	67.14% <82.06%> (+0.92%)	⬆️
...ning/robot_model_loader/src/robot_model_loader.cpp	77.09% <0.00%> (-0.69%)	⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp	45.29% <0.00%> (-0.43%)	⬇️
moveit_core/robot_state/src/robot_state.cpp	47.37% <0.00%> (-0.07%)	⬇️

---

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 b954380...7435399. Read the comment docs.

[AndyZe]

Can you put more detail in the PR description of how to test this with/without bio_ik?

I put a print statement in servo_calcs and I see that it's finding a kinematics plugin, but I'm not really sure where it found that plugin from or how to configure it ;)

If bio_ik is going to be available in MoveIt2 by default, then I think it should become the default in Servo and we can simplify this a lot.

[wyattrees]

@AndyZe I've added more to the description showing how to change the kinematics plugin.

[AndyZe]

Sorry for these very nitpicky changes. LGTM after this.

[wyattrees]

Found a bug in how rotations are applied while doing some singularity avoidance testing, we should hold off on merging for now.

[wyattrees]

@AndyZe I've fixed an issue where rotations were always being applied in the same frame--would you mind taking a second look at the recent changes (lines 613-633)?

[AndyZe]

I think these comments ought to be more generic. It's not necessarily the tip link reference frame, but rather whatever the IK reference frame is set to, right?

Otherwise the feature of switching from World to EE frame as the user's frame of reference (mentioned in the tutorial) would not work.

[wyattrees]

Yeah, it really should be more generic -- but it's actually more than just the comment. Really, we shouldn't just blindly use tf_moveit_to_ee_frame_, but rather a transform from what the IK solver knows as the base frame -> what the IK solver knows as the tip frame (which is likely the same as what servo knows as moveit -> ee frame, but not necessarily).

Switching of the reference frame for the incoming twist commands is already handled above in the same function, so either way it should work for any incoming frame of reference.

See the latest commit for my solution to this