Add pinocchio plugin

.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

@@ -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/include/kinematics_interface/kinematics_interface.hpp

@@ -119,11 +119,13 @@ class KinematicsInterface
 
   /**
    * \brief Calculates the difference between two Cartesian frames
-   * \param[in] x_a first Cartesian frame (x, y, z, wx, wy, wz, ww)
-   * \param[in] x_b second Cartesian frame (x, y, z, wx, wy, wz, ww)
+   * \param[in] x_a first Cartesian frame (x, y, z, qx, qy, qz, qw)
+   * \param[in] x_b second Cartesian frame (x, y, z, qx, qy, qz, qw)
    * \param[in] dt time interval over which the numerical differentiation takes place
-   * \param[out] delta_x  Cartesian deltas (x, y, z, wx, wy, wz)
+   * \param[out] delta_x  Cartesian deltas (vx, vy, vz, wx, wy, wz)
    * \return true if successful
+   *
+   * \note This method is independent of robot kinematics and the model loaded to the plugin
    */
   virtual bool calculate_frame_difference(
     Eigen::Matrix<double, 7, 1> & x_a, Eigen::Matrix<double, 7, 1> & x_b, double dt,

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,99 @@
+// 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;
+
+  bool calculate_frame_difference(
+    Eigen::Matrix<double, 7, 1> & x_a, Eigen::Matrix<double, 7, 1> & x_b, double dt,
+    Eigen::Matrix<double, 6, 1> & delta_x) 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="[email protected]">Bence Magyar</maintainer>
+  <maintainer email="[email protected]">Denis Štogl</maintainer>
+  <maintainer email="[email protected]">Christoph Froehlich</maintainer>
+  <maintainer email="[email protected]">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="[email protected]">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>