Add pinocchio plugin (backport #199)

.github/workflows/rolling-semi-binary-build-win.yml

@@ -24,5 +24,5 @@ jobs:
     uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-win-build.yml@master
     with:
       ros_distro: rolling
-      pixi_dependencies: typeguard jinja2 boost compilers
+      pixi_dependencies: typeguard jinja2 boost compilers octomap
       ninja_packages: rsl

README.md

@@ -3,7 +3,7 @@
 [![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0)
 [![codecov](https://codecov.io/gh/ros-controls/kinematics_interface/graph/badge.svg?token=NS73VKPG9V)](https://codecov.io/gh/ros-controls/kinematics_interface)
 
-This is a ROS 2 package for using C++ kinematics frameworks in the context of ROS 2 control. A kinematics interface is designed to allow ROS 2 controllers to control robots in Cartesian space. This package also contains a basic implementation of the interface using KDL.
+This is a ROS 2 package for using C++ kinematics frameworks in the context of ROS 2 control. A kinematics interface is designed to allow ROS 2 controllers to control robots in Cartesian space. This package also contains a basic implementation of the interface using KDL and a pinocchio-based implementation. Use  `kinematics_interface_pinocchio`  plugin when you need high-performance kinematics computations for complex robots, require efficient handling of large kinematic chains, or want to leverage Pinocchio's advanced features such as automatic differentiation and collision detection integration. The plugin supports damped least-squares Jacobian inversion for robust singularity handling.
 
 ## Contributing
 
@@ -14,10 +14,10 @@ If you are new to the project, please read the [contributing guide](https://cont
 
 ROS2 Distro | Branch | Build status | Documentation | Package Build
 :---------: | :----: | :----------: | :-----------: | :---------------:
-**Rolling** | [`master`](https://github.com/ros-controls/kinematics_interface/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-binary-build.yml) <br> [![Rolling Semi-Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-semi-binary-build.yml) <br> [![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Rdev__kinematics_interface__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Rdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/rolling/p/kinematics_interface/) <br> [API kdl](http://docs.ros.org/en/rolling/p/kinematics_interface_kdl/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary/)
-**Kilted** | [`master`](https://github.com/ros-controls/kinematics_interface/tree/master) | see above <br> [![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Kdev__kinematics_interface__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Kdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/kilted/p/kinematics_interface/) <br> [API kdl](http://docs.ros.org/en/kilted/p/kinematics_interface_kdl/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Kbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Kbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary/)
-**Jazzy** | [`jazzy`](https://github.com/ros-controls/kinematics_interface/tree/jazzy) | [![Jazzy Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-binary-build.yml) <br> [![Jazzy Semi-Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-semi-binary-build.yml) <br> [![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Jdev__kinematics_interface__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Jdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/jazzy/p/kinematics_interface/) <br> [API kdl](http://docs.ros.org/en/jazzy/p/kinematics_interface_kdl/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary/)
-**Humble** | [`humble`](https://github.com/ros-controls/kinematics_interface/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-binary-build.yml/badge.svg?branch=humble)](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-binary-build.yml) <br> [![Humble Semi-Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=humble)](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-semi-binary-build.yml) <br> [![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Hdev__kinematics_interface__ubuntu_jammy_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Hdev__kinematics_interface__ubuntu_jammy_amd64/) | [API](http://docs.ros.org/en/humble/p/kinematics_interface/) <br> [API kdl](http://docs.ros.org/en/humble/p/kinematics_interface_kdl/)  | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__kinematics_interface__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__kinematics_interface__ubuntu_jammy_amd64__binary/)
+**Rolling** | [`master`](https://github.com/ros-controls/kinematics_interface/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-binary-build.yml) <br> [![Rolling Semi-Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-semi-binary-build.yml) <br> [![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Rdev__kinematics_interface__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Rdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/rolling/p/kinematics_interface/) <br> [API kdl](http://docs.ros.org/en/rolling/p/kinematics_interface_kdl/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary/) <br> [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__kinematics_interface_kdl__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__kinematics_interface_kdl__ubuntu_noble_amd64__binary/) <br> [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__kinematics_interface_pinocchio__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__kinematics_interface_pinocchio__ubuntu_noble_amd64__binary/)
+**Kilted** | [`master`](https://github.com/ros-controls/kinematics_interface/tree/master) | see above <br> [![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Kdev__kinematics_interface__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Kdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/kilted/p/kinematics_interface/) <br> [API kdl](http://docs.ros.org/en/kilted/p/kinematics_interface_kdl/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Kbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Kbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary/) <br> [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__kinematics_interface_kdl__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__kinematics_interface_kdl__ubuntu_noble_amd64__binary/) <br> [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__kinematics_interface_pinocchio__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__kinematics_interface_pinocchio__ubuntu_noble_amd64__binary/)
+**Jazzy** | [`jazzy`](https://github.com/ros-controls/kinematics_interface/tree/jazzy) | [![Jazzy Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-binary-build.yml) <br> [![Jazzy Semi-Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-semi-binary-build.yml) <br> [![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Jdev__kinematics_interface__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Jdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/jazzy/p/kinematics_interface/) <br> [API kdl](http://docs.ros.org/en/jazzy/p/kinematics_interface_kdl/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary/)  <br> [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__kinematics_interface_kdl__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__kinematics_interface_kdl__ubuntu_noble_amd64__binary/)
+**Humble** | [`humble`](https://github.com/ros-controls/kinematics_interface/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-binary-build.yml/badge.svg?branch=humble)](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-binary-build.yml) <br> [![Humble Semi-Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=humble)](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-semi-binary-build.yml) <br> [![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Hdev__kinematics_interface__ubuntu_jammy_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Hdev__kinematics_interface__ubuntu_jammy_amd64/) | [API](http://docs.ros.org/en/humble/p/kinematics_interface/) <br> [API kdl](http://docs.ros.org/en/humble/p/kinematics_interface_kdl/)  | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__kinematics_interface__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__kinematics_interface__ubuntu_jammy_amd64__binary/) <br> [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__kinematics_interface_kdl__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__kinematics_interface_kdl__ubuntu_jammy_amd64__binary/)
 
 ## Acknowledgements
 

kinematics_interface_pinocchio/CHANGELOG.rst

@@ -0,0 +1,13 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package kinematics_interface_pinocchio
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.0.2 (2025-06-28)
+------------------
+* fixes to adapt to upstream changes in kinematics_interface
+* Contributors: David V. Lu, Saif Sidhik
+
+0.0.1 (2024-08-05)
+------------------
+* create a pinocchio kinematics plugin following ros2 kinematics_interface format
+* Contributors: Saif Sidhik

kinematics_interface_pinocchio/CMakeLists.txt

@@ -0,0 +1,69 @@
+cmake_minimum_required(VERSION 3.16)
+project(kinematics_interface_pinocchio LANGUAGES CXX)
+
+find_package(ros2_control_cmake REQUIRED)
+set_compiler_options()
+export_windows_symbols()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  kinematics_interface
+  pluginlib
+  eigen3_cmake_module
+  pinocchio
+)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+find_package(Eigen3 REQUIRED NO_MODULE)
+
+add_library(
+  kinematics_interface_pinocchio
+  SHARED
+  src/kinematics_interface_pinocchio.cpp
+)
+target_include_directories(kinematics_interface_pinocchio PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/kinematics_interface_pinocchio>
+)
+target_compile_features(kinematics_interface_pinocchio PUBLIC cxx_std_17)
+target_link_libraries(kinematics_interface_pinocchio PUBLIC
+  kinematics_interface::kinematics_interface
+  pluginlib::pluginlib
+  Eigen3::Eigen
+  pinocchio::pinocchio
+)
+
+pluginlib_export_plugin_description_file(kinematics_interface kinematics_interface_pinocchio.xml)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ament_add_gmock(
+    test_kinematics_interface_pinocchio
+    test/test_kinematics_interface_pinocchio.cpp
+  )
+  target_link_libraries(test_kinematics_interface_pinocchio
+    kinematics_interface_pinocchio
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+endif()
+
+install(
+  DIRECTORY include/
+  DESTINATION include/kinematics_interface_pinocchio
+)
+install(
+  TARGETS kinematics_interface_pinocchio
+  EXPORT export_kinematics_interface_pinocchio
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+ament_export_targets(export_kinematics_interface_pinocchio HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

kinematics_interface_pinocchio/include/kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp

@@ -0,0 +1,95 @@
+// Copyright (c) 2024, Saif Sidhik.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+/// \author: Saif Sidhik
+/// \description: Pinocchio plugin for kinematics interface
+
+#ifndef KINEMATICS_INTERFACE_PINOCCHIO__KINEMATICS_INTERFACE_PINOCCHIO_HPP_
+#define KINEMATICS_INTERFACE_PINOCCHIO__KINEMATICS_INTERFACE_PINOCCHIO_HPP_
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "eigen3/Eigen/Core"
+#include "eigen3/Eigen/LU"
+#include "kinematics_interface/kinematics_interface.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+
+#include "pinocchio/algorithm/frames.hpp"
+#include "pinocchio/algorithm/geometry.hpp"
+#include "pinocchio/algorithm/jacobian.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+
+namespace kinematics_interface_pinocchio
+{
+class KinematicsInterfacePinocchio : public kinematics_interface::KinematicsInterface
+{
+public:
+  bool initialize(
+    const std::string & robot_description,
+    std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
+    const std::string & param_namespace) override;
+
+  bool convert_cartesian_deltas_to_joint_deltas(
+    const Eigen::VectorXd & joint_pos, const Eigen::Matrix<double, 6, 1> & delta_x,
+    const std::string & link_name, Eigen::VectorXd & delta_theta) override;
+
+  bool convert_joint_deltas_to_cartesian_deltas(
+    const Eigen::VectorXd & joint_pos, const Eigen::VectorXd & delta_theta,
+    const std::string & link_name, Eigen::Matrix<double, 6, 1> & delta_x) override;
+
+  bool calculate_link_transform(
+    const Eigen::VectorXd & joint_pos, const std::string & link_name,
+    Eigen::Isometry3d & transform) override;
+
+  bool calculate_jacobian(
+    const Eigen::VectorXd & joint_pos, const std::string & link_name,
+    Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian) override;
+
+  bool calculate_jacobian_inverse(
+    const Eigen::VectorXd & joint_pos, const std::string & link_name,
+    Eigen::Matrix<double, Eigen::Dynamic, 6> & jacobian_inverse) override;
+
+private:
+  // verification methods
+  bool verify_initialized();
+  bool verify_link_name(const std::string & link_name);
+  bool verify_joint_vector(const Eigen::VectorXd & joint_vector);
+  bool verify_jacobian(const Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian);
+  bool verify_jacobian_inverse(const Eigen::Matrix<double, Eigen::Dynamic, 6> & jacobian_inverse);
+  bool verify_period(const double dt);
+
+  bool initialized = false;
+  std::string root_name_;
+  Eigen::Index num_joints_;
+
+  pinocchio::Model model_;
+  std::shared_ptr<pinocchio::Data> data_;
+  Eigen::VectorXd q_;
+  Eigen::MatrixXd jacobian_;
+  Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian_inverse_;
+  Eigen::MatrixXd frame_tf_;
+
+  std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface_;
+  std::unordered_map<std::string, int> link_name_map_;
+  double alpha;  // damping term for Jacobian inverse
+  Eigen::MatrixXd I;
+};
+
+}  // namespace kinematics_interface_pinocchio
+
+#endif  // KINEMATICS_INTERFACE_PINOCCHIO__KINEMATICS_INTERFACE_PINOCCHIO_HPP_

kinematics_interface_pinocchio/kinematics_interface_pinocchio.xml

@@ -0,0 +1,9 @@
+<library path="kinematics_interface_pinocchio">
+  <class name="kinematics_interface_pinocchio/KinematicsInterfacePinocchio"
+    type="kinematics_interface_pinocchio::KinematicsInterfacePinocchio"
+    base_class_type="kinematics_interface::KinematicsInterface">
+    <description>
+      Pinocchio plugin for ros2_control kinematics interface
+    </description>
+  </class>
+</library>

kinematics_interface_pinocchio/package.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>kinematics_interface_pinocchio</name>
+  <version>0.0.2</version>
+  <description>Pinocchio-based implementation of ros2_control kinematics interface</description>
+
+  <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
+  <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>
+  <maintainer email="christoph.froehlich@ait.ac.at">Christoph Froehlich</maintainer>
+  <maintainer email="sai.kishor@pal-robotics.com">Sai Kishor Kothakota</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <url type="website">https://control.ros.org</url>
+  <url type="bugtracker">https://github.com/ros-controls/ros2_controllers/issues</url>
+  <url type="repository">https://github.com/ros-controls/ros2_controllers/</url>
+
+  <author email="mail@saifsidhik.page">Saif Sidhik</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+
+  <build_depend>ros2_control_cmake</build_depend>
+
+  <depend>eigen</depend>
+  <depend>kinematics_interface</depend>
+  <depend>eigen3_cmake_module</depend>
+  <depend>pinocchio</depend>
+  <depend>pluginlib</depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>