KDL and pinocchio use different reference frames for jacobian calculation result

[baconbrot]

Describe the bug
KDL and Pinocchio implementations of the kinematics interface are using different reference frames for the calculated jacobian. KDL gives jacobian in base/world frame while pinocchio gives jacobian in local/ee frame:

KDL kinematics interface call to JntToJac calculates jacobian in base frame of chain, as described here.

Pinocchio kinematics interface call to computeFrameJacobian expresses the jacobian in the LOCAL frame by default, as shown here.

To Reproduce
The following Dockerfile (ai-generated) contains a quick demonstration of the problem:

FROM ros:jazzy

RUN apt-get update && apt-get install -y \
    ros-jazzy-kinematics-interface \
    ros-jazzy-kinematics-interface-kdl \
    ros-jazzy-kinematics-interface-pinocchio \
    ros-jazzy-ros2-control-test-assets \
    libeigen3-dev \
  && rm -rf /var/lib/apt/lists/*

RUN cat <<'EOF' > /demo.cpp
#include <iostream>
#include <memory>
#include <string>

#include <rclcpp/rclcpp.hpp>
#include <Eigen/Core>

#include <ros2_control_test_assets/descriptions.hpp>
#include <kinematics_interface_kdl/kinematics_interface_kdl/kinematics_interface_kdl.hpp>
#include <kinematics_interface_pinocchio/kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp>

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("jacobian_comparison_demo");

  const std::string urdf = ros2_control_test_assets::minimal_robot_urdf;
  const std::string tip  = "link3";  // end of the 3-DOF chain
  const int n_dof        = 3;

  node->declare_parameter("tip", tip);

  Eigen::VectorXd q(n_dof);
  q << 0.5, -0.3, 0.8;  // rad

  Eigen::Matrix<double, 6, Eigen::Dynamic> J_kdl(6, n_dof);
  Eigen::Matrix<double, 6, Eigen::Dynamic> J_pin(6, n_dof);

  // KDL plugin: Jacobian in the BASE frame
  {
    kinematics_interface_kdl::KinematicsInterfaceKDL kin;
    if (!kin.initialize(urdf, node->get_node_parameters_interface(), "")) {
      std::cerr << "[ERROR] KDL initialize failed\n";
      return 1;
    }
    if (!kin.calculate_jacobian(q, tip, J_kdl)) {
      std::cerr << "[ERROR] KDL calculate_jacobian failed\n";
      return 1;
    }
  }

  // Pinocchio plugin: Jacobian in the LOCAL/ee frame
  {
    kinematics_interface_pinocchio::KinematicsInterfacePinocchio kin;
    if (!kin.initialize(urdf, node->get_node_parameters_interface(), "")) {
      std::cerr << "[ERROR] Pinocchio initialize failed\n";
      return 1;
    }
    if (!kin.calculate_jacobian(q, tip, J_pin)) {
      std::cerr << "[ERROR] Pinocchio calculate_jacobian failed\n";
      return 1;
    }
  }

  const Eigen::Matrix<double, 6, Eigen::Dynamic> diff = J_pin - J_kdl;

  std::cout << "\nq = [" << q.transpose() << "] rad\n";
  std::cout << "\nKDL Jacobian (base frame):\n"             << J_kdl << "\n";
  std::cout << "\nPinocchio Jacobian (local / EE frame):\n" << J_pin << "\n";
  std::cout << "\nDifference (Pinocchio - KDL):\n"          << diff  << "\n";
  std::cout << "\nFrobenius norm of difference: " << diff.norm() << "\n\n";

  rclcpp::shutdown();
  return 0;
}
EOF

RUN . /opt/ros/jazzy/setup.sh && \
    g++ -std=c++17 /demo.cpp \
      -I/opt/ros/jazzy/include \
      -I/opt/ros/jazzy/include/rclcpp \
      -I/opt/ros/jazzy/include/rcl \
      -I/opt/ros/jazzy/include/rcl_action \
      -I/opt/ros/jazzy/include/rcl_interfaces \
      -I/opt/ros/jazzy/include/rcl_lifecycle \
      -I/opt/ros/jazzy/include/rcl_logging_interface \
      -I/opt/ros/jazzy/include/rcl_yaml_param_parser \
      -I/opt/ros/jazzy/include/rcutils \
      -I/opt/ros/jazzy/include/rmw \
      -I/opt/ros/jazzy/include/builtin_interfaces \
      -I/opt/ros/jazzy/include/rosidl_runtime_c \
      -I/opt/ros/jazzy/include/rosidl_runtime_cpp \
      -I/opt/ros/jazzy/include/rosidl_typesupport_interface \
      -I/opt/ros/jazzy/include/rosidl_typesupport_introspection_c \
      -I/opt/ros/jazzy/include/rosidl_typesupport_introspection_cpp \
      -I/opt/ros/jazzy/include/rosidl_dynamic_typesupport \
      -I/opt/ros/jazzy/include/tracetools \
      -I/opt/ros/jazzy/include/rcpputils \
      -I/opt/ros/jazzy/include/ament_index_cpp \
      -I/opt/ros/jazzy/include/statistics_msgs \
      -I/opt/ros/jazzy/include/libstatistics_collector \
      -I/opt/ros/jazzy/include/lifecycle_msgs \
      -I/opt/ros/jazzy/include/service_msgs \
      -I/opt/ros/jazzy/include/type_description_interfaces \
      -I/opt/ros/jazzy/include/composition_interfaces \
      -I/opt/ros/jazzy/include/rclcpp_action \
      -I/opt/ros/jazzy/include/rclcpp_lifecycle \
      -I/opt/ros/jazzy/include/action_msgs \
      -I/opt/ros/jazzy/include/unique_identifier_msgs \
      -I/opt/ros/jazzy/include/std_msgs \
      -I/opt/ros/jazzy/include/pluginlib \
      -I/opt/ros/jazzy/include/class_loader \
      -I/opt/ros/jazzy/include/kinematics_interface \
      -I/opt/ros/jazzy/include/kinematics_interface_kdl \
      -I/opt/ros/jazzy/include/kinematics_interface_pinocchio \
      -I/opt/ros/jazzy/include/kdl_parser \
      -I/opt/ros/jazzy/include/tf2_eigen_kdl \
      -I/opt/ros/jazzy/include/tf2_kdl \
      -I/opt/ros/jazzy/include/tf2 \
      -I/opt/ros/jazzy/include/pinocchio \
      -I/opt/ros/jazzy/include/ros2_control_test_assets \
      -I/opt/ros/jazzy/include/urdfdom_headers \
      -I/opt/ros/jazzy/include/urdf \
      -I/usr/include/eigen3 \
      -L/opt/ros/jazzy/lib \
      -lkinematics_interface_kdl \
      -lkinematics_interface_pinocchio \
      -lorocos-kdl \
      -lrclcpp \
      -lrcutils \
      -lrcl \
      -lrmw \
      -Wl,-rpath,/opt/ros/jazzy/lib \
      -o /demo

CMD ["/demo"]

Build docker build -f <Dockerfile> -t test
Run docker run --rm test

Example output:

q = [ 0.5 -0.3  0.8] rad

KDL Jacobian (base frame):
        0         0         0
 -1.31354  -0.88206         0
 0.611022 -0.178802         0
        1         1         1
        0         0         0
        0         0         0

Pinocchio Jacobian (local / EE frame):
       0        0        0
 1.43544  0.64562        0
0.195553 0.627036        0
       1        1        1
       0        0        0
       0        0        0

Difference (Pinocchio - KDL):
        0         0         0
  2.74899   1.52768         0
-0.415469  0.805838         0
        0         0         0
        0         0         0
        0         0         0

Frobenius norm of difference: 3.27303

Expected behavior
Swapping the kinematics interface plugin from KDL to pinocchio (or other way) should not break jacobian-dependent calculations.

Environment (please complete the following information):

• OS: Ubuntu 24.04
• Version Jazzy

Additional context
Pinocchio offers a 5 argument and 6 argument computeFrameJacobian method. The 6 argument method allows to pass an additional reference frame argument. Using the 6 argument method, the reference frame could be set to WORLD to align with KDL calculations.

[christophfroehlich]

This sounds reasonable. Have you tried the 6-argument method, does it give the same result as KDL? Do you mind opening a PR for fixing this?

[baconbrot]

Using the 6-argument method with LOCAL_WORLD_ALIGNED (instead of WORLD) is the correct choice:

- pinocchio::computeFrameJacobian(model_, *data_, q_, ee_frame_id, jacobian_);
+ pinocchio::computeFrameJacobian(model_, *data_, q_, ee_frame_id, pinocchio::LOCAL_WORLD_ALIGNED, jacobian_);

Resulting in 0 difference between kdl and pinocchio:

q = [ 0.5 -0.3  0.8] rad

KDL Jacobian (base frame):
        0         0         0
 -1.31354  -0.88206         0
 0.611022 -0.178802         0
        1         1         1
        0         0         0
        0         0         0

Pinocchio Jacobian (LOCAL_WORLD_ALIGNED):
        0         0         0
 -1.31354  -0.88206         0
 0.611022 -0.178802         0
        1         1         1
        0         0         0
        0         0         0

Difference (Pinocchio - KDL):
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0

Frobenius norm of difference: 0

I have to check if the other methods in pinocchio kinematics interface are also expressed w.r.t a different frame compared to kdl.
How to proceed with a PR from here, given that aligning Pinocchio and kdl calculations would be a breaking change? I.e. could a fix be backported to jazzy?

[christophfroehlich]

Unsure about that. There are no released packages dependent on it, I'll consider this as a bug and it should be fixed. Can you also improve the docstring of the plugin description, to make this explicit?

[baconbrot]

Alright, I will open a PR including the fix and updated docstring. I will also check if I can extend the test suite to validate that the current and possible future kinematics interface implementations match expected outputs/frames.

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.