diff --git a/.github/workflows/rolling-semi-binary-build-win.yml b/.github/workflows/rolling-semi-binary-build-win.yml
index 8723c69..25e227c 100644
--- a/.github/workflows/rolling-semi-binary-build-win.yml
+++ b/.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
diff --git a/README.md b/README.md
index 96dbded..29181de 100644
--- a/README.md
+++ b/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) | [](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-binary-build.yml)
[](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-semi-binary-build.yml)
[](https://build.ros2.org/job/Rdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/rolling/p/kinematics_interface/)
[API kdl](http://docs.ros.org/en/rolling/p/kinematics_interface_kdl/) | [](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
[](https://build.ros2.org/job/Kdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/kilted/p/kinematics_interface/)
[API kdl](http://docs.ros.org/en/kilted/p/kinematics_interface_kdl/) | [](https://build.ros2.org/job/Kbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary/)
-**Jazzy** | [`jazzy`](https://github.com/ros-controls/kinematics_interface/tree/jazzy) | [](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-binary-build.yml)
[](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-semi-binary-build.yml)
[](https://build.ros2.org/job/Jdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/jazzy/p/kinematics_interface/)
[API kdl](http://docs.ros.org/en/jazzy/p/kinematics_interface_kdl/) | [](https://build.ros2.org/job/Jbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary/)
-**Humble** | [`humble`](https://github.com/ros-controls/kinematics_interface/tree/humble) | [](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-binary-build.yml)
[](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-semi-binary-build.yml)
[](https://build.ros2.org/job/Hdev__kinematics_interface__ubuntu_jammy_amd64/) | [API](http://docs.ros.org/en/humble/p/kinematics_interface/)
[API kdl](http://docs.ros.org/en/humble/p/kinematics_interface_kdl/) | [](https://build.ros2.org/job/Hbin_uJ64__kinematics_interface__ubuntu_jammy_amd64__binary/)
+**Rolling** | [`master`](https://github.com/ros-controls/kinematics_interface/tree/master) | [](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-binary-build.yml)
[](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-semi-binary-build.yml)
[](https://build.ros2.org/job/Rdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/rolling/p/kinematics_interface/)
[API kdl](http://docs.ros.org/en/rolling/p/kinematics_interface_kdl/) | [](https://build.ros2.org/job/Rbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary/)
[](https://build.ros2.org/job/Rbin_uN64__kinematics_interface_kdl__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
[](https://build.ros2.org/job/Kdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/kilted/p/kinematics_interface/)
[API kdl](http://docs.ros.org/en/kilted/p/kinematics_interface_kdl/) | [](https://build.ros2.org/job/Kbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary/)
[](https://build.ros2.org/job/Rbin_uN64__kinematics_interface_kdl__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) | [](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-binary-build.yml)
[](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-semi-binary-build.yml)
[](https://build.ros2.org/job/Jdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/jazzy/p/kinematics_interface/)
[API kdl](http://docs.ros.org/en/jazzy/p/kinematics_interface_kdl/) | [](https://build.ros2.org/job/Jbin_uN64__kinematics_interface__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) | [](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-binary-build.yml)
[](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-semi-binary-build.yml)
[](https://build.ros2.org/job/Hdev__kinematics_interface__ubuntu_jammy_amd64/) | [API](http://docs.ros.org/en/humble/p/kinematics_interface/)
[API kdl](http://docs.ros.org/en/humble/p/kinematics_interface_kdl/) | [](https://build.ros2.org/job/Hbin_uJ64__kinematics_interface__ubuntu_jammy_amd64__binary/)
[](https://build.ros2.org/job/Hbin_uJ64__kinematics_interface_kdl__ubuntu_jammy_amd64__binary/)
## Acknowledgements
diff --git a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp
index b55d669..aa1cf47 100644
--- a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp
+++ b/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 & x_a, Eigen::Matrix & x_b, double dt,
diff --git a/kinematics_interface_pinocchio/CHANGELOG.rst b/kinematics_interface_pinocchio/CHANGELOG.rst
new file mode 100644
index 0000000..88e6441
--- /dev/null
+++ b/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
diff --git a/kinematics_interface_pinocchio/CMakeLists.txt b/kinematics_interface_pinocchio/CMakeLists.txt
new file mode 100644
index 0000000..89911e7
--- /dev/null
+++ b/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
+ $
+ $
+)
+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()
diff --git a/kinematics_interface_pinocchio/include/kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp b/kinematics_interface_pinocchio/include/kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp
new file mode 100644
index 0000000..855536c
--- /dev/null
+++ b/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
+#include
+#include
+#include
+
+#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 parameters_interface,
+ const std::string & param_namespace) override;
+
+ bool convert_cartesian_deltas_to_joint_deltas(
+ const Eigen::VectorXd & joint_pos, const Eigen::Matrix & 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 & 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 & jacobian) override;
+
+ bool calculate_jacobian_inverse(
+ const Eigen::VectorXd & joint_pos, const std::string & link_name,
+ Eigen::Matrix & jacobian_inverse) override;
+
+ bool calculate_frame_difference(
+ Eigen::Matrix & x_a, Eigen::Matrix & x_b, double dt,
+ Eigen::Matrix & 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 & jacobian);
+ bool verify_jacobian_inverse(const Eigen::Matrix & 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 data_;
+ Eigen::VectorXd q_;
+ Eigen::MatrixXd jacobian_;
+ Eigen::Matrix jacobian_inverse_;
+ Eigen::MatrixXd frame_tf_;
+
+ std::shared_ptr parameters_interface_;
+ std::unordered_map 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_
diff --git a/kinematics_interface_pinocchio/kinematics_interface_pinocchio.xml b/kinematics_interface_pinocchio/kinematics_interface_pinocchio.xml
new file mode 100644
index 0000000..c6bd1a7
--- /dev/null
+++ b/kinematics_interface_pinocchio/kinematics_interface_pinocchio.xml
@@ -0,0 +1,9 @@
+
+
+
+ Pinocchio plugin for ros2_control kinematics interface
+
+
+
diff --git a/kinematics_interface_pinocchio/package.xml b/kinematics_interface_pinocchio/package.xml
new file mode 100644
index 0000000..f33cc82
--- /dev/null
+++ b/kinematics_interface_pinocchio/package.xml
@@ -0,0 +1,38 @@
+
+
+
+ kinematics_interface_pinocchio
+ 0.0.2
+ Pinocchio-based implementation of ros2_control kinematics interface
+
+ Bence Magyar
+ Denis Štogl
+ Christoph Froehlich
+ Sai Kishor Kothakota
+
+ Apache License 2.0
+
+ https://control.ros.org
+ https://github.com/ros-controls/ros2_controllers/issues
+ https://github.com/ros-controls/ros2_controllers/
+
+ Saif Sidhik
+
+ ament_cmake
+ eigen3_cmake_module
+
+ ros2_control_cmake
+
+ eigen
+ kinematics_interface
+ eigen3_cmake_module
+ pinocchio
+ pluginlib
+
+ ament_cmake_gmock
+ ros2_control_test_assets
+
+
+ ament_cmake
+
+
diff --git a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp
new file mode 100644
index 0000000..569ce2e
--- /dev/null
+++ b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp
@@ -0,0 +1,509 @@
+// 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
+
+#include "kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp"
+
+#include
+#include
+
+namespace kinematics_interface_pinocchio
+{
+rclcpp::Logger LOGGER = rclcpp::get_logger("kinematics_interface_pinocchio");
+
+bool KinematicsInterfacePinocchio::initialize(
+ const std::string & robot_description,
+ std::shared_ptr parameters_interface,
+ const std::string & param_namespace)
+{
+ // track initialization plugin
+ initialized = true;
+
+ // get parameters
+ std::string ns = !param_namespace.empty() ? param_namespace + "." : "";
+
+ std::string robot_description_local;
+ if (robot_description.empty())
+ {
+ // If the robot_description input argument is empty, try to get the
+ // robot_description from the node's parameters.
+ auto robot_param = rclcpp::Parameter();
+ if (!parameters_interface->get_parameter("robot_description", robot_param))
+ {
+ RCLCPP_ERROR(LOGGER, "parameter robot_description not set in kinematics_interface_pinocchio");
+ return false;
+ }
+ robot_description_local = robot_param.as_string();
+ }
+ else
+ {
+ robot_description_local = robot_description;
+ }
+ // get verbose flag
+ bool verbose = false;
+ auto verbose_param = rclcpp::Parameter("verbose", verbose);
+ if (parameters_interface->has_parameter(ns + "verbose"))
+ {
+ parameters_interface->get_parameter(ns + "verbose", verbose_param);
+ }
+ verbose = verbose_param.as_bool();
+
+ // get alpha damping term
+ auto alpha_param = rclcpp::Parameter("alpha", 0.000005);
+ if (parameters_interface->has_parameter(ns + "alpha"))
+ {
+ parameters_interface->get_parameter(ns + "alpha", alpha_param);
+ }
+ alpha = alpha_param.as_double();
+
+ // get end-effector name
+ auto end_effector_name_param = rclcpp::Parameter("tip");
+ if (parameters_interface->has_parameter(ns + "tip"))
+ {
+ parameters_interface->get_parameter(ns + "tip", end_effector_name_param);
+ }
+ else
+ {
+ RCLCPP_ERROR(LOGGER, "Failed to find end effector name parameter [tip].");
+ return false;
+ }
+ std::string end_effector_name = end_effector_name_param.as_string();
+
+ // get model from parameter root name (if set)
+ pinocchio::Model full_model;
+ auto base_param = rclcpp::Parameter();
+ if (parameters_interface->has_parameter(ns + "base"))
+ {
+ parameters_interface->get_parameter(ns + "base", base_param);
+ root_name_ = base_param.as_string();
+ }
+
+ // load the model
+ try
+ {
+ pinocchio::urdf::buildModelFromXML(robot_description_local, full_model, verbose, true);
+ }
+ catch (const std::exception & e)
+ {
+ RCLCPP_ERROR(LOGGER, "Error parsing URDF to build Pinocchio model: %s", e.what());
+ return false;
+ }
+ if (root_name_.empty())
+ {
+ // look for the first frame whose parent joint’s parent is universe (0)
+ for (const auto & frame : full_model.frames)
+ {
+ if (frame.parent == 0 && frame.type == pinocchio::FrameType::BODY) // BODY frame = link frame
+ {
+ root_name_ = frame.name;
+ break;
+ }
+ }
+
+ // Fallback if somehow not found
+ if (root_name_.empty()) root_name_ = full_model.frames[0].name; // usually "universe"
+ }
+ if (!full_model.existFrame(root_name_))
+ {
+ RCLCPP_ERROR(LOGGER, "failed to find robot root '%s' ", root_name_.c_str());
+ return false;
+ }
+ if (!full_model.existFrame(end_effector_name))
+ {
+ RCLCPP_ERROR(LOGGER, "failed to find robot end effector '%s'", end_effector_name.c_str());
+ return false;
+ }
+
+ // create reduced model by locking joints
+ auto const get_descendants = [](const pinocchio::Model & model, pinocchio::JointIndex root)
+ -> std::unordered_set
+ {
+ std::unordered_set descendants;
+ std::queue q;
+ q.push(root);
+ while (!q.empty())
+ {
+ auto j = q.front();
+ q.pop();
+ for (pinocchio::JointIndex k = 1; k < static_cast(model.njoints); ++k)
+ {
+ if (model.parents[k] == j)
+ {
+ descendants.insert(k);
+ q.push(k);
+ }
+ }
+ }
+ return descendants;
+ };
+ auto const get_chain_joints =
+ [](
+ const pinocchio::Model & model, pinocchio::JointIndex base_joint,
+ pinocchio::JointIndex tool_joint) -> std::vector
+ {
+ std::vector chain;
+ // Start from tool_joint and go upward to base_joint
+ pinocchio::JointIndex current = tool_joint;
+ while (current > 0)
+ {
+ chain.push_back(current);
+ if (current == base_joint) break;
+ current = model.parents[current];
+ }
+ std::reverse(chain.begin(), chain.end());
+ // Remove base_joint itself → chain starts AFTER base link
+ if (!chain.empty() && chain.front() == base_joint) chain.erase(chain.begin());
+ return chain;
+ };
+
+ pinocchio::JointIndex base_joint_id = 0; // default to universe
+ if (root_name_ != "universe")
+ {
+ pinocchio::FrameIndex base_frame_id = full_model.getFrameId(root_name_);
+ const pinocchio::Frame & base_frame = full_model.frames[base_frame_id];
+ base_joint_id = base_frame.parent; // the joint to which this frame is attached
+ }
+
+ pinocchio::FrameIndex end_effector_frame_id = full_model.getFrameId(end_effector_name);
+ const pinocchio::Frame & end_effector_frame = full_model.frames[end_effector_frame_id];
+ pinocchio::JointIndex tool_joint_id =
+ end_effector_frame.parent; // the joint to which this frame is attached
+
+ // Validate: is tool under base?
+ auto base_descendants = get_descendants(full_model, base_joint_id);
+ if (base_descendants.find(tool_joint_id) == base_descendants.end())
+ {
+ RCLCPP_WARN(
+ LOGGER, "Tool '%s' is not a descendant of base '%s'", end_effector_name.c_str(),
+ root_name_.c_str());
+ auto tip_descendants = get_descendants(full_model, tool_joint_id);
+ if (tip_descendants.find(base_joint_id) == tip_descendants.end())
+ {
+ RCLCPP_ERROR(
+ LOGGER, "Base frame '%s' is also not a descendant of tip '%s' — cannot form a chain.",
+ end_effector_name.c_str(), root_name_.c_str());
+ return false;
+ }
+ else
+ {
+ std::swap(root_name_, end_effector_name);
+ RCLCPP_WARN(LOGGER, "Swapping tool and base frame");
+ }
+ }
+
+ // Get joints in the subchain
+ auto chain_joints = get_chain_joints(full_model, base_joint_id, tool_joint_id);
+ RCLCPP_INFO(
+ LOGGER, "Found chain from '%s' to tool frame '%s' with %zu joint(s)", root_name_.c_str(),
+ end_effector_name.c_str(), chain_joints.size());
+ std::unordered_set chain_set(chain_joints.begin(), chain_joints.end());
+
+ // Build list of joints to lock
+ std::vector locked_joints;
+ for (pinocchio::JointIndex jid = 1; jid < static_cast(full_model.njoints);
+ ++jid)
+ {
+ if (chain_set.find(jid) == chain_set.end())
+ {
+ locked_joints.push_back(jid);
+ }
+ }
+ Eigen::VectorXd q_fixed =
+ Eigen::VectorXd::Zero(full_model.nq); // actual value is not important for kinematics
+ model_ = pinocchio::buildReducedModel(full_model, locked_joints, q_fixed);
+
+ // allocate dynamic memory
+ data_ = std::make_shared(model_);
+ num_joints_ = static_cast(model_.nq);
+ q_.resize(num_joints_);
+ I = Eigen::MatrixXd(num_joints_, num_joints_);
+ I.setIdentity();
+ jacobian_.resize(6, num_joints_);
+ jacobian_inverse_.resize(num_joints_, 6);
+
+ return true;
+}
+
+bool KinematicsInterfacePinocchio::convert_joint_deltas_to_cartesian_deltas(
+ const Eigen::Matrix & joint_pos,
+ const Eigen::Matrix & delta_theta, const std::string & link_name,
+ Eigen::Matrix & delta_x)
+{
+ // verify inputs
+ if (
+ !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
+ !verify_joint_vector(delta_theta))
+ {
+ RCLCPP_ERROR(LOGGER, "Input verification failed in '%s'", FUNCTION_SIGNATURE);
+ return false;
+ }
+
+ // get joint array
+ q_ = joint_pos;
+
+ // calculate Jacobian
+ const auto ee_frame_id = model_.getFrameId(link_name);
+ pinocchio::computeFrameJacobian(model_, *data_, q_, ee_frame_id, jacobian_);
+ delta_x = jacobian_ * delta_theta;
+
+ return true;
+}
+
+bool KinematicsInterfacePinocchio::convert_cartesian_deltas_to_joint_deltas(
+ const Eigen::Matrix & joint_pos,
+ const Eigen::Matrix & delta_x, const std::string & link_name,
+ Eigen::Matrix & delta_theta)
+{
+ // verify inputs
+ if (
+ !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
+ !verify_joint_vector(delta_theta))
+ {
+ RCLCPP_ERROR(LOGGER, "Input verification failed in '%s'", FUNCTION_SIGNATURE);
+ return false;
+ }
+
+ // calculate Jacobian inverse
+ if (!calculate_jacobian_inverse(joint_pos, link_name, jacobian_inverse_))
+ {
+ return false;
+ }
+
+ delta_theta = jacobian_inverse_ * delta_x;
+
+ return true;
+}
+
+bool KinematicsInterfacePinocchio::calculate_jacobian(
+ const Eigen::Matrix & joint_pos, const std::string & link_name,
+ Eigen::Matrix & jacobian)
+{
+ // verify inputs
+ if (
+ !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
+ !verify_jacobian(jacobian))
+ {
+ RCLCPP_ERROR(LOGGER, "Input verification failed in '%s'", FUNCTION_SIGNATURE);
+ return false;
+ }
+
+ // get joint array
+ q_ = joint_pos;
+
+ // calculate Jacobian
+ const auto ee_frame_id = model_.getFrameId(link_name);
+ pinocchio::computeFrameJacobian(model_, *data_, q_, ee_frame_id, jacobian_);
+ jacobian = jacobian_;
+
+ return true;
+}
+
+bool KinematicsInterfacePinocchio::calculate_jacobian_inverse(
+ const Eigen::Matrix & joint_pos, const std::string & link_name,
+ Eigen::Matrix & jacobian_inverse)
+{
+ // verify inputs
+ if (
+ !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
+ !verify_jacobian_inverse(jacobian_inverse))
+ {
+ RCLCPP_ERROR(LOGGER, "Input verification failed in '%s'", FUNCTION_SIGNATURE);
+ return false;
+ }
+
+ // get joint array
+ q_ = joint_pos;
+
+ // calculate Jacobian
+ const auto ee_frame_id = model_.getFrameId(link_name);
+ pinocchio::computeFrameJacobian(model_, *data_, q_, ee_frame_id, jacobian_);
+ // damped inverse
+ jacobian_inverse_ =
+ (jacobian_.transpose() * jacobian_ + alpha * I).inverse() * jacobian_.transpose();
+
+ // TODO(anyone): fix sizes of jacobian inverse
+ jacobian_inverse = jacobian_inverse_;
+
+ return true;
+}
+
+bool KinematicsInterfacePinocchio::calculate_link_transform(
+ const Eigen::Matrix & joint_pos, const std::string & link_name,
+ Eigen::Isometry3d & transform)
+{
+ // verify inputs
+ if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name))
+ {
+ RCLCPP_ERROR(
+ LOGGER, "Verification failed: %s",
+ !verify_initialized() ? "Not initialized"
+ : !verify_joint_vector(joint_pos) ? "Invalid joint vector"
+ : !verify_link_name(link_name) ? "Invalid link name"
+ : "Unknown error");
+ return false;
+ }
+
+ // get joint array
+ q_ = joint_pos;
+
+ // reset transform_vec
+ transform.setIdentity();
+
+ // special case: since the root is not in the robot tree, need to return identity transform
+ if (link_name == root_name_)
+ {
+ return true;
+ }
+
+ // calculate Jacobian
+ const auto ee_frame_id = model_.getFrameId(link_name);
+
+ // Perform forward kinematics and get a transform.
+ pinocchio::framesForwardKinematics(model_, *data_, q_);
+ frame_tf_ = data_->oMf[ee_frame_id].toHomogeneousMatrix();
+
+ transform.linear() = frame_tf_.block<3, 3>(0, 0);
+ transform.translation() = frame_tf_.block<3, 1>(0, 3);
+ return true;
+}
+
+bool KinematicsInterfacePinocchio::calculate_frame_difference(
+ Eigen::Matrix & x_a, Eigen::Matrix & x_b, double dt,
+ Eigen::Matrix & delta_x)
+{
+ // verify inputs
+ if (!verify_period(dt))
+ {
+ RCLCPP_ERROR(LOGGER, "Input verification failed in '%s'", FUNCTION_SIGNATURE);
+ return false;
+ }
+
+ // translation
+ const Eigen::Vector3d t_a = x_a.head<3>();
+ const Eigen::Vector3d t_b = x_b.head<3>();
+
+ // quaternions: input layout is [tx,ty,tz, qx,qy,qz,qw]
+ Eigen::Quaterniond q_a(x_a(6), x_a(3), x_a(4), x_a(5)); // (w,x,y,z)
+ Eigen::Quaterniond q_b(x_b(6), x_b(3), x_b(4), x_b(5));
+ q_a.normalize();
+ q_b.normalize();
+
+ const Eigen::Matrix3d R_a = q_a.toRotationMatrix();
+ const Eigen::Matrix3d R_b = q_b.toRotationMatrix();
+
+ // KDL-like behaviour:
+ // - linear part: simple difference of positions
+ // - angular part: log3(R_a^T * R_b) (axis-angle vector)
+ const Eigen::Vector3d linear = (t_b - t_a) / dt;
+
+ const Eigen::Matrix3d R_rel = R_a.transpose() * R_b;
+ const Eigen::Vector3d angular = pinocchio::log3(R_rel) / dt; // 3-vector (axis * angle)
+
+ delta_x.head<3>() = linear;
+ delta_x.tail<3>() = angular;
+
+ return true;
+}
+
+bool KinematicsInterfacePinocchio::verify_link_name(const std::string & link_name)
+{
+ if (link_name == root_name_)
+ {
+ return true;
+ }
+ if (!model_.existBodyName(link_name))
+ {
+ std::string links;
+ for (size_t i = 0; i < model_.frames.size(); ++i)
+ {
+ links += "\n" + model_.frames[i].name;
+ }
+ RCLCPP_ERROR(
+ LOGGER, "The link %s was not found in the robot chain. Available links are: %s",
+ link_name.c_str(), links.c_str());
+ return false;
+ }
+ return true;
+}
+
+bool KinematicsInterfacePinocchio::verify_joint_vector(const Eigen::VectorXd & joint_vector)
+{
+ if (joint_vector.size() != num_joints_)
+ {
+ RCLCPP_ERROR(
+ LOGGER, "Invalid joint vector size (%zu). Expected size is %zu.", joint_vector.size(),
+ num_joints_);
+ return false;
+ }
+ return true;
+}
+
+bool KinematicsInterfacePinocchio::verify_initialized()
+{
+ // check if interface is initialized
+ if (!initialized)
+ {
+ RCLCPP_ERROR(
+ LOGGER,
+ "The Pinocchio kinematics plugin was not initialized. Ensure you called the initialize "
+ "method.");
+ return false;
+ }
+ return true;
+}
+
+bool KinematicsInterfacePinocchio::verify_jacobian(
+ const Eigen::Matrix & jacobian)
+{
+ if (jacobian.rows() != jacobian_.rows() || jacobian.cols() != jacobian_.cols())
+ {
+ RCLCPP_ERROR(
+ LOGGER, "The size of the jacobian (%zu, %zu) does not match the required size of (%zu, %zu)",
+ jacobian.rows(), jacobian.cols(), jacobian_.rows(), jacobian_.cols());
+ return false;
+ }
+ return true;
+}
+
+bool KinematicsInterfacePinocchio::verify_jacobian_inverse(
+ const Eigen::Matrix & jacobian_inverse)
+{
+ if (jacobian_inverse.rows() != jacobian_.cols() || jacobian_inverse.cols() != jacobian_.rows())
+ {
+ RCLCPP_ERROR(
+ LOGGER, "The size of the jacobian (%zu, %zu) does not match the required size of (%zu, %zu)",
+ jacobian_inverse.rows(), jacobian_inverse.cols(), jacobian_.cols(), jacobian_.rows());
+ return false;
+ }
+ return true;
+}
+
+bool KinematicsInterfacePinocchio::verify_period(const double dt)
+{
+ if (dt < 0)
+ {
+ RCLCPP_ERROR(LOGGER, "The period (%f) must be a non-negative number", dt);
+ return false;
+ }
+ return true;
+}
+
+} // namespace kinematics_interface_pinocchio
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+ kinematics_interface_pinocchio::KinematicsInterfacePinocchio,
+ kinematics_interface::KinematicsInterface)
diff --git a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp
new file mode 100644
index 0000000..1265fa0
--- /dev/null
+++ b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp
@@ -0,0 +1,34 @@
+// Copyright (c) 2025, Austrian Institute of Technology.
+//
+// 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: Christoph Froehlich
+
+#include
+
+#include "kinematics_interface_tests/kinematics_interface_common_tests.hpp"
+
+struct PluginPinocchio
+{
+ static std::string Name()
+ {
+ return "kinematics_interface_pinocchio/KinematicsInterfacePinocchio";
+ }
+ static void set_custom_node_parameters(rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+ {
+ node->declare_parameter("alpha", 0.005);
+ }
+};
+
+using MyTypes = ::testing::Types;
+INSTANTIATE_TYPED_TEST_SUITE_P(PluginTestPinocchio, TestPlugin, MyTypes);