From 7f28338bc3eadcfaab938f5f36c7849e94101682 Mon Sep 17 00:00:00 2001 From: JustaGist Date: Mon, 5 Aug 2024 12:11:05 +0100 Subject: [PATCH 01/27] create a pinocchio kinematics plugin following ros2 kinematics_interface format --- .../.github/dependabot.yml | 20 ++ kinematics_interface_pinocchio/CMakeLists.txt | 70 +++++ kinematics_interface_pinocchio/LICENSE | 202 ++++++++++++++ kinematics_interface_pinocchio/README.md | 16 ++ .../kinematics_interface_pinocchio.hpp | 87 ++++++ .../kinematics_interface_pinocchio.xml | 9 + kinematics_interface_pinocchio/package.xml | 26 ++ .../src/kinematics_interface_pinocchio.cpp | 247 ++++++++++++++++++ .../test_kinematics_interface_pinocchio.cpp | 179 +++++++++++++ 9 files changed, 856 insertions(+) create mode 100644 kinematics_interface_pinocchio/.github/dependabot.yml create mode 100644 kinematics_interface_pinocchio/CMakeLists.txt create mode 100644 kinematics_interface_pinocchio/LICENSE create mode 100644 kinematics_interface_pinocchio/README.md create mode 100644 kinematics_interface_pinocchio/include/kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp create mode 100644 kinematics_interface_pinocchio/kinematics_interface_pinocchio.xml create mode 100644 kinematics_interface_pinocchio/package.xml create mode 100644 kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp create mode 100644 kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp diff --git a/kinematics_interface_pinocchio/.github/dependabot.yml b/kinematics_interface_pinocchio/.github/dependabot.yml new file mode 100644 index 0000000..ae63b0d --- /dev/null +++ b/kinematics_interface_pinocchio/.github/dependabot.yml @@ -0,0 +1,20 @@ +# To get started with Dependabot version updates, you'll need to specify which +# package ecosystems to update and where the package manifests are located. +# Please see the documentation for all configuration options: +# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates + +version: 2 +updates: + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "monthly" + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "monthly" + target-branch: "main" diff --git a/kinematics_interface_pinocchio/CMakeLists.txt b/kinematics_interface_pinocchio/CMakeLists.txt new file mode 100644 index 0000000..9431537 --- /dev/null +++ b/kinematics_interface_pinocchio/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.16) +project(kinematics_interface_pinocchio LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +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) +ament_target_dependencies(kinematics_interface_pinocchio PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +target_link_libraries(kinematics_interface_pinocchio PUBLIC + 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) + + # TODO: Use target_link_libraries once ros2_control_test_assets' + # CMake include export is fixed + ament_target_dependencies(test_kinematics_interface_pinocchio 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/LICENSE b/kinematics_interface_pinocchio/LICENSE new file mode 100644 index 0000000..fb9b7c7 --- /dev/null +++ b/kinematics_interface_pinocchio/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright 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. diff --git a/kinematics_interface_pinocchio/README.md b/kinematics_interface_pinocchio/README.md new file mode 100644 index 0000000..0146423 --- /dev/null +++ b/kinematics_interface_pinocchio/README.md @@ -0,0 +1,16 @@ +# kinematics_interface_pinocchio +[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) +[![ROS Version](https://img.shields.io/badge/ROS-Humble-brightgreen.svg?logo=ros)](https://docs.ros.org/en/humble/index.html) + +A [Pinocchio](https://github.com/stack-of-tasks/pinocchio)-based [ROS2 Kinematics Interface](https://github.com/ros-controls/kinematics_interface) implementation. This can be used instead of the default [KDL implementation](https://github.com/ros-controls/kinematics_interface/tree/master/kinematics_interface_kdl) shipped with default ROS installations. + +Requires pinocchio to be installed: `apt install ros-humble-pinocchio`. + +## Roadmap + +- [x] Replicate KDL implemenations and capabilities with Pinocchio +- [x] Package as a ROS2 plugin which can be used in place of `kinematics_interface_kdl`. +- [ ] Benchmark and compare performances +- [ ] Allow arbitrary frames to be used as base (currently uses root joint from model) +- [ ] Add support for floating-base robots +- [ ] Add support for continuous joints and other composite joints supported by Pinocchio 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..828f51e --- /dev/null +++ b/kinematics_interface_pinocchio/include/kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp @@ -0,0 +1,87 @@ +// 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( + std::shared_ptr parameters_interface, + const std::string & end_effector_name) 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; + +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 initialized = false; + std::string root_name_; + size_t num_joints_; + + pinocchio::Model model_; + std::shared_ptr data_; + Eigen::VectorXd q_; + Eigen::MatrixXd jacobian_; + 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..32ceae4 --- /dev/null +++ b/kinematics_interface_pinocchio/kinematics_interface_pinocchio.xml @@ -0,0 +1,9 @@ + + + + Pinocchio plugin for ros2_control kinematics interface + + + \ No newline at end of file diff --git a/kinematics_interface_pinocchio/package.xml b/kinematics_interface_pinocchio/package.xml new file mode 100644 index 0000000..e81fc7d --- /dev/null +++ b/kinematics_interface_pinocchio/package.xml @@ -0,0 +1,26 @@ + + + + kinematics_interface_pinocchio + 1.1.0 + Pinocchio-based implementation of ros2_control kinematics interface + Saif Sidhik + Apache License 2.0 + Saif Sidhik + + ament_cmake + eigen3_cmake_module + + eigen + kinematics_interface + eigen3_cmake_module + pinocchio + pluginlib + + ament_cmake_gmock + ros2_control_test_assets + + + ament_cmake + + \ No newline at end of file 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..2393a0d --- /dev/null +++ b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp @@ -0,0 +1,247 @@ +// Copyright (c) 2022, PickNik, Inc. +// +// 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" + +namespace kinematics_interface_pinocchio +{ +rclcpp::Logger LOGGER = rclcpp::get_logger("kinematics_interface_pinocchio"); + +bool KinematicsInterfacePinocchio::initialize( + std::shared_ptr parameters_interface, + const std::string & end_effector_name) +{ + // track initialization plugin + initialized = true; + + // get robot description + auto robot_param = rclcpp::Parameter(); + if (!parameters_interface->get_parameter("robot_description", robot_param)) + { + RCLCPP_ERROR(LOGGER, "parameter robot_description not set"); + return false; + } + auto robot_description = robot_param.as_string(); + // get alpha damping term + auto alpha_param = rclcpp::Parameter("alpha", 0.000005); + if (parameters_interface->has_parameter("alpha")) + { + parameters_interface->get_parameter("alpha", alpha_param); + } + alpha = alpha_param.as_double(); + + + // get root name + auto base_param = rclcpp::Parameter(); + if (parameters_interface->has_parameter("base")) + { + parameters_interface->get_parameter("base", base_param); + root_name_ = base_param.as_string(); + } + else + { + root_name_ = model_.frames[0].name; + } + // TODO: only handling fixed base now + model_ = pinocchio::urdf::buildModelFromXML(robot_description, /*root_name_,*/ model_, true); + + + // allocate dynamics memory + data_ = std::make_shared(model_); + num_joints_ = model_.nq; // TODO: handle floating base + q_.resize(num_joints_); + I = Eigen::MatrixXd(num_joints_, num_joints_); + I.setIdentity(); + jacobian_.resize(6, num_joints_); + + 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)) + { + 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)) + { + 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 + Eigen::Matrix J_inverse = + (jacobian_.transpose() * jacobian_ + alpha * I).inverse() * jacobian_.transpose(); + delta_theta = J_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)) + { + 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_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)) + { + 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::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 (static_cast(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; +} + +} // 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..325231f --- /dev/null +++ b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp @@ -0,0 +1,179 @@ +// Copyright (c) 2022, PickNik, Inc. +// +// 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: Paul Gesel + +#include +#include +#include "kinematics_interface/kinematics_interface.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +class TestPinocchioPlugin : public ::testing::Test +{ +public: + std::shared_ptr> ik_loader_; + std::shared_ptr ik_; + std::shared_ptr node_; + std::string end_effector_ = "link2"; + + void SetUp() + { + // init ros + rclcpp::init(0, nullptr); + node_ = std::make_shared("test_node"); + std::string plugin_name = "kinematics_interface_pinocchio/KinematicsInterfacePinocchio"; + ik_loader_ = + std::make_shared>( + "kinematics_interface", "kinematics_interface::KinematicsInterface"); + ik_ = std::unique_ptr( + ik_loader_->createUnmanagedInstance(plugin_name)); + } + + void TearDown() + { + // shutdown ros + rclcpp::shutdown(); + } + + void loadURDFParameter() + { + auto urdf = std::string(ros2_control_test_assets::urdf_head) + + std::string(ros2_control_test_assets::urdf_tail); + rclcpp::Parameter param("robot_description", urdf); + node_->declare_parameter("robot_description", ""); + node_->set_parameter(param); + } + + void loadAlphaParameter() + { + rclcpp::Parameter param("alpha", 0.005); + node_->declare_parameter("alpha", 0.005); + node_->set_parameter(param); + } +}; + +TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) +{ + // load robot description and alpha to parameter server + loadURDFParameter(); + loadAlphaParameter(); + + // initialize the plugin + ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + + // calculate end effector transform + Eigen::Matrix pos = Eigen::Matrix::Zero(); + Eigen::Isometry3d end_effector_transform; + ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); + + // convert cartesian delta to joint delta + Eigen::Matrix delta_x = Eigen::Matrix::Zero(); + delta_x[2] = 1; + Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); + ASSERT_TRUE( + ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); + + // convert joint delta to cartesian delta + Eigen::Matrix delta_x_est; + ASSERT_TRUE( + ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); + + // Ensure kinematics math is correct + for (size_t i = 0; i < static_cast(delta_x.size()); ++i) + { + ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); + } +} + +TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) +{ + // load robot description and alpha to parameter server + loadURDFParameter(); + loadAlphaParameter(); + + // initialize the plugin + ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + + // calculate end effector transform + std::vector pos = {0, 0}; + Eigen::Isometry3d end_effector_transform; + ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); + + // convert cartesian delta to joint delta + std::vector delta_x = {0, 0, 0, 0, 0, 0}; + delta_x[2] = 1; + std::vector delta_theta = {0, 0}; + ASSERT_TRUE( + ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); + + // convert joint delta to cartesian delta + std::vector delta_x_est(6); + ASSERT_TRUE( + ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); + + // Ensure kinematics math is correct + for (size_t i = 0; i < static_cast(delta_x.size()); ++i) + { + ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); + } +} + +TEST_F(TestPinocchioPlugin, incorrect_input_sizes) +{ + // load robot description and alpha to parameter server + loadURDFParameter(); + loadAlphaParameter(); + + // initialize the plugin + ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + + // define correct values + Eigen::Matrix pos = Eigen::Matrix::Zero(); + Eigen::Isometry3d end_effector_transform; + Eigen::Matrix delta_x = Eigen::Matrix::Zero(); + delta_x[2] = 1; + Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); + Eigen::Matrix delta_x_est; + + // wrong size input vector + Eigen::Matrix vec_5 = Eigen::Matrix::Zero(); + + // calculate transform + ASSERT_FALSE(ik_->calculate_link_transform(vec_5, end_effector_, end_effector_transform)); + ASSERT_FALSE(ik_->calculate_link_transform(pos, "link_not_in_model", end_effector_transform)); + + // convert cartesian delta to joint delta + ASSERT_FALSE( + ik_->convert_cartesian_deltas_to_joint_deltas(vec_5, delta_x, end_effector_, delta_theta)); + ASSERT_FALSE( + ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, "link_not_in_model", delta_theta)); + ASSERT_FALSE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, vec_5)); + + // convert joint delta to cartesian delta + ASSERT_FALSE( + ik_->convert_joint_deltas_to_cartesian_deltas(vec_5, delta_theta, end_effector_, delta_x_est)); + ASSERT_FALSE( + ik_->convert_joint_deltas_to_cartesian_deltas(pos, vec_5, end_effector_, delta_x_est)); + ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas( + pos, delta_theta, "link_not_in_model", delta_x_est)); +} + +TEST_F(TestPinocchioPlugin, Pinocchio_plugin_no_robot_description) +{ + // load alpha to parameter server + loadAlphaParameter(); + ASSERT_FALSE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); +} From a36ec9a8c18870e4c6dd376ecdc426575b20a79f Mon Sep 17 00:00:00 2001 From: JustaGist Date: Mon, 5 Aug 2024 17:39:52 +0100 Subject: [PATCH 02/27] add changelog and versioning --- kinematics_interface_pinocchio/CHANGELOG.rst | 8 ++++++++ kinematics_interface_pinocchio/package.xml | 2 +- 2 files changed, 9 insertions(+), 1 deletion(-) create mode 100644 kinematics_interface_pinocchio/CHANGELOG.rst diff --git a/kinematics_interface_pinocchio/CHANGELOG.rst b/kinematics_interface_pinocchio/CHANGELOG.rst new file mode 100644 index 0000000..230f421 --- /dev/null +++ b/kinematics_interface_pinocchio/CHANGELOG.rst @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package kinematics_interface_pinocchio +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.1 +----- +* create a pinocchio kinematics plugin following ros2 kinematics_interface format +* Contributors: Saif Sidhik diff --git a/kinematics_interface_pinocchio/package.xml b/kinematics_interface_pinocchio/package.xml index e81fc7d..c907d16 100644 --- a/kinematics_interface_pinocchio/package.xml +++ b/kinematics_interface_pinocchio/package.xml @@ -2,7 +2,7 @@ kinematics_interface_pinocchio - 1.1.0 + 0.0.1 Pinocchio-based implementation of ros2_control kinematics interface Saif Sidhik Apache License 2.0 From fe238db1110f9e16359ebfe7b84c8d962a21a3d3 Mon Sep 17 00:00:00 2001 From: JustaGist Date: Tue, 24 Dec 2024 21:18:12 +0000 Subject: [PATCH 03/27] update readme; fix formatting; update license --- kinematics_interface_pinocchio/README.md | 2 + .../kinematics_interface_pinocchio.hpp | 74 ++-- .../src/kinematics_interface_pinocchio.cpp | 361 +++++++++--------- .../test_kinematics_interface_pinocchio.cpp | 270 +++++++------ 4 files changed, 351 insertions(+), 356 deletions(-) diff --git a/kinematics_interface_pinocchio/README.md b/kinematics_interface_pinocchio/README.md index 0146423..f827635 100644 --- a/kinematics_interface_pinocchio/README.md +++ b/kinematics_interface_pinocchio/README.md @@ -6,6 +6,8 @@ A [Pinocchio](https://github.com/stack-of-tasks/pinocchio)-based [ROS2 Kinematic Requires pinocchio to be installed: `apt install ros-humble-pinocchio`. +This package can now be installed directly from ros-distro for humble: `apt install ros-humble-kinematics-interface-pinocchio`. + ## Roadmap - [x] Replicate KDL implemenations and capabilities with Pinocchio 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 index 828f51e..28685a9 100644 --- a/kinematics_interface_pinocchio/include/kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp +++ b/kinematics_interface_pinocchio/include/kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp @@ -39,49 +39,53 @@ namespace kinematics_interface_pinocchio class KinematicsInterfacePinocchio : public kinematics_interface::KinematicsInterface { public: - bool initialize( - std::shared_ptr parameters_interface, - const std::string & end_effector_name) override; + bool initialize( + std::shared_ptr parameters_interface, + const std::string& end_effector_name + ) 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_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 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_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( + const Eigen::VectorXd& joint_pos, const std::string& link_name, + Eigen::Matrix& jacobian + ) 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); + // 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 initialized = false; - std::string root_name_; - size_t num_joints_; - - pinocchio::Model model_; - std::shared_ptr data_; - Eigen::VectorXd q_; - Eigen::MatrixXd jacobian_; - Eigen::MatrixXd frame_tf_; + bool initialized = false; + std::string root_name_; + size_t num_joints_; - std::shared_ptr parameters_interface_; - std::unordered_map link_name_map_; - double alpha; // damping term for Jacobian inverse - Eigen::MatrixXd I; + pinocchio::Model model_; + std::shared_ptr data_; + Eigen::VectorXd q_; + Eigen::MatrixXd jacobian_; + 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 +} // namespace kinematics_interface_pinocchio -#endif // kinematics_interface_pinocchio__kinematics_interface_pinocchio_HPP_ +#endif // kinematics_interface_pinocchio__kinematics_interface_pinocchio_HPP_ diff --git a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp index 2393a0d..043f6b3 100644 --- a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, PickNik, Inc. +// 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. @@ -21,227 +21,226 @@ namespace kinematics_interface_pinocchio rclcpp::Logger LOGGER = rclcpp::get_logger("kinematics_interface_pinocchio"); bool KinematicsInterfacePinocchio::initialize( - std::shared_ptr parameters_interface, - const std::string & end_effector_name) + std::shared_ptr parameters_interface, + const std::string& end_effector_name +) { - // track initialization plugin - initialized = true; - - // get robot description - auto robot_param = rclcpp::Parameter(); - if (!parameters_interface->get_parameter("robot_description", robot_param)) - { - RCLCPP_ERROR(LOGGER, "parameter robot_description not set"); - return false; - } - auto robot_description = robot_param.as_string(); - // get alpha damping term - auto alpha_param = rclcpp::Parameter("alpha", 0.000005); - if (parameters_interface->has_parameter("alpha")) - { - parameters_interface->get_parameter("alpha", alpha_param); - } - alpha = alpha_param.as_double(); - - - // get root name - auto base_param = rclcpp::Parameter(); - if (parameters_interface->has_parameter("base")) - { - parameters_interface->get_parameter("base", base_param); - root_name_ = base_param.as_string(); - } - else - { - root_name_ = model_.frames[0].name; - } - // TODO: only handling fixed base now - model_ = pinocchio::urdf::buildModelFromXML(robot_description, /*root_name_,*/ model_, true); - - - // allocate dynamics memory - data_ = std::make_shared(model_); - num_joints_ = model_.nq; // TODO: handle floating base - q_.resize(num_joints_); - I = Eigen::MatrixXd(num_joints_, num_joints_); - I.setIdentity(); - jacobian_.resize(6, num_joints_); - - return true; + // track initialization plugin + initialized = true; + + // get robot description + auto robot_param = rclcpp::Parameter(); + if (!parameters_interface->get_parameter("robot_description", robot_param)) + { + RCLCPP_ERROR(LOGGER, "parameter robot_description not set"); + return false; + } + auto robot_description = robot_param.as_string(); + // get alpha damping term + auto alpha_param = rclcpp::Parameter("alpha", 0.000005); + if (parameters_interface->has_parameter("alpha")) + { + parameters_interface->get_parameter("alpha", alpha_param); + } + alpha = alpha_param.as_double(); + + // get root name + auto base_param = rclcpp::Parameter(); + if (parameters_interface->has_parameter("base")) + { + parameters_interface->get_parameter("base", base_param); + root_name_ = base_param.as_string(); + } + else + { + root_name_ = model_.frames[0].name; + } + // TODO: only handling fixed base now + model_ = pinocchio::urdf::buildModelFromXML(robot_description, /*root_name_,*/ model_, true); + + // allocate dynamics memory + data_ = std::make_shared(model_); + num_joints_ = model_.nq; // TODO: handle floating base + q_.resize(num_joints_); + I = Eigen::MatrixXd(num_joints_, num_joints_); + I.setIdentity(); + jacobian_.resize(6, num_joints_); + + 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) + 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)) - { - 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; + // verify inputs + if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || + !verify_joint_vector(delta_theta)) + { + 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) + 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)) - { - 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 - Eigen::Matrix J_inverse = - (jacobian_.transpose() * jacobian_ + alpha * I).inverse() * jacobian_.transpose(); - delta_theta = J_inverse * delta_x; - - return true; + // verify inputs + if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || + !verify_joint_vector(delta_theta)) + { + 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 + Eigen::Matrix J_inverse = + (jacobian_.transpose() * jacobian_ + alpha * I).inverse() * jacobian_.transpose(); + delta_theta = J_inverse * delta_x; + + return true; } bool KinematicsInterfacePinocchio::calculate_jacobian( - const Eigen::Matrix & joint_pos, const std::string & link_name, - Eigen::Matrix & 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)) - { - 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; + // verify inputs + if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || + !verify_jacobian(jacobian)) + { + 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_link_transform( - const Eigen::Matrix & joint_pos, const std::string & link_name, - Eigen::Isometry3d & 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)) - { - return false; - } + // verify inputs + if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name)) + { + return false; + } + + // get joint array + q_ = joint_pos; + // reset transform_vec + transform.setIdentity(); - // get joint array - q_ = joint_pos; + // 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); - // reset transform_vec - transform.setIdentity(); + // Perform forward kinematics and get a transform. + pinocchio::framesForwardKinematics(model_, *data_, q_); + frame_tf_ = data_->oMf[ee_frame_id].toHomogeneousMatrix(); - // special case: since the root is not in the robot tree, need to return identity transform - if (link_name == root_name_) - { + transform.linear() = frame_tf_.block<3, 3>(0, 0); + transform.translation() = frame_tf_.block<3, 1>(0, 3); 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::verify_link_name(const std::string & link_name) +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) + if (link_name == root_name_) { - links += "\n" + model_.frames[i].name; + return true; } - 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; + 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) +bool KinematicsInterfacePinocchio::verify_joint_vector(const Eigen::VectorXd& joint_vector) { - if (static_cast(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; + if (static_cast(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; + // 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) +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; + 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; } -} // namespace kinematics_interface_pinocchio +} // namespace kinematics_interface_pinocchio #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - kinematics_interface_pinocchio::KinematicsInterfacePinocchio, kinematics_interface::KinematicsInterface) + 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 index 325231f..96a7b5b 100644 --- a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, PickNik, Inc. +// 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. @@ -12,168 +12,158 @@ // See the License for the specific language governing permissions and // limitations under the License. // -/// \author: Paul Gesel +/// \author: Saif Sidhik -#include -#include #include "kinematics_interface/kinematics_interface.hpp" #include "pluginlib/class_loader.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "ros2_control_test_assets/descriptions.hpp" +#include +#include class TestPinocchioPlugin : public ::testing::Test { public: - std::shared_ptr> ik_loader_; - std::shared_ptr ik_; - std::shared_ptr node_; - std::string end_effector_ = "link2"; - - void SetUp() - { - // init ros - rclcpp::init(0, nullptr); - node_ = std::make_shared("test_node"); - std::string plugin_name = "kinematics_interface_pinocchio/KinematicsInterfacePinocchio"; - ik_loader_ = - std::make_shared>( - "kinematics_interface", "kinematics_interface::KinematicsInterface"); - ik_ = std::unique_ptr( - ik_loader_->createUnmanagedInstance(plugin_name)); - } - - void TearDown() - { - // shutdown ros - rclcpp::shutdown(); - } - - void loadURDFParameter() - { - auto urdf = std::string(ros2_control_test_assets::urdf_head) + - std::string(ros2_control_test_assets::urdf_tail); - rclcpp::Parameter param("robot_description", urdf); - node_->declare_parameter("robot_description", ""); - node_->set_parameter(param); - } - - void loadAlphaParameter() - { - rclcpp::Parameter param("alpha", 0.005); - node_->declare_parameter("alpha", 0.005); - node_->set_parameter(param); - } + std::shared_ptr> ik_loader_; + std::shared_ptr ik_; + std::shared_ptr node_; + std::string end_effector_ = "link2"; + + void SetUp() + { + // init ros + rclcpp::init(0, nullptr); + node_ = std::make_shared("test_node"); + std::string plugin_name = "kinematics_interface_pinocchio/KinematicsInterfacePinocchio"; + ik_loader_ = std::make_shared>( + "kinematics_interface", "kinematics_interface::KinematicsInterface" + ); + ik_ = std::unique_ptr(ik_loader_->createUnmanagedInstance(plugin_name + )); + } + + void TearDown() + { + // shutdown ros + rclcpp::shutdown(); + } + + void loadURDFParameter() + { + auto urdf = std::string(ros2_control_test_assets::urdf_head) + std::string(ros2_control_test_assets::urdf_tail); + rclcpp::Parameter param("robot_description", urdf); + node_->declare_parameter("robot_description", ""); + node_->set_parameter(param); + } + + void loadAlphaParameter() + { + rclcpp::Parameter param("alpha", 0.005); + node_->declare_parameter("alpha", 0.005); + node_->set_parameter(param); + } }; TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) { - // load robot description and alpha to parameter server - loadURDFParameter(); - loadAlphaParameter(); - - // initialize the plugin - ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); - - // calculate end effector transform - Eigen::Matrix pos = Eigen::Matrix::Zero(); - Eigen::Isometry3d end_effector_transform; - ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); - - // convert cartesian delta to joint delta - Eigen::Matrix delta_x = Eigen::Matrix::Zero(); - delta_x[2] = 1; - Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); - ASSERT_TRUE( - ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); - - // convert joint delta to cartesian delta - Eigen::Matrix delta_x_est; - ASSERT_TRUE( - ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); - - // Ensure kinematics math is correct - for (size_t i = 0; i < static_cast(delta_x.size()); ++i) - { - ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); - } + // load robot description and alpha to parameter server + loadURDFParameter(); + loadAlphaParameter(); + + // initialize the plugin + ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + + // calculate end effector transform + Eigen::Matrix pos = Eigen::Matrix::Zero(); + Eigen::Isometry3d end_effector_transform; + ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); + + // convert cartesian delta to joint delta + Eigen::Matrix delta_x = Eigen::Matrix::Zero(); + delta_x[2] = 1; + Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); + ASSERT_TRUE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); + + // convert joint delta to cartesian delta + Eigen::Matrix delta_x_est; + ASSERT_TRUE(ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); + + // Ensure kinematics math is correct + for (size_t i = 0; i < static_cast(delta_x.size()); ++i) + { + ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); + } } TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) { - // load robot description and alpha to parameter server - loadURDFParameter(); - loadAlphaParameter(); - - // initialize the plugin - ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); - - // calculate end effector transform - std::vector pos = {0, 0}; - Eigen::Isometry3d end_effector_transform; - ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); - - // convert cartesian delta to joint delta - std::vector delta_x = {0, 0, 0, 0, 0, 0}; - delta_x[2] = 1; - std::vector delta_theta = {0, 0}; - ASSERT_TRUE( - ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); - - // convert joint delta to cartesian delta - std::vector delta_x_est(6); - ASSERT_TRUE( - ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); - - // Ensure kinematics math is correct - for (size_t i = 0; i < static_cast(delta_x.size()); ++i) - { - ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); - } + // load robot description and alpha to parameter server + loadURDFParameter(); + loadAlphaParameter(); + + // initialize the plugin + ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + + // calculate end effector transform + std::vector pos = {0, 0}; + Eigen::Isometry3d end_effector_transform; + ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); + + // convert cartesian delta to joint delta + std::vector delta_x = {0, 0, 0, 0, 0, 0}; + delta_x[2] = 1; + std::vector delta_theta = {0, 0}; + ASSERT_TRUE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); + + // convert joint delta to cartesian delta + std::vector delta_x_est(6); + ASSERT_TRUE(ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); + + // Ensure kinematics math is correct + for (size_t i = 0; i < static_cast(delta_x.size()); ++i) + { + ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); + } } TEST_F(TestPinocchioPlugin, incorrect_input_sizes) { - // load robot description and alpha to parameter server - loadURDFParameter(); - loadAlphaParameter(); - - // initialize the plugin - ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); - - // define correct values - Eigen::Matrix pos = Eigen::Matrix::Zero(); - Eigen::Isometry3d end_effector_transform; - Eigen::Matrix delta_x = Eigen::Matrix::Zero(); - delta_x[2] = 1; - Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); - Eigen::Matrix delta_x_est; - - // wrong size input vector - Eigen::Matrix vec_5 = Eigen::Matrix::Zero(); - - // calculate transform - ASSERT_FALSE(ik_->calculate_link_transform(vec_5, end_effector_, end_effector_transform)); - ASSERT_FALSE(ik_->calculate_link_transform(pos, "link_not_in_model", end_effector_transform)); - - // convert cartesian delta to joint delta - ASSERT_FALSE( - ik_->convert_cartesian_deltas_to_joint_deltas(vec_5, delta_x, end_effector_, delta_theta)); - ASSERT_FALSE( - ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, "link_not_in_model", delta_theta)); - ASSERT_FALSE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, vec_5)); - - // convert joint delta to cartesian delta - ASSERT_FALSE( - ik_->convert_joint_deltas_to_cartesian_deltas(vec_5, delta_theta, end_effector_, delta_x_est)); - ASSERT_FALSE( - ik_->convert_joint_deltas_to_cartesian_deltas(pos, vec_5, end_effector_, delta_x_est)); - ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas( - pos, delta_theta, "link_not_in_model", delta_x_est)); + // load robot description and alpha to parameter server + loadURDFParameter(); + loadAlphaParameter(); + + // initialize the plugin + ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + + // define correct values + Eigen::Matrix pos = Eigen::Matrix::Zero(); + Eigen::Isometry3d end_effector_transform; + Eigen::Matrix delta_x = Eigen::Matrix::Zero(); + delta_x[2] = 1; + Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); + Eigen::Matrix delta_x_est; + + // wrong size input vector + Eigen::Matrix vec_5 = Eigen::Matrix::Zero(); + + // calculate transform + ASSERT_FALSE(ik_->calculate_link_transform(vec_5, end_effector_, end_effector_transform)); + ASSERT_FALSE(ik_->calculate_link_transform(pos, "link_not_in_model", end_effector_transform)); + + // convert cartesian delta to joint delta + ASSERT_FALSE(ik_->convert_cartesian_deltas_to_joint_deltas(vec_5, delta_x, end_effector_, delta_theta)); + ASSERT_FALSE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, "link_not_in_model", delta_theta)); + ASSERT_FALSE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, vec_5)); + + // convert joint delta to cartesian delta + ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas(vec_5, delta_theta, end_effector_, delta_x_est)); + ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas(pos, vec_5, end_effector_, delta_x_est)); + ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, "link_not_in_model", delta_x_est)); } TEST_F(TestPinocchioPlugin, Pinocchio_plugin_no_robot_description) { - // load alpha to parameter server - loadAlphaParameter(); - ASSERT_FALSE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + // load alpha to parameter server + loadAlphaParameter(); + ASSERT_FALSE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); } From 6294333db5c5233a73ec204cb7992b845fd104cd Mon Sep 17 00:00:00 2001 From: JustaGist Date: Tue, 7 Jan 2025 22:54:32 +0000 Subject: [PATCH 04/27] add CI workflows; update readme --- .../.github/workflows/ros2-humble-ci.yml | 21 +++++ .../.github/workflows/ros2-jazzy-ci.yml | 21 +++++ .../.github/workflows/ros2-rolling-ci.yml | 21 +++++ kinematics_interface_pinocchio/README.md | 84 ++++++++++++++++--- 4 files changed, 135 insertions(+), 12 deletions(-) create mode 100644 kinematics_interface_pinocchio/.github/workflows/ros2-humble-ci.yml create mode 100644 kinematics_interface_pinocchio/.github/workflows/ros2-jazzy-ci.yml create mode 100644 kinematics_interface_pinocchio/.github/workflows/ros2-rolling-ci.yml diff --git a/kinematics_interface_pinocchio/.github/workflows/ros2-humble-ci.yml b/kinematics_interface_pinocchio/.github/workflows/ros2-humble-ci.yml new file mode 100644 index 0000000..fd939c5 --- /dev/null +++ b/kinematics_interface_pinocchio/.github/workflows/ros2-humble-ci.yml @@ -0,0 +1,21 @@ +name: Build and Test (ROS 2 Humble) + +on: + pull_request: + branches: [main, ros-humble] + workflow_dispatch: + +jobs: + build-and-test: + strategy: + fail-fast: false + matrix: + env: + - { ROS_DISTRO: humble, ROS_REPO: main } + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + with: + ref: ros-${{ matrix.env.ROS_DISTRO }} + - uses: "ros-industrial/industrial_ci@master" + env: ${{matrix.env}} diff --git a/kinematics_interface_pinocchio/.github/workflows/ros2-jazzy-ci.yml b/kinematics_interface_pinocchio/.github/workflows/ros2-jazzy-ci.yml new file mode 100644 index 0000000..76de16e --- /dev/null +++ b/kinematics_interface_pinocchio/.github/workflows/ros2-jazzy-ci.yml @@ -0,0 +1,21 @@ +name: Build and Test (ROS 2 Jazzy) + +on: + pull_request: + branches: [ros-jazzy] + workflow_dispatch: + +jobs: + build-and-test: + strategy: + fail-fast: false + matrix: + env: + - { ROS_DISTRO: jazzy, ROS_REPO: main } + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + with: + ref: ros-${{ matrix.env.ROS_DISTRO }} + - uses: "ros-industrial/industrial_ci@master" + env: ${{matrix.env}} diff --git a/kinematics_interface_pinocchio/.github/workflows/ros2-rolling-ci.yml b/kinematics_interface_pinocchio/.github/workflows/ros2-rolling-ci.yml new file mode 100644 index 0000000..5c2d5bc --- /dev/null +++ b/kinematics_interface_pinocchio/.github/workflows/ros2-rolling-ci.yml @@ -0,0 +1,21 @@ +name: Build and Test (ROS 2 Rolling) + +on: + pull_request: + branches: [ros-rolling] + workflow_dispatch: + +jobs: + build-and-test: + strategy: + fail-fast: false + matrix: + env: + - { ROS_DISTRO: rolling, ROS_REPO: main } + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + with: + ref: ros-${{ matrix.env.ROS_DISTRO }} + - uses: "ros-industrial/industrial_ci@master" + env: ${{matrix.env}} diff --git a/kinematics_interface_pinocchio/README.md b/kinematics_interface_pinocchio/README.md index f827635..65369e1 100644 --- a/kinematics_interface_pinocchio/README.md +++ b/kinematics_interface_pinocchio/README.md @@ -1,18 +1,78 @@ + # kinematics_interface_pinocchio -[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) -[![ROS Version](https://img.shields.io/badge/ROS-Humble-brightgreen.svg?logo=ros)](https://docs.ros.org/en/humble/index.html) -A [Pinocchio](https://github.com/stack-of-tasks/pinocchio)-based [ROS2 Kinematics Interface](https://github.com/ros-controls/kinematics_interface) implementation. This can be used instead of the default [KDL implementation](https://github.com/ros-controls/kinematics_interface/tree/master/kinematics_interface_kdl) shipped with default ROS installations. +[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) +[![ROS Humble](https://img.shields.io/badge/ROS-Humble-brightgreen.svg?logo=ros)](https://docs.ros.org/en/humble/index.html) +[![ROS Jazzy](https://img.shields.io/badge/ROS-Jazzy-brightgreen.svg?logo=ros)](https://docs.ros.org/en/jazzy/index.html) +[![ROS Rolling](https://img.shields.io/badge/ROS-Rolling-brightgreen.svg?logo=ros)](https://docs.ros.org/en/rolling/index.html) + +A [Pinocchio](https://github.com/stack-of-tasks/pinocchio)-based [ROS 2 Kinematics Interface](https://github.com/ros-controls/kinematics_interface) plugin. This can serve as a drop-in replacement for the default [KDL plugin](https://github.com/ros-controls/kinematics_interface/tree/master/kinematics_interface_kdl) in ROS 2, offering a different backend for forward and inverse kinematics. + +--- + +## Overview + +This package provides: + +- A Pinocchio-based implementation of the `kinematics_interface` for ROS 2. +- The same plugin interface as the default KDL plugin (`kinematics_interface_kdl`). +- A path to potentially better performance (to be benchmarked) and additional features enabled by Pinocchio. + +### Installation + +You can install this package directly from your ROS 2 distribution’s repositories: + +```bash +sudo apt-get update +sudo apt-get install ros-${ROS_DISTRO}-kinematics-interface-pinocchio +``` + +Alternatively, you can clone this repository into your ROS workspace and build from source (requires pinocchio to be installed/built). + +--- + +## Build Status + +| ROS Distro | Build Status | +| ---------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| Humble | [![Build and Test (ROS 2 Humble)](https://github.com/justagist/kinematics_interface_pinocchio/actions/workflows/ros2-humble-ci.yml/badge.svg)](https://github.com/justagist/kinematics_interface_pinocchio/actions/workflows/ros2-humble-ci.yml) | +| Jazzy | [![Build and Test (ROS 2 Jazzy)](https://github.com/justagist/kinematics_interface_pinocchio/actions/workflows/ros2-jazzy-ci.yml/badge.svg)](https://github.com/justagist/kinematics_interface_pinocchio/actions/workflows/ros2-jazzy-ci.yml) | +| Rolling | [![Build and Test (ROS 2 Rolling)](https://github.com/justagist/kinematics_interface_pinocchio/actions/workflows/ros2-rolling-ci.yml/badge.svg)](https://github.com/justagist/kinematics_interface_pinocchio/actions/workflows/ros2-rolling-ci.yml) | + + +--- + +## Usage + +To use this plugin in place of the default `KDL` plugin: + +1. Update your ROS 2 package dependencies to include `kinematics_interface_pinocchio`. +2. In your configuration or launch files, specify the Pinocchio plugin shared library instead of `kinematics_interface_kdl`. + +An example snippet: + +```xml + +kinematics_interface_pinocchio/KinematicsInterfacePinocchio +``` + +--- + +## TODO + +- [x] Replicate KDL implementations and capabilities with Pinocchio +- [x] Package as a ROS 2 plugin usable in place of `kinematics_interface_kdl` +- [ ] Benchmark and compare performance +- [ ] Allow arbitrary frames to be used as base (currently uses the model’s root joint) +- [ ] Add support for floating-base robots +- [ ] Add support for continuous joints and other composite joints supported by Pinocchio + +--- -Requires pinocchio to be installed: `apt install ros-humble-pinocchio`. +## License -This package can now be installed directly from ros-distro for humble: `apt install ros-humble-kinematics-interface-pinocchio`. +This project is licensed under the [Apache 2.0 License](LICENSE). See the [LICENSE](LICENSE) file for details. -## Roadmap +--- -- [x] Replicate KDL implemenations and capabilities with Pinocchio -- [x] Package as a ROS2 plugin which can be used in place of `kinematics_interface_kdl`. -- [ ] Benchmark and compare performances -- [ ] Allow arbitrary frames to be used as base (currently uses root joint from model) -- [ ] Add support for floating-base robots -- [ ] Add support for continuous joints and other composite joints supported by Pinocchio +Contributions welcome! From d32bb23d37aa3ce603a0e714e49e7d05b6909b1f Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Sat, 1 Feb 2025 22:40:29 +0000 Subject: [PATCH 05/27] Bump actions/checkout from 3 to 4 Bumps [actions/checkout](https://github.com/actions/checkout) from 3 to 4. - [Release notes](https://github.com/actions/checkout/releases) - [Changelog](https://github.com/actions/checkout/blob/main/CHANGELOG.md) - [Commits](https://github.com/actions/checkout/compare/v3...v4) --- updated-dependencies: - dependency-name: actions/checkout dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] --- .../.github/workflows/ros2-humble-ci.yml | 2 +- .../.github/workflows/ros2-jazzy-ci.yml | 2 +- .../.github/workflows/ros2-rolling-ci.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/kinematics_interface_pinocchio/.github/workflows/ros2-humble-ci.yml b/kinematics_interface_pinocchio/.github/workflows/ros2-humble-ci.yml index fd939c5..d7c0c1f 100644 --- a/kinematics_interface_pinocchio/.github/workflows/ros2-humble-ci.yml +++ b/kinematics_interface_pinocchio/.github/workflows/ros2-humble-ci.yml @@ -14,7 +14,7 @@ jobs: - { ROS_DISTRO: humble, ROS_REPO: main } runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: ref: ros-${{ matrix.env.ROS_DISTRO }} - uses: "ros-industrial/industrial_ci@master" diff --git a/kinematics_interface_pinocchio/.github/workflows/ros2-jazzy-ci.yml b/kinematics_interface_pinocchio/.github/workflows/ros2-jazzy-ci.yml index 76de16e..cb9520a 100644 --- a/kinematics_interface_pinocchio/.github/workflows/ros2-jazzy-ci.yml +++ b/kinematics_interface_pinocchio/.github/workflows/ros2-jazzy-ci.yml @@ -14,7 +14,7 @@ jobs: - { ROS_DISTRO: jazzy, ROS_REPO: main } runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: ref: ros-${{ matrix.env.ROS_DISTRO }} - uses: "ros-industrial/industrial_ci@master" diff --git a/kinematics_interface_pinocchio/.github/workflows/ros2-rolling-ci.yml b/kinematics_interface_pinocchio/.github/workflows/ros2-rolling-ci.yml index 5c2d5bc..8641436 100644 --- a/kinematics_interface_pinocchio/.github/workflows/ros2-rolling-ci.yml +++ b/kinematics_interface_pinocchio/.github/workflows/ros2-rolling-ci.yml @@ -14,7 +14,7 @@ jobs: - { ROS_DISTRO: rolling, ROS_REPO: main } runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: ref: ros-${{ matrix.env.ROS_DISTRO }} - uses: "ros-industrial/industrial_ci@master" From da51bffd7fa60d6bf2168a2a7f6e6999c5a1a4a4 Mon Sep 17 00:00:00 2001 From: Saif Sidhik Date: Sat, 28 Jun 2025 14:05:40 +0100 Subject: [PATCH 06/27] implement calculate inverse jacobian (#6) * bump version: changes to support rolling distro * Add virtual function to avoid compilation error. Signed-off-by: Marco A. Gutierrez * implement inv jacobian function and tests similar to pinocchio_interface_kdl --------- Signed-off-by: Marco A. Gutierrez Co-authored-by: Marco A. Gutierrez --- kinematics_interface_pinocchio/CHANGELOG.rst | 5 ++ kinematics_interface_pinocchio/README.md | 1 + .../kinematics_interface_pinocchio.hpp | 10 ++- .../src/kinematics_interface_pinocchio.cpp | 90 +++++++++++++++---- .../test_kinematics_interface_pinocchio.cpp | 64 +++++++++++-- 5 files changed, 145 insertions(+), 25 deletions(-) diff --git a/kinematics_interface_pinocchio/CHANGELOG.rst b/kinematics_interface_pinocchio/CHANGELOG.rst index 230f421..541ec09 100644 --- a/kinematics_interface_pinocchio/CHANGELOG.rst +++ b/kinematics_interface_pinocchio/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package kinematics_interface_pinocchio ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.0.2 +----- +* fixes to adapt to upstream changes in kinematics_interface +* Contributors: David V. Lu, Saif Sidhik + 0.0.1 ----- * create a pinocchio kinematics plugin following ros2 kinematics_interface format diff --git a/kinematics_interface_pinocchio/README.md b/kinematics_interface_pinocchio/README.md index 65369e1..112383f 100644 --- a/kinematics_interface_pinocchio/README.md +++ b/kinematics_interface_pinocchio/README.md @@ -7,6 +7,7 @@ [![ROS Rolling](https://img.shields.io/badge/ROS-Rolling-brightgreen.svg?logo=ros)](https://docs.ros.org/en/rolling/index.html) A [Pinocchio](https://github.com/stack-of-tasks/pinocchio)-based [ROS 2 Kinematics Interface](https://github.com/ros-controls/kinematics_interface) plugin. This can serve as a drop-in replacement for the default [KDL plugin](https://github.com/ros-controls/kinematics_interface/tree/master/kinematics_interface_kdl) in ROS 2, offering a different backend for forward and inverse kinematics. +Requires pinocchio to be installed: `apt install ros-rolling-pinocchio`. --- 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 index 28685a9..f1a9fce 100644 --- a/kinematics_interface_pinocchio/include/kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp +++ b/kinematics_interface_pinocchio/include/kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp @@ -40,8 +40,9 @@ class KinematicsInterfacePinocchio : public kinematics_interface::KinematicsInte { public: bool initialize( + const std::string& robot_description, std::shared_ptr parameters_interface, - const std::string& end_effector_name + const std::string& param_namespace ) override; bool convert_cartesian_deltas_to_joint_deltas( @@ -63,12 +64,18 @@ class KinematicsInterfacePinocchio : public kinematics_interface::KinematicsInte Eigen::Matrix& jacobian ) override; + bool calculate_jacobian_inverse( + const Eigen::VectorXd& joint_pos, const std::string& link_name, + Eigen::Matrix& 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& jacobian); + bool verify_jacobian_inverse(const Eigen::Matrix& jacobian_inverse); bool initialized = false; std::string root_name_; @@ -78,6 +85,7 @@ class KinematicsInterfacePinocchio : public kinematics_interface::KinematicsInte std::shared_ptr data_; Eigen::VectorXd q_; Eigen::MatrixXd jacobian_; + Eigen::Matrix jacobian_inverse_; Eigen::MatrixXd frame_tf_; std::shared_ptr parameters_interface_; diff --git a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp index 043f6b3..d912dd0 100644 --- a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp @@ -21,21 +21,34 @@ 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& end_effector_name + const std::string& param_namespace ) { // track initialization plugin initialized = true; - // get robot description - auto robot_param = rclcpp::Parameter(); - if (!parameters_interface->get_parameter("robot_description", robot_param)) + // get parameters + std::string ns = !param_namespace.empty() ? param_namespace + "." : ""; + + std::string robot_description_local; + if (robot_description.empty()) { - RCLCPP_ERROR(LOGGER, "parameter robot_description not set"); - return false; + // 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; } - auto robot_description = robot_param.as_string(); // get alpha damping term auto alpha_param = rclcpp::Parameter("alpha", 0.000005); if (parameters_interface->has_parameter("alpha")) @@ -56,7 +69,7 @@ bool KinematicsInterfacePinocchio::initialize( root_name_ = model_.frames[0].name; } // TODO: only handling fixed base now - model_ = pinocchio::urdf::buildModelFromXML(robot_description, /*root_name_,*/ model_, true); + model_ = pinocchio::urdf::buildModelFromXML(robot_description_local, /*root_name_,*/ model_, true); // allocate dynamics memory data_ = std::make_shared(model_); @@ -65,6 +78,7 @@ bool KinematicsInterfacePinocchio::initialize( I = Eigen::MatrixXd(num_joints_, num_joints_); I.setIdentity(); jacobian_.resize(6, num_joints_); + jacobian_inverse_.resize(num_joints_, 6); return true; } @@ -105,28 +119,48 @@ bool KinematicsInterfacePinocchio::convert_cartesian_deltas_to_joint_deltas( 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)) + { + 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 - Eigen::Matrix J_inverse = - (jacobian_.transpose() * jacobian_ + alpha * I).inverse() * jacobian_.transpose(); - delta_theta = J_inverse * delta_x; + jacobian = jacobian_; return true; } -bool KinematicsInterfacePinocchio::calculate_jacobian( +bool KinematicsInterfacePinocchio::calculate_jacobian_inverse( const Eigen::Matrix& joint_pos, const std::string& link_name, - Eigen::Matrix& jacobian + Eigen::Matrix& jacobian_inverse ) { // verify inputs if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || - !verify_jacobian(jacobian)) + !verify_jacobian_inverse(jacobian_inverse)) { return false; } @@ -137,7 +171,10 @@ bool KinematicsInterfacePinocchio::calculate_jacobian( // calculate Jacobian const auto ee_frame_id = model_.getFrameId(link_name); pinocchio::computeFrameJacobian(model_, *data_, q_, ee_frame_id, jacobian_); - jacobian = jacobian_; + // damped inverse + jacobian_inverse_ = (jacobian_.transpose() * jacobian_ + alpha * I).inverse() * jacobian_.transpose(); + + jacobian_inverse = jacobian_inverse_; return true; } @@ -150,6 +187,13 @@ bool KinematicsInterfacePinocchio::calculate_link_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; } @@ -237,6 +281,20 @@ bool KinematicsInterfacePinocchio::verify_jacobian(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; +} + } // namespace kinematics_interface_pinocchio #include "pluginlib/class_list_macros.hpp" diff --git a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp index 96a7b5b..353c90c 100644 --- a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp @@ -28,6 +28,8 @@ class TestPinocchioPlugin : public ::testing::Test std::shared_ptr ik_; std::shared_ptr node_; std::string end_effector_ = "link2"; + std::string urdf_ = + std::string(ros2_control_test_assets::urdf_head) + std::string(ros2_control_test_assets::urdf_tail); void SetUp() { @@ -71,17 +73,17 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) loadAlphaParameter(); // initialize the plugin - ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); // calculate end effector transform - Eigen::Matrix pos = Eigen::Matrix::Zero(); + Eigen::Matrix pos = Eigen::Matrix::Zero(); Eigen::Isometry3d end_effector_transform; ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); // convert cartesian delta to joint delta Eigen::Matrix delta_x = Eigen::Matrix::Zero(); delta_x[2] = 1; - Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); + Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); ASSERT_TRUE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); // convert joint delta to cartesian delta @@ -93,6 +95,25 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) { ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); } + + // calculate jacobian + Eigen::Matrix jacobian = Eigen::Matrix::Zero(); + ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian)); + + // calculate jacobian inverse + Eigen::Matrix jacobian_inverse = + jacobian.completeOrthogonalDecomposition().pseudoInverse(); + Eigen::Matrix jacobian_inverse_est = Eigen::Matrix::Zero(); + ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); + + // ensure jacobian inverse math is correct + for (Eigen::Index i = 0; i < jacobian_inverse.rows(); ++i) + { + for (Eigen::Index j = 0; j < jacobian_inverse.cols(); ++j) + { + ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02); + } + } } TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) @@ -102,17 +123,17 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) loadAlphaParameter(); // initialize the plugin - ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); // calculate end effector transform - std::vector pos = {0, 0}; + std::vector pos = {0, 0, 0}; Eigen::Isometry3d end_effector_transform; ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); // convert cartesian delta to joint delta std::vector delta_x = {0, 0, 0, 0, 0, 0}; delta_x[2] = 1; - std::vector delta_theta = {0, 0}; + std::vector delta_theta = {0, 0, 0}; ASSERT_TRUE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); // convert joint delta to cartesian delta @@ -124,6 +145,25 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) { ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); } + + // calculate jacobian + Eigen::Matrix jacobian = Eigen::Matrix::Zero(); + ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian)); + + // calculate jacobian inverse + Eigen::Matrix jacobian_inverse = + jacobian.completeOrthogonalDecomposition().pseudoInverse(); + Eigen::Matrix jacobian_inverse_est = Eigen::Matrix::Zero(); + ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); + + // ensure jacobian inverse math is correct + for (Eigen::Index i = 0; i < jacobian_inverse.rows(); ++i) + { + for (Eigen::Index j = 0; j < jacobian_inverse.cols(); ++j) + { + ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02); + } + } } TEST_F(TestPinocchioPlugin, incorrect_input_sizes) @@ -133,7 +173,7 @@ TEST_F(TestPinocchioPlugin, incorrect_input_sizes) loadAlphaParameter(); // initialize the plugin - ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); // define correct values Eigen::Matrix pos = Eigen::Matrix::Zero(); @@ -142,9 +182,12 @@ TEST_F(TestPinocchioPlugin, incorrect_input_sizes) delta_x[2] = 1; Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); Eigen::Matrix delta_x_est; + Eigen::Matrix jacobian = Eigen::Matrix::Zero(); // wrong size input vector Eigen::Matrix vec_5 = Eigen::Matrix::Zero(); + // wrong size input jacobian + Eigen::Matrix mat_5_6 = Eigen::Matrix::Zero(); // calculate transform ASSERT_FALSE(ik_->calculate_link_transform(vec_5, end_effector_, end_effector_transform)); @@ -159,11 +202,16 @@ TEST_F(TestPinocchioPlugin, incorrect_input_sizes) ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas(vec_5, delta_theta, end_effector_, delta_x_est)); ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas(pos, vec_5, end_effector_, delta_x_est)); ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, "link_not_in_model", delta_x_est)); + + // calculate jacobian inverse + ASSERT_FALSE(ik_->calculate_jacobian_inverse(vec_5, end_effector_, jacobian)); + ASSERT_FALSE(ik_->calculate_jacobian_inverse(pos, end_effector_, mat_5_6)); + ASSERT_FALSE(ik_->calculate_jacobian_inverse(pos, "link_not_in_model", jacobian)); } TEST_F(TestPinocchioPlugin, Pinocchio_plugin_no_robot_description) { // load alpha to parameter server loadAlphaParameter(); - ASSERT_FALSE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + ASSERT_FALSE(ik_->initialize("", node_->get_node_parameters_interface(), "")); } From fd74559d5dbd96301d7cf3d30703946070c4b995 Mon Sep 17 00:00:00 2001 From: JustaGist Date: Sat, 28 Jun 2025 14:31:57 +0100 Subject: [PATCH 07/27] prepare for new release --- kinematics_interface_pinocchio/CHANGELOG.rst | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/kinematics_interface_pinocchio/CHANGELOG.rst b/kinematics_interface_pinocchio/CHANGELOG.rst index 541ec09..443d450 100644 --- a/kinematics_interface_pinocchio/CHANGELOG.rst +++ b/kinematics_interface_pinocchio/CHANGELOG.rst @@ -2,12 +2,7 @@ Changelog for package kinematics_interface_pinocchio ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -0.0.2 ------ -* fixes to adapt to upstream changes in kinematics_interface -* Contributors: David V. Lu, Saif Sidhik - -0.0.1 ------ +0.0.1 (2024-08-05) +------------------ * create a pinocchio kinematics plugin following ros2 kinematics_interface format * Contributors: Saif Sidhik From b28ec9f1919ec5e578d4853947a3fc884440f612 Mon Sep 17 00:00:00 2001 From: JustaGist Date: Sat, 28 Jun 2025 13:32:16 +0000 Subject: [PATCH 08/27] 0.0.2 --- kinematics_interface_pinocchio/CHANGELOG.rst | 5 +++++ kinematics_interface_pinocchio/package.xml | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/kinematics_interface_pinocchio/CHANGELOG.rst b/kinematics_interface_pinocchio/CHANGELOG.rst index 443d450..88e6441 100644 --- a/kinematics_interface_pinocchio/CHANGELOG.rst +++ b/kinematics_interface_pinocchio/CHANGELOG.rst @@ -2,6 +2,11 @@ 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 diff --git a/kinematics_interface_pinocchio/package.xml b/kinematics_interface_pinocchio/package.xml index c907d16..98012c2 100644 --- a/kinematics_interface_pinocchio/package.xml +++ b/kinematics_interface_pinocchio/package.xml @@ -2,7 +2,7 @@ kinematics_interface_pinocchio - 0.0.1 + 0.0.2 Pinocchio-based implementation of ros2_control kinematics interface Saif Sidhik Apache License 2.0 From 73eb65dbe1bce510be3eb97acd7f2aa9c093faa6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 19 Sep 2025 08:22:57 +0200 Subject: [PATCH 09/27] Add calculate_frame_difference (#8) * Modernize CMakeLists * Add calculate_frame_difference --- kinematics_interface_pinocchio/CMakeLists.txt | 17 ++++--- .../kinematics_interface_pinocchio.hpp | 6 +++ .../src/kinematics_interface_pinocchio.cpp | 47 +++++++++++++++++++ .../test_kinematics_interface_pinocchio.cpp | 15 ++++++ 4 files changed, 76 insertions(+), 9 deletions(-) diff --git a/kinematics_interface_pinocchio/CMakeLists.txt b/kinematics_interface_pinocchio/CMakeLists.txt index 9431537..14b8f9e 100644 --- a/kinematics_interface_pinocchio/CMakeLists.txt +++ b/kinematics_interface_pinocchio/CMakeLists.txt @@ -29,11 +29,11 @@ target_include_directories(kinematics_interface_pinocchio PUBLIC $ ) target_compile_features(kinematics_interface_pinocchio PUBLIC cxx_std_17) -ament_target_dependencies(kinematics_interface_pinocchio PUBLIC - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) target_link_libraries(kinematics_interface_pinocchio PUBLIC - Eigen3::Eigen pinocchio::pinocchio + kinematics_interface::kinematics_interface + pluginlib::pluginlib + Eigen3::Eigen + pinocchio::pinocchio ) pluginlib_export_plugin_description_file(kinematics_interface kinematics_interface_pinocchio.xml) @@ -46,11 +46,10 @@ if(BUILD_TESTING) test_kinematics_interface_pinocchio test/test_kinematics_interface_pinocchio.cpp ) - target_link_libraries(test_kinematics_interface_pinocchio kinematics_interface_pinocchio) - - # TODO: Use target_link_libraries once ros2_control_test_assets' - # CMake include export is fixed - ament_target_dependencies(test_kinematics_interface_pinocchio ros2_control_test_assets) + target_link_libraries(test_kinematics_interface_pinocchio + kinematics_interface_pinocchio + ros2_control_test_assets::ros2_control_test_assets + ) endif() install( 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 index f1a9fce..90fb42e 100644 --- a/kinematics_interface_pinocchio/include/kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp +++ b/kinematics_interface_pinocchio/include/kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp @@ -69,6 +69,11 @@ class KinematicsInterfacePinocchio : public kinematics_interface::KinematicsInte 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(); @@ -76,6 +81,7 @@ class KinematicsInterfacePinocchio : public kinematics_interface::KinematicsInte 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_; diff --git a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp index d912dd0..5ec1547 100644 --- a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp @@ -221,6 +221,43 @@ bool KinematicsInterfacePinocchio::calculate_link_transform( 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_initialized() || !verify_period(dt)) + { + 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_) @@ -295,6 +332,16 @@ bool KinematicsInterfacePinocchio::verify_jacobian_inverse( 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" diff --git a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp index 353c90c..6e401cf 100644 --- a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp @@ -114,6 +114,21 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02); } } + + // compute the difference between two cartesian frames + Eigen::Matrix x_a, x_b; + x_a << 0, 1, 0, 0, 0, 0, 1; + x_b << 2, 3, 0, 0, 1, 0, 0; + double dt = 1.0; + delta_x = Eigen::Matrix::Zero(); + delta_x_est << 2, 2, 0, 0, 3.14, 0; + ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x)); + + // ensure that difference math is correct + for (Eigen::Index i = 0; i < delta_x.size(); ++i) + { + EXPECT_NEAR(delta_x(i), delta_x_est(i), 0.02) << " for index " << i; + } } TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) From 52766720053aad66208a0844420ef7f5f8fb0302 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 3 Oct 2025 10:29:27 +0000 Subject: [PATCH 10/27] Adapt to our repo structure --- .../.github/dependabot.yml | 20 -- .../.github/workflows/ros2-humble-ci.yml | 21 -- .../.github/workflows/ros2-jazzy-ci.yml | 21 -- .../.github/workflows/ros2-rolling-ci.yml | 21 -- kinematics_interface_pinocchio/CMakeLists.txt | 6 +- kinematics_interface_pinocchio/LICENSE | 202 ------------------ kinematics_interface_pinocchio/README.md | 79 ------- kinematics_interface_pinocchio/package.xml | 14 +- 8 files changed, 16 insertions(+), 368 deletions(-) delete mode 100644 kinematics_interface_pinocchio/.github/dependabot.yml delete mode 100644 kinematics_interface_pinocchio/.github/workflows/ros2-humble-ci.yml delete mode 100644 kinematics_interface_pinocchio/.github/workflows/ros2-jazzy-ci.yml delete mode 100644 kinematics_interface_pinocchio/.github/workflows/ros2-rolling-ci.yml delete mode 100644 kinematics_interface_pinocchio/LICENSE delete mode 100644 kinematics_interface_pinocchio/README.md diff --git a/kinematics_interface_pinocchio/.github/dependabot.yml b/kinematics_interface_pinocchio/.github/dependabot.yml deleted file mode 100644 index ae63b0d..0000000 --- a/kinematics_interface_pinocchio/.github/dependabot.yml +++ /dev/null @@ -1,20 +0,0 @@ -# To get started with Dependabot version updates, you'll need to specify which -# package ecosystems to update and where the package manifests are located. -# Please see the documentation for all configuration options: -# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates - -version: 2 -updates: - - package-ecosystem: "github-actions" - # Workflow files stored in the - # default location of `.github/workflows` - directory: "/" - schedule: - interval: "monthly" - - package-ecosystem: "github-actions" - # Workflow files stored in the - # default location of `.github/workflows` - directory: "/" - schedule: - interval: "monthly" - target-branch: "main" diff --git a/kinematics_interface_pinocchio/.github/workflows/ros2-humble-ci.yml b/kinematics_interface_pinocchio/.github/workflows/ros2-humble-ci.yml deleted file mode 100644 index d7c0c1f..0000000 --- a/kinematics_interface_pinocchio/.github/workflows/ros2-humble-ci.yml +++ /dev/null @@ -1,21 +0,0 @@ -name: Build and Test (ROS 2 Humble) - -on: - pull_request: - branches: [main, ros-humble] - workflow_dispatch: - -jobs: - build-and-test: - strategy: - fail-fast: false - matrix: - env: - - { ROS_DISTRO: humble, ROS_REPO: main } - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - with: - ref: ros-${{ matrix.env.ROS_DISTRO }} - - uses: "ros-industrial/industrial_ci@master" - env: ${{matrix.env}} diff --git a/kinematics_interface_pinocchio/.github/workflows/ros2-jazzy-ci.yml b/kinematics_interface_pinocchio/.github/workflows/ros2-jazzy-ci.yml deleted file mode 100644 index cb9520a..0000000 --- a/kinematics_interface_pinocchio/.github/workflows/ros2-jazzy-ci.yml +++ /dev/null @@ -1,21 +0,0 @@ -name: Build and Test (ROS 2 Jazzy) - -on: - pull_request: - branches: [ros-jazzy] - workflow_dispatch: - -jobs: - build-and-test: - strategy: - fail-fast: false - matrix: - env: - - { ROS_DISTRO: jazzy, ROS_REPO: main } - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - with: - ref: ros-${{ matrix.env.ROS_DISTRO }} - - uses: "ros-industrial/industrial_ci@master" - env: ${{matrix.env}} diff --git a/kinematics_interface_pinocchio/.github/workflows/ros2-rolling-ci.yml b/kinematics_interface_pinocchio/.github/workflows/ros2-rolling-ci.yml deleted file mode 100644 index 8641436..0000000 --- a/kinematics_interface_pinocchio/.github/workflows/ros2-rolling-ci.yml +++ /dev/null @@ -1,21 +0,0 @@ -name: Build and Test (ROS 2 Rolling) - -on: - pull_request: - branches: [ros-rolling] - workflow_dispatch: - -jobs: - build-and-test: - strategy: - fail-fast: false - matrix: - env: - - { ROS_DISTRO: rolling, ROS_REPO: main } - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - with: - ref: ros-${{ matrix.env.ROS_DISTRO }} - - uses: "ros-industrial/industrial_ci@master" - env: ${{matrix.env}} diff --git a/kinematics_interface_pinocchio/CMakeLists.txt b/kinematics_interface_pinocchio/CMakeLists.txt index 14b8f9e..89911e7 100644 --- a/kinematics_interface_pinocchio/CMakeLists.txt +++ b/kinematics_interface_pinocchio/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.16) project(kinematics_interface_pinocchio LANGUAGES CXX) -if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() +find_package(ros2_control_cmake REQUIRED) +set_compiler_options() +export_windows_symbols() set(THIS_PACKAGE_INCLUDE_DEPENDS kinematics_interface diff --git a/kinematics_interface_pinocchio/LICENSE b/kinematics_interface_pinocchio/LICENSE deleted file mode 100644 index fb9b7c7..0000000 --- a/kinematics_interface_pinocchio/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright 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. diff --git a/kinematics_interface_pinocchio/README.md b/kinematics_interface_pinocchio/README.md deleted file mode 100644 index 112383f..0000000 --- a/kinematics_interface_pinocchio/README.md +++ /dev/null @@ -1,79 +0,0 @@ - -# kinematics_interface_pinocchio - -[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) -[![ROS Humble](https://img.shields.io/badge/ROS-Humble-brightgreen.svg?logo=ros)](https://docs.ros.org/en/humble/index.html) -[![ROS Jazzy](https://img.shields.io/badge/ROS-Jazzy-brightgreen.svg?logo=ros)](https://docs.ros.org/en/jazzy/index.html) -[![ROS Rolling](https://img.shields.io/badge/ROS-Rolling-brightgreen.svg?logo=ros)](https://docs.ros.org/en/rolling/index.html) - -A [Pinocchio](https://github.com/stack-of-tasks/pinocchio)-based [ROS 2 Kinematics Interface](https://github.com/ros-controls/kinematics_interface) plugin. This can serve as a drop-in replacement for the default [KDL plugin](https://github.com/ros-controls/kinematics_interface/tree/master/kinematics_interface_kdl) in ROS 2, offering a different backend for forward and inverse kinematics. -Requires pinocchio to be installed: `apt install ros-rolling-pinocchio`. - ---- - -## Overview - -This package provides: - -- A Pinocchio-based implementation of the `kinematics_interface` for ROS 2. -- The same plugin interface as the default KDL plugin (`kinematics_interface_kdl`). -- A path to potentially better performance (to be benchmarked) and additional features enabled by Pinocchio. - -### Installation - -You can install this package directly from your ROS 2 distribution’s repositories: - -```bash -sudo apt-get update -sudo apt-get install ros-${ROS_DISTRO}-kinematics-interface-pinocchio -``` - -Alternatively, you can clone this repository into your ROS workspace and build from source (requires pinocchio to be installed/built). - ---- - -## Build Status - -| ROS Distro | Build Status | -| ---------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| Humble | [![Build and Test (ROS 2 Humble)](https://github.com/justagist/kinematics_interface_pinocchio/actions/workflows/ros2-humble-ci.yml/badge.svg)](https://github.com/justagist/kinematics_interface_pinocchio/actions/workflows/ros2-humble-ci.yml) | -| Jazzy | [![Build and Test (ROS 2 Jazzy)](https://github.com/justagist/kinematics_interface_pinocchio/actions/workflows/ros2-jazzy-ci.yml/badge.svg)](https://github.com/justagist/kinematics_interface_pinocchio/actions/workflows/ros2-jazzy-ci.yml) | -| Rolling | [![Build and Test (ROS 2 Rolling)](https://github.com/justagist/kinematics_interface_pinocchio/actions/workflows/ros2-rolling-ci.yml/badge.svg)](https://github.com/justagist/kinematics_interface_pinocchio/actions/workflows/ros2-rolling-ci.yml) | - - ---- - -## Usage - -To use this plugin in place of the default `KDL` plugin: - -1. Update your ROS 2 package dependencies to include `kinematics_interface_pinocchio`. -2. In your configuration or launch files, specify the Pinocchio plugin shared library instead of `kinematics_interface_kdl`. - -An example snippet: - -```xml - -kinematics_interface_pinocchio/KinematicsInterfacePinocchio -``` - ---- - -## TODO - -- [x] Replicate KDL implementations and capabilities with Pinocchio -- [x] Package as a ROS 2 plugin usable in place of `kinematics_interface_kdl` -- [ ] Benchmark and compare performance -- [ ] Allow arbitrary frames to be used as base (currently uses the model’s root joint) -- [ ] Add support for floating-base robots -- [ ] Add support for continuous joints and other composite joints supported by Pinocchio - ---- - -## License - -This project is licensed under the [Apache 2.0 License](LICENSE). See the [LICENSE](LICENSE) file for details. - ---- - -Contributions welcome! diff --git a/kinematics_interface_pinocchio/package.xml b/kinematics_interface_pinocchio/package.xml index 98012c2..0a25a62 100644 --- a/kinematics_interface_pinocchio/package.xml +++ b/kinematics_interface_pinocchio/package.xml @@ -4,13 +4,25 @@ kinematics_interface_pinocchio 0.0.2 Pinocchio-based implementation of ros2_control kinematics interface - Saif Sidhik + + 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 From 828859cf85e3f458b24d8e55cd692bfdb3b61ad6 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 3 Oct 2025 10:37:26 +0000 Subject: [PATCH 11/27] Apply ros-controls format --- .../kinematics_interface_pinocchio.hpp | 116 +++-- .../kinematics_interface_pinocchio.xml | 2 +- kinematics_interface_pinocchio/package.xml | 8 +- .../src/kinematics_interface_pinocchio.cpp | 409 +++++++++--------- .../test_kinematics_interface_pinocchio.cpp | 386 +++++++++-------- 5 files changed, 463 insertions(+), 458 deletions(-) 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 index 90fb42e..037ca47 100644 --- a/kinematics_interface_pinocchio/include/kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp +++ b/kinematics_interface_pinocchio/include/kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp @@ -15,8 +15,8 @@ /// \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_ +#ifndef KINEMATICS_INTERFACE_PINOCCHIO__KINEMATICS_INTERFACE_PINOCCHIO_HPP_ +#define KINEMATICS_INTERFACE_PINOCCHIO__KINEMATICS_INTERFACE_PINOCCHIO_HPP_ #include #include @@ -39,67 +39,61 @@ 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; + 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_; - size_t 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; + // 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_; + size_t 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 +} // namespace kinematics_interface_pinocchio -#endif // kinematics_interface_pinocchio__kinematics_interface_pinocchio_HPP_ +#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 index 32ceae4..c6bd1a7 100644 --- a/kinematics_interface_pinocchio/kinematics_interface_pinocchio.xml +++ b/kinematics_interface_pinocchio/kinematics_interface_pinocchio.xml @@ -6,4 +6,4 @@ Pinocchio plugin for ros2_control kinematics interface - \ No newline at end of file + diff --git a/kinematics_interface_pinocchio/package.xml b/kinematics_interface_pinocchio/package.xml index 0a25a62..f33cc82 100644 --- a/kinematics_interface_pinocchio/package.xml +++ b/kinematics_interface_pinocchio/package.xml @@ -4,14 +4,14 @@ 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/ @@ -35,4 +35,4 @@ ament_cmake - \ No newline at end of file + diff --git a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp index 5ec1547..cb5cdcc 100644 --- a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp @@ -21,204 +21,204 @@ 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 -) + const std::string & robot_description, + std::shared_ptr parameters_interface, + const std::string & param_namespace) { - // track initialization plugin - initialized = true; + // track initialization plugin + initialized = true; - // get parameters - std::string ns = !param_namespace.empty() ? param_namespace + "." : ""; + // 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 alpha damping term - auto alpha_param = rclcpp::Parameter("alpha", 0.000005); - if (parameters_interface->has_parameter("alpha")) + 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)) { - parameters_interface->get_parameter("alpha", alpha_param); + RCLCPP_ERROR(LOGGER, "parameter robot_description not set in kinematics_interface_pinocchio"); + return false; } - alpha = alpha_param.as_double(); + robot_description_local = robot_param.as_string(); + } + else + { + robot_description_local = robot_description; + } + // get alpha damping term + auto alpha_param = rclcpp::Parameter("alpha", 0.000005); + if (parameters_interface->has_parameter("alpha")) + { + parameters_interface->get_parameter("alpha", alpha_param); + } + alpha = alpha_param.as_double(); - // get root name - auto base_param = rclcpp::Parameter(); - if (parameters_interface->has_parameter("base")) - { - parameters_interface->get_parameter("base", base_param); - root_name_ = base_param.as_string(); - } - else - { - root_name_ = model_.frames[0].name; - } - // TODO: only handling fixed base now - model_ = pinocchio::urdf::buildModelFromXML(robot_description_local, /*root_name_,*/ model_, true); - - // allocate dynamics memory - data_ = std::make_shared(model_); - num_joints_ = model_.nq; // TODO: handle floating base - q_.resize(num_joints_); - I = Eigen::MatrixXd(num_joints_, num_joints_); - I.setIdentity(); - jacobian_.resize(6, num_joints_); - jacobian_inverse_.resize(num_joints_, 6); + // get root name + auto base_param = rclcpp::Parameter(); + if (parameters_interface->has_parameter("base")) + { + parameters_interface->get_parameter("base", base_param); + root_name_ = base_param.as_string(); + } + else + { + root_name_ = model_.frames[0].name; + } + // TODO(anyone): only handling fixed base now + model_ = + pinocchio::urdf::buildModelFromXML(robot_description_local, /*root_name_,*/ model_, true); + + // allocate dynamics memory + data_ = std::make_shared(model_); + num_joints_ = model_.nq; // TODO(anyone): handle floating base + 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; + 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 -) + 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)) - { - return false; - } + // verify inputs + if ( + !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || + !verify_joint_vector(delta_theta)) + { + return false; + } - // get joint array - q_ = joint_pos; + // 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; + // 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; + 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 -) + 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)) - { - return false; - } + // verify inputs + if ( + !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || + !verify_joint_vector(delta_theta)) + { + return false; + } - // calculate Jacobian inverse - if (!calculate_jacobian_inverse(joint_pos, link_name, jacobian_inverse_)) - { - return false; - } + // calculate Jacobian inverse + if (!calculate_jacobian_inverse(joint_pos, link_name, jacobian_inverse_)) + { + return false; + } - delta_theta = jacobian_inverse_ * delta_x; + delta_theta = jacobian_inverse_ * delta_x; - return true; + return true; } bool KinematicsInterfacePinocchio::calculate_jacobian( - const Eigen::Matrix& joint_pos, const std::string& link_name, - Eigen::Matrix& 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)) - { - return false; - } + // verify inputs + if ( + !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || + !verify_jacobian(jacobian)) + { + return false; + } - // get joint array - q_ = joint_pos; + // 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_; + // calculate Jacobian + const auto ee_frame_id = model_.getFrameId(link_name); + pinocchio::computeFrameJacobian(model_, *data_, q_, ee_frame_id, jacobian_); + jacobian = jacobian_; - return true; + return true; } bool KinematicsInterfacePinocchio::calculate_jacobian_inverse( - const Eigen::Matrix& joint_pos, const std::string& link_name, - Eigen::Matrix& 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)) - { - return false; - } + // verify inputs + if ( + !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || + !verify_jacobian_inverse(jacobian_inverse)) + { + return false; + } - // get joint array - q_ = joint_pos; + // 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(); + // 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(); - jacobian_inverse = jacobian_inverse_; + jacobian_inverse = jacobian_inverse_; - return true; + return true; } bool KinematicsInterfacePinocchio::calculate_link_transform( - const Eigen::Matrix& joint_pos, const std::string& link_name, - Eigen::Isometry3d& 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; - } + // 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; + // get joint array + q_ = joint_pos; - // reset transform_vec - transform.setIdentity(); + // 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; - } + // 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); + // 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(); + // 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; + 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( @@ -229,14 +229,14 @@ bool KinematicsInterfacePinocchio::calculate_frame_difference( if (!verify_initialized() || !verify_period(dt)) { 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_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(); @@ -250,7 +250,7 @@ bool KinematicsInterfacePinocchio::calculate_frame_difference( 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) + const Eigen::Vector3d angular = pinocchio::log3(R_rel) / dt; // 3-vector (axis * angle) delta_x.head<3>() = linear; delta_x.tail<3>() = angular; @@ -258,71 +258,70 @@ bool KinematicsInterfacePinocchio::calculate_frame_difference( return true; } -bool KinematicsInterfacePinocchio::verify_link_name(const std::string& link_name) +bool KinematicsInterfacePinocchio::verify_link_name(const std::string & link_name) { - if (link_name == root_name_) - { - return true; - } - if (!model_.existBodyName(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) { - 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; + links += "\n" + model_.frames[i].name; } - return true; + 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) +bool KinematicsInterfacePinocchio::verify_joint_vector(const Eigen::VectorXd & joint_vector) { - if (static_cast(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; + if (static_cast(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; + // 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) +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; + 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()) + 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)", @@ -342,10 +341,10 @@ bool KinematicsInterfacePinocchio::verify_period(const double dt) return true; } -} // namespace kinematics_interface_pinocchio +} // namespace kinematics_interface_pinocchio #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - kinematics_interface_pinocchio::KinematicsInterfacePinocchio, kinematics_interface::KinematicsInterface -) + 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 index 6e401cf..b2c85ee 100644 --- a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp @@ -14,219 +14,231 @@ // /// \author: Saif Sidhik +#include +#include #include "kinematics_interface/kinematics_interface.hpp" #include "pluginlib/class_loader.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "ros2_control_test_assets/descriptions.hpp" -#include -#include class TestPinocchioPlugin : public ::testing::Test { public: - std::shared_ptr> ik_loader_; - std::shared_ptr ik_; - std::shared_ptr node_; - std::string end_effector_ = "link2"; - std::string urdf_ = - std::string(ros2_control_test_assets::urdf_head) + std::string(ros2_control_test_assets::urdf_tail); - - void SetUp() - { - // init ros - rclcpp::init(0, nullptr); - node_ = std::make_shared("test_node"); - std::string plugin_name = "kinematics_interface_pinocchio/KinematicsInterfacePinocchio"; - ik_loader_ = std::make_shared>( - "kinematics_interface", "kinematics_interface::KinematicsInterface" - ); - ik_ = std::unique_ptr(ik_loader_->createUnmanagedInstance(plugin_name - )); - } - - void TearDown() - { - // shutdown ros - rclcpp::shutdown(); - } - - void loadURDFParameter() - { - auto urdf = std::string(ros2_control_test_assets::urdf_head) + std::string(ros2_control_test_assets::urdf_tail); - rclcpp::Parameter param("robot_description", urdf); - node_->declare_parameter("robot_description", ""); - node_->set_parameter(param); - } - - void loadAlphaParameter() - { - rclcpp::Parameter param("alpha", 0.005); - node_->declare_parameter("alpha", 0.005); - node_->set_parameter(param); - } + std::shared_ptr> ik_loader_; + std::shared_ptr ik_; + std::shared_ptr node_; + std::string end_effector_ = "link2"; + std::string urdf_ = std::string(ros2_control_test_assets::urdf_head) + + std::string(ros2_control_test_assets::urdf_tail); + + void SetUp() + { + // init ros + rclcpp::init(0, nullptr); + node_ = std::make_shared("test_node"); + std::string plugin_name = "kinematics_interface_pinocchio/KinematicsInterfacePinocchio"; + ik_loader_ = + std::make_shared>( + "kinematics_interface", "kinematics_interface::KinematicsInterface"); + ik_ = std::unique_ptr( + ik_loader_->createUnmanagedInstance(plugin_name)); + } + + void TearDown() + { + // shutdown ros + rclcpp::shutdown(); + } + + void loadURDFParameter() + { + auto urdf = std::string(ros2_control_test_assets::urdf_head) + + std::string(ros2_control_test_assets::urdf_tail); + rclcpp::Parameter param("robot_description", urdf); + node_->declare_parameter("robot_description", ""); + node_->set_parameter(param); + } + + void loadAlphaParameter() + { + rclcpp::Parameter param("alpha", 0.005); + node_->declare_parameter("alpha", 0.005); + node_->set_parameter(param); + } }; TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) { - // load robot description and alpha to parameter server - loadURDFParameter(); - loadAlphaParameter(); - - // initialize the plugin - ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); - - // calculate end effector transform - Eigen::Matrix pos = Eigen::Matrix::Zero(); - Eigen::Isometry3d end_effector_transform; - ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); - - // convert cartesian delta to joint delta - Eigen::Matrix delta_x = Eigen::Matrix::Zero(); - delta_x[2] = 1; - Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); - ASSERT_TRUE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); - - // convert joint delta to cartesian delta - Eigen::Matrix delta_x_est; - ASSERT_TRUE(ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); - - // Ensure kinematics math is correct - for (size_t i = 0; i < static_cast(delta_x.size()); ++i) - { - ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); - } - - // calculate jacobian - Eigen::Matrix jacobian = Eigen::Matrix::Zero(); - ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian)); - - // calculate jacobian inverse - Eigen::Matrix jacobian_inverse = - jacobian.completeOrthogonalDecomposition().pseudoInverse(); - Eigen::Matrix jacobian_inverse_est = Eigen::Matrix::Zero(); - ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); - - // ensure jacobian inverse math is correct - for (Eigen::Index i = 0; i < jacobian_inverse.rows(); ++i) - { - for (Eigen::Index j = 0; j < jacobian_inverse.cols(); ++j) - { - ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02); - } - } - - // compute the difference between two cartesian frames - Eigen::Matrix x_a, x_b; - x_a << 0, 1, 0, 0, 0, 0, 1; - x_b << 2, 3, 0, 0, 1, 0, 0; - double dt = 1.0; - delta_x = Eigen::Matrix::Zero(); - delta_x_est << 2, 2, 0, 0, 3.14, 0; - ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x)); - - // ensure that difference math is correct - for (Eigen::Index i = 0; i < delta_x.size(); ++i) + // load robot description and alpha to parameter server + loadURDFParameter(); + loadAlphaParameter(); + + // initialize the plugin + ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); + + // calculate end effector transform + Eigen::Matrix pos = Eigen::Matrix::Zero(); + Eigen::Isometry3d end_effector_transform; + ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); + + // convert cartesian delta to joint delta + Eigen::Matrix delta_x = Eigen::Matrix::Zero(); + delta_x[2] = 1; + Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); + ASSERT_TRUE( + ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); + + // convert joint delta to cartesian delta + Eigen::Matrix delta_x_est; + ASSERT_TRUE( + ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); + + // Ensure kinematics math is correct + for (size_t i = 0; i < static_cast(delta_x.size()); ++i) + { + ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); + } + + // calculate jacobian + Eigen::Matrix jacobian = Eigen::Matrix::Zero(); + ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian)); + + // calculate jacobian inverse + Eigen::Matrix jacobian_inverse = + jacobian.completeOrthogonalDecomposition().pseudoInverse(); + Eigen::Matrix jacobian_inverse_est = + Eigen::Matrix::Zero(); + ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); + + // ensure jacobian inverse math is correct + for (Eigen::Index i = 0; i < jacobian_inverse.rows(); ++i) + { + for (Eigen::Index j = 0; j < jacobian_inverse.cols(); ++j) { - EXPECT_NEAR(delta_x(i), delta_x_est(i), 0.02) << " for index " << i; + ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02); } + } + + // compute the difference between two cartesian frames + Eigen::Matrix x_a, x_b; + x_a << 0, 1, 0, 0, 0, 0, 1; + x_b << 2, 3, 0, 0, 1, 0, 0; + double dt = 1.0; + delta_x = Eigen::Matrix::Zero(); + delta_x_est << 2, 2, 0, 0, 3.14, 0; + ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x)); + + // ensure that difference math is correct + for (Eigen::Index i = 0; i < delta_x.size(); ++i) + { + EXPECT_NEAR(delta_x(i), delta_x_est(i), 0.02) << " for index " << i; + } } TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) { - // load robot description and alpha to parameter server - loadURDFParameter(); - loadAlphaParameter(); - - // initialize the plugin - ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); - - // calculate end effector transform - std::vector pos = {0, 0, 0}; - Eigen::Isometry3d end_effector_transform; - ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); - - // convert cartesian delta to joint delta - std::vector delta_x = {0, 0, 0, 0, 0, 0}; - delta_x[2] = 1; - std::vector delta_theta = {0, 0, 0}; - ASSERT_TRUE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); - - // convert joint delta to cartesian delta - std::vector delta_x_est(6); - ASSERT_TRUE(ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); - - // Ensure kinematics math is correct - for (size_t i = 0; i < static_cast(delta_x.size()); ++i) - { - ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); - } - - // calculate jacobian - Eigen::Matrix jacobian = Eigen::Matrix::Zero(); - ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian)); - - // calculate jacobian inverse - Eigen::Matrix jacobian_inverse = - jacobian.completeOrthogonalDecomposition().pseudoInverse(); - Eigen::Matrix jacobian_inverse_est = Eigen::Matrix::Zero(); - ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); - - // ensure jacobian inverse math is correct - for (Eigen::Index i = 0; i < jacobian_inverse.rows(); ++i) + // load robot description and alpha to parameter server + loadURDFParameter(); + loadAlphaParameter(); + + // initialize the plugin + ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); + + // calculate end effector transform + std::vector pos = {0, 0, 0}; + Eigen::Isometry3d end_effector_transform; + ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); + + // convert cartesian delta to joint delta + std::vector delta_x = {0, 0, 0, 0, 0, 0}; + delta_x[2] = 1; + std::vector delta_theta = {0, 0, 0}; + ASSERT_TRUE( + ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); + + // convert joint delta to cartesian delta + std::vector delta_x_est(6); + ASSERT_TRUE( + ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); + + // Ensure kinematics math is correct + for (size_t i = 0; i < static_cast(delta_x.size()); ++i) + { + ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); + } + + // calculate jacobian + Eigen::Matrix jacobian = Eigen::Matrix::Zero(); + ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian)); + + // calculate jacobian inverse + Eigen::Matrix jacobian_inverse = + jacobian.completeOrthogonalDecomposition().pseudoInverse(); + Eigen::Matrix jacobian_inverse_est = + Eigen::Matrix::Zero(); + ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); + + // ensure jacobian inverse math is correct + for (Eigen::Index i = 0; i < jacobian_inverse.rows(); ++i) + { + for (Eigen::Index j = 0; j < jacobian_inverse.cols(); ++j) { - for (Eigen::Index j = 0; j < jacobian_inverse.cols(); ++j) - { - ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02); - } + ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02); } + } } TEST_F(TestPinocchioPlugin, incorrect_input_sizes) { - // load robot description and alpha to parameter server - loadURDFParameter(); - loadAlphaParameter(); - - // initialize the plugin - ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); - - // define correct values - Eigen::Matrix pos = Eigen::Matrix::Zero(); - Eigen::Isometry3d end_effector_transform; - Eigen::Matrix delta_x = Eigen::Matrix::Zero(); - delta_x[2] = 1; - Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); - Eigen::Matrix delta_x_est; - Eigen::Matrix jacobian = Eigen::Matrix::Zero(); - - // wrong size input vector - Eigen::Matrix vec_5 = Eigen::Matrix::Zero(); - // wrong size input jacobian - Eigen::Matrix mat_5_6 = Eigen::Matrix::Zero(); - - // calculate transform - ASSERT_FALSE(ik_->calculate_link_transform(vec_5, end_effector_, end_effector_transform)); - ASSERT_FALSE(ik_->calculate_link_transform(pos, "link_not_in_model", end_effector_transform)); - - // convert cartesian delta to joint delta - ASSERT_FALSE(ik_->convert_cartesian_deltas_to_joint_deltas(vec_5, delta_x, end_effector_, delta_theta)); - ASSERT_FALSE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, "link_not_in_model", delta_theta)); - ASSERT_FALSE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, vec_5)); - - // convert joint delta to cartesian delta - ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas(vec_5, delta_theta, end_effector_, delta_x_est)); - ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas(pos, vec_5, end_effector_, delta_x_est)); - ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, "link_not_in_model", delta_x_est)); - - // calculate jacobian inverse - ASSERT_FALSE(ik_->calculate_jacobian_inverse(vec_5, end_effector_, jacobian)); - ASSERT_FALSE(ik_->calculate_jacobian_inverse(pos, end_effector_, mat_5_6)); - ASSERT_FALSE(ik_->calculate_jacobian_inverse(pos, "link_not_in_model", jacobian)); + // load robot description and alpha to parameter server + loadURDFParameter(); + loadAlphaParameter(); + + // initialize the plugin + ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); + + // define correct values + Eigen::Matrix pos = Eigen::Matrix::Zero(); + Eigen::Isometry3d end_effector_transform; + Eigen::Matrix delta_x = Eigen::Matrix::Zero(); + delta_x[2] = 1; + Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); + Eigen::Matrix delta_x_est; + Eigen::Matrix jacobian = Eigen::Matrix::Zero(); + + // wrong size input vector + Eigen::Matrix vec_5 = Eigen::Matrix::Zero(); + // wrong size input jacobian + Eigen::Matrix mat_5_6 = Eigen::Matrix::Zero(); + + // calculate transform + ASSERT_FALSE(ik_->calculate_link_transform(vec_5, end_effector_, end_effector_transform)); + ASSERT_FALSE(ik_->calculate_link_transform(pos, "link_not_in_model", end_effector_transform)); + + // convert cartesian delta to joint delta + ASSERT_FALSE( + ik_->convert_cartesian_deltas_to_joint_deltas(vec_5, delta_x, end_effector_, delta_theta)); + ASSERT_FALSE( + ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, "link_not_in_model", delta_theta)); + ASSERT_FALSE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, vec_5)); + + // convert joint delta to cartesian delta + ASSERT_FALSE( + ik_->convert_joint_deltas_to_cartesian_deltas(vec_5, delta_theta, end_effector_, delta_x_est)); + ASSERT_FALSE( + ik_->convert_joint_deltas_to_cartesian_deltas(pos, vec_5, end_effector_, delta_x_est)); + ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas( + pos, delta_theta, "link_not_in_model", delta_x_est)); + + // calculate jacobian inverse + ASSERT_FALSE(ik_->calculate_jacobian_inverse(vec_5, end_effector_, jacobian)); + ASSERT_FALSE(ik_->calculate_jacobian_inverse(pos, end_effector_, mat_5_6)); + ASSERT_FALSE(ik_->calculate_jacobian_inverse(pos, "link_not_in_model", jacobian)); } TEST_F(TestPinocchioPlugin, Pinocchio_plugin_no_robot_description) { - // load alpha to parameter server - loadAlphaParameter(); - ASSERT_FALSE(ik_->initialize("", node_->get_node_parameters_interface(), "")); + // load alpha to parameter server + loadAlphaParameter(); + ASSERT_FALSE(ik_->initialize("", node_->get_node_parameters_interface(), "")); } From 936d8a80c3b2d0070f7507bdd45939864ba275bc Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 3 Oct 2025 11:21:55 +0000 Subject: [PATCH 12/27] Fix -Wsign-conversion --- .../kinematics_interface_pinocchio.hpp | 2 +- .../src/kinematics_interface_pinocchio.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) 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 index 037ca47..855536c 100644 --- a/kinematics_interface_pinocchio/include/kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp +++ b/kinematics_interface_pinocchio/include/kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp @@ -79,7 +79,7 @@ class KinematicsInterfacePinocchio : public kinematics_interface::KinematicsInte bool initialized = false; std::string root_name_; - size_t num_joints_; + Eigen::Index num_joints_; pinocchio::Model model_; std::shared_ptr data_; diff --git a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp index cb5cdcc..8ea5b45 100644 --- a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp @@ -73,7 +73,7 @@ bool KinematicsInterfacePinocchio::initialize( // allocate dynamics memory data_ = std::make_shared(model_); - num_joints_ = model_.nq; // TODO(anyone): handle floating base + num_joints_ = static_cast(model_.nq); // TODO(anyone): handle floating base q_.resize(num_joints_); I = Eigen::MatrixXd(num_joints_, num_joints_); I.setIdentity(); @@ -281,7 +281,7 @@ bool KinematicsInterfacePinocchio::verify_link_name(const std::string & link_nam bool KinematicsInterfacePinocchio::verify_joint_vector(const Eigen::VectorXd & joint_vector) { - if (static_cast(joint_vector.size()) != num_joints_) + if (joint_vector.size() != num_joints_) { RCLCPP_ERROR( LOGGER, "Invalid joint vector size (%zu). Expected size is %zu.", joint_vector.size(), From d9248486b0b209255786143a4eb2cf21eaa2ad0d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 3 Oct 2025 11:28:48 +0000 Subject: [PATCH 13/27] Install octomap --- .github/workflows/rolling-semi-binary-build-win.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/rolling-semi-binary-build-win.yml b/.github/workflows/rolling-semi-binary-build-win.yml index c98e30e..8a7303c 100644 --- a/.github/workflows/rolling-semi-binary-build-win.yml +++ b/.github/workflows/rolling-semi-binary-build-win.yml @@ -16,5 +16,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 From 616058e53aff67e2fe692602eba1de3eb98f614f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 3 Oct 2025 11:46:02 +0000 Subject: [PATCH 14/27] Proper shut down the lifecycle node --- kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp | 1 + .../test/test_kinematics_interface_pinocchio.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp index 48ead26..2e492ed 100644 --- a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp @@ -46,6 +46,7 @@ class TestKDLPlugin : public ::testing::Test void TearDown() { + node_->shutdown(); // shutdown ros rclcpp::shutdown(); } diff --git a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp index b2c85ee..a17a89b 100644 --- a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp @@ -46,6 +46,7 @@ class TestPinocchioPlugin : public ::testing::Test void TearDown() { + node_->shutdown(); // shutdown ros rclcpp::shutdown(); } From f3c4b0a758d2d4231fe310d9b761ea09faca8a2e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 3 Oct 2025 11:46:21 +0000 Subject: [PATCH 15/27] Fix another -Wsign-conversion --- .../test/test_kinematics_interface_pinocchio.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp index a17a89b..3ceadcf 100644 --- a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp @@ -95,7 +95,7 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); // Ensure kinematics math is correct - for (size_t i = 0; i < static_cast(delta_x.size()); ++i) + for (Eigen::Index i = 0; i < delta_x.size(); ++i) { ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); } From ece9aeb51c778218c9567bd1491760b9d7aa1e5a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 3 Oct 2025 12:38:32 +0000 Subject: [PATCH 16/27] Fix tests with jacobian --- .../src/kinematics_interface_pinocchio.cpp | 5 ++++- .../test/test_kinematics_interface_pinocchio.cpp | 8 ++++++-- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp index 8ea5b45..348432e 100644 --- a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp @@ -73,8 +73,9 @@ bool KinematicsInterfacePinocchio::initialize( // allocate dynamics memory data_ = std::make_shared(model_); - num_joints_ = static_cast(model_.nq); // TODO(anyone): handle floating base + num_joints_ = static_cast(model_.nq); q_.resize(num_joints_); + // TODO(anyone): fix sizes of jabian if tool frame is not the last one I = Eigen::MatrixXd(num_joints_, num_joints_); I.setIdentity(); jacobian_.resize(6, num_joints_); @@ -149,6 +150,7 @@ bool KinematicsInterfacePinocchio::calculate_jacobian( // calculate Jacobian const auto ee_frame_id = model_.getFrameId(link_name); pinocchio::computeFrameJacobian(model_, *data_, q_, ee_frame_id, jacobian_); + // TODO(anyone): fix sizes of jacobian jacobian = jacobian_; return true; @@ -176,6 +178,7 @@ bool KinematicsInterfacePinocchio::calculate_jacobian_inverse( jacobian_inverse_ = (jacobian_.transpose() * jacobian_ + alpha * I).inverse() * jacobian_.transpose(); + // TODO(anyone): fix sizes of jacobian inverse jacobian_inverse = jacobian_inverse_; return true; diff --git a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp index 3ceadcf..8b70952 100644 --- a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp @@ -171,20 +171,24 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) // calculate jacobian Eigen::Matrix jacobian = Eigen::Matrix::Zero(); ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian)); + // TODO(anyone): fix sizes of jacobian + auto jacobian_6x2 = jacobian.block(0, 0, 6, 2); // calculate jacobian inverse Eigen::Matrix jacobian_inverse = - jacobian.completeOrthogonalDecomposition().pseudoInverse(); + jacobian_6x2.completeOrthogonalDecomposition().pseudoInverse(); Eigen::Matrix jacobian_inverse_est = Eigen::Matrix::Zero(); ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); + // TODO(anyone): fix sizes of jacobian inverse + auto jacobian_inverse_est_2x6 = jacobian_inverse_est.block(0, 0, 2, 6); // ensure jacobian inverse math is correct for (Eigen::Index i = 0; i < jacobian_inverse.rows(); ++i) { for (Eigen::Index j = 0; j < jacobian_inverse.cols(); ++j) { - ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02); + ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est_2x6(i, j), 0.02); } } } From 20fa92428943e51aba98f8bfacbfb794f855ed61 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 3 Oct 2025 13:05:26 +0000 Subject: [PATCH 17/27] Hardcode last column of jacobian --- .../src/kinematics_interface_pinocchio.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp index 348432e..7113ec8 100644 --- a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp @@ -103,6 +103,8 @@ bool KinematicsInterfacePinocchio::convert_joint_deltas_to_cartesian_deltas( // calculate Jacobian const auto ee_frame_id = model_.getFrameId(link_name); pinocchio::computeFrameJacobian(model_, *data_, q_, ee_frame_id, jacobian_); + // TODO(anyone): fix sizes of jacobian + jacobian_.col(jacobian_.cols() - 1).setZero(); delta_x = jacobian_ * delta_theta; return true; From 4c8ac2665c0dabf2eb081b5c02973e4cb714727e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 3 Oct 2025 18:56:35 +0000 Subject: [PATCH 18/27] Add correct buildfarm badges --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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) | [![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)
[![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)
[![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/)
[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
[![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/)
[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)
[![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)
[![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/)
[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)
[![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)
[![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/)
[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)
[![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)
[![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/)
[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/)
[![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/)
[![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
[![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/)
[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/)
[![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/)
[![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)
[![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)
[![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/)
[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/)
[![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)
[![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)
[![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/)
[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/)
[![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 From 38e7ceb5bf67b41be10f9d9c7ad41d6ec32cf9d1 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 20 Oct 2025 13:47:29 +0000 Subject: [PATCH 19/27] Use a reduced model if configured tip parameter --- .../src/kinematics_interface_pinocchio.cpp | 74 +++++++++++++++---- .../test_kinematics_interface_pinocchio.cpp | 40 +++++----- 2 files changed, 84 insertions(+), 30 deletions(-) diff --git a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp index 7113ec8..ecb82cd 100644 --- a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp @@ -48,34 +48,85 @@ bool KinematicsInterfacePinocchio::initialize( { 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("alpha")) + if (parameters_interface->has_parameter(ns + "alpha")) { - parameters_interface->get_parameter("alpha", alpha_param); + parameters_interface->get_parameter(ns + "alpha", alpha_param); } alpha = alpha_param.as_double(); - // get root name + // 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("base")) + if (parameters_interface->has_parameter(ns + "base")) { - parameters_interface->get_parameter("base", base_param); + parameters_interface->get_parameter(ns + "base", base_param); root_name_ = base_param.as_string(); + // TODO(anyone): fix root name usage + pinocchio::urdf::buildModelFromXML( + robot_description_local, /*root_name_,*/ full_model, verbose, true); } else { - root_name_ = model_.frames[0].name; + pinocchio::urdf::buildModelFromXML(robot_description_local, full_model, verbose, true); + } + // create reduced model by locking joints after end-effector + pinocchio::FrameIndex frame_id = full_model.getFrameId(end_effector_name); + const pinocchio::Frame & frame = full_model.frames[frame_id]; + pinocchio::JointIndex tool_joint_id = frame.parent; // the joint to which this frame is attached + auto const isChildOf = []( + const pinocchio::Model & model, pinocchio::JointIndex ancestor, + pinocchio::JointIndex descendant) + { + pinocchio::JointIndex current = descendant; + while (current > 0) + { + if (current == ancestor) return true; + current = model.parents[current]; + } + return false; + }; + std::vector locked_joints; + for (pinocchio::JointIndex jid = 1; jid < static_cast(full_model.njoints); + ++jid) + { + if (isChildOf(full_model, tool_joint_id, jid) && jid != tool_joint_id) + locked_joints.push_back(jid); } - // TODO(anyone): only handling fixed base now - model_ = - pinocchio::urdf::buildModelFromXML(robot_description_local, /*root_name_,*/ model_, true); + RCLCPP_INFO( + LOGGER, "Locked %zu joint(s) after tool frame %s", locked_joints.size(), + end_effector_name.c_str()); + 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 dynamics memory data_ = std::make_shared(model_); num_joints_ = static_cast(model_.nq); q_.resize(num_joints_); - // TODO(anyone): fix sizes of jabian if tool frame is not the last one I = Eigen::MatrixXd(num_joints_, num_joints_); I.setIdentity(); jacobian_.resize(6, num_joints_); @@ -103,8 +154,6 @@ bool KinematicsInterfacePinocchio::convert_joint_deltas_to_cartesian_deltas( // calculate Jacobian const auto ee_frame_id = model_.getFrameId(link_name); pinocchio::computeFrameJacobian(model_, *data_, q_, ee_frame_id, jacobian_); - // TODO(anyone): fix sizes of jacobian - jacobian_.col(jacobian_.cols() - 1).setZero(); delta_x = jacobian_ * delta_theta; return true; @@ -152,7 +201,6 @@ bool KinematicsInterfacePinocchio::calculate_jacobian( // calculate Jacobian const auto ee_frame_id = model_.getFrameId(link_name); pinocchio::computeFrameJacobian(model_, *data_, q_, ee_frame_id, jacobian_); - // TODO(anyone): fix sizes of jacobian jacobian = jacobian_; return true; diff --git a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp index 8b70952..93ebcad 100644 --- a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp @@ -42,6 +42,8 @@ class TestPinocchioPlugin : public ::testing::Test "kinematics_interface", "kinematics_interface::KinematicsInterface"); ik_ = std::unique_ptr( ik_loader_->createUnmanagedInstance(plugin_name)); + + node_->declare_parameter("verbose", true); } void TearDown() @@ -53,9 +55,7 @@ class TestPinocchioPlugin : public ::testing::Test void loadURDFParameter() { - auto urdf = std::string(ros2_control_test_assets::urdf_head) + - std::string(ros2_control_test_assets::urdf_tail); - rclcpp::Parameter param("robot_description", urdf); + rclcpp::Parameter param("robot_description", urdf_); node_->declare_parameter("robot_description", ""); node_->set_parameter(param); } @@ -66,6 +66,13 @@ class TestPinocchioPlugin : public ::testing::Test node_->declare_parameter("alpha", 0.005); node_->set_parameter(param); } + + void loadTipParameter() + { + rclcpp::Parameter param("tip", end_effector_); + node_->declare_parameter("tip", end_effector_); + node_->set_parameter(param); + } }; TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) @@ -73,19 +80,20 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) // load robot description and alpha to parameter server loadURDFParameter(); loadAlphaParameter(); + loadTipParameter(); // initialize the plugin ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); // calculate end effector transform - Eigen::Matrix pos = Eigen::Matrix::Zero(); + Eigen::Matrix pos = Eigen::Matrix::Zero(); Eigen::Isometry3d end_effector_transform; ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); // convert cartesian delta to joint delta Eigen::Matrix delta_x = Eigen::Matrix::Zero(); delta_x[2] = 1; - Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); + Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); ASSERT_TRUE( ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); @@ -101,14 +109,14 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) } // calculate jacobian - Eigen::Matrix jacobian = Eigen::Matrix::Zero(); + Eigen::Matrix jacobian = Eigen::Matrix::Zero(); ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian)); // calculate jacobian inverse Eigen::Matrix jacobian_inverse = jacobian.completeOrthogonalDecomposition().pseudoInverse(); Eigen::Matrix jacobian_inverse_est = - Eigen::Matrix::Zero(); + Eigen::Matrix::Zero(); ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); // ensure jacobian inverse math is correct @@ -141,19 +149,20 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) // load robot description and alpha to parameter server loadURDFParameter(); loadAlphaParameter(); + loadTipParameter(); // initialize the plugin ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); // calculate end effector transform - std::vector pos = {0, 0, 0}; + std::vector pos = {0, 0}; Eigen::Isometry3d end_effector_transform; ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); // convert cartesian delta to joint delta std::vector delta_x = {0, 0, 0, 0, 0, 0}; delta_x[2] = 1; - std::vector delta_theta = {0, 0, 0}; + std::vector delta_theta = {0, 0}; ASSERT_TRUE( ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); @@ -169,26 +178,22 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) } // calculate jacobian - Eigen::Matrix jacobian = Eigen::Matrix::Zero(); + Eigen::Matrix jacobian = Eigen::Matrix::Zero(); ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian)); - // TODO(anyone): fix sizes of jacobian - auto jacobian_6x2 = jacobian.block(0, 0, 6, 2); // calculate jacobian inverse Eigen::Matrix jacobian_inverse = - jacobian_6x2.completeOrthogonalDecomposition().pseudoInverse(); + jacobian.completeOrthogonalDecomposition().pseudoInverse(); Eigen::Matrix jacobian_inverse_est = - Eigen::Matrix::Zero(); + Eigen::Matrix::Zero(); ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); - // TODO(anyone): fix sizes of jacobian inverse - auto jacobian_inverse_est_2x6 = jacobian_inverse_est.block(0, 0, 2, 6); // ensure jacobian inverse math is correct for (Eigen::Index i = 0; i < jacobian_inverse.rows(); ++i) { for (Eigen::Index j = 0; j < jacobian_inverse.cols(); ++j) { - ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est_2x6(i, j), 0.02); + ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02); } } } @@ -198,6 +203,7 @@ TEST_F(TestPinocchioPlugin, incorrect_input_sizes) // load robot description and alpha to parameter server loadURDFParameter(); loadAlphaParameter(); + loadTipParameter(); // initialize the plugin ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); From f613abaefcd6fdeb9c8f8e510c2d8eb6e524a84b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 20 Oct 2025 14:39:45 +0000 Subject: [PATCH 20/27] Add more tests and harden parameter handling --- .../src/kinematics_interface_pinocchio.cpp | 36 +++++- .../test_kinematics_interface_pinocchio.cpp | 119 ++++++++++++++---- 2 files changed, 125 insertions(+), 30 deletions(-) diff --git a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp index ecb82cd..b44437e 100644 --- a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp @@ -85,13 +85,43 @@ bool KinematicsInterfacePinocchio::initialize( { parameters_interface->get_parameter(ns + "base", base_param); root_name_ = base_param.as_string(); + } + if (!root_name_.empty()) + { // TODO(anyone): fix root name usage - pinocchio::urdf::buildModelFromXML( - robot_description_local, /*root_name_,*/ full_model, verbose, true); + try + { + pinocchio::urdf::buildModelFromXML( + robot_description_local, /*root_name_,*/ full_model, verbose, true); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + LOGGER, "Error parsing URDF to build Pinocchio model from base '%s': %s", + root_name_.c_str(), e.what()); + return false; + } } else { - pinocchio::urdf::buildModelFromXML(robot_description_local, full_model, verbose, true); + 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; + } + root_name_ = full_model.names[full_model.frames[0].parent]; + } + + if (!full_model.existFrame(end_effector_name)) + { + RCLCPP_ERROR( + LOGGER, "failed to find chain from robot root '%s' to end effector '%s'", root_name_.c_str(), + end_effector_name.c_str()); + return false; } // create reduced model by locking joints after end-effector pinocchio::FrameIndex frame_id = full_model.getFrameId(end_effector_name); diff --git a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp index 93ebcad..1142ceb 100644 --- a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp @@ -44,6 +44,9 @@ class TestPinocchioPlugin : public ::testing::Test ik_loader_->createUnmanagedInstance(plugin_name)); node_->declare_parameter("verbose", true); + node_->declare_parameter("alpha", 0.005); + node_->declare_parameter("robot_description", urdf_); + node_->declare_parameter("tip", end_effector_); } void TearDown() @@ -53,35 +56,102 @@ class TestPinocchioPlugin : public ::testing::Test rclcpp::shutdown(); } - void loadURDFParameter() + void loadURDFParameter(std::string urdf) { - rclcpp::Parameter param("robot_description", urdf_); - node_->declare_parameter("robot_description", ""); + rclcpp::Parameter param("robot_description", urdf); node_->set_parameter(param); } - void loadAlphaParameter() + /** + * \brief Used for testing initialization from parameters. + * Elsewhere, default 0.005 is used. + */ + + void loadAlphaParameter(double alpha) { - rclcpp::Parameter param("alpha", 0.005); - node_->declare_parameter("alpha", 0.005); + rclcpp::Parameter param("alpha", alpha); node_->set_parameter(param); } - void loadTipParameter() + /** + * \brief Used for testing initialization from parameters. + * Elsewhere, `end_effector_` member is used. + */ + void loadTipParameter(std::string tip) { - rclcpp::Parameter param("tip", end_effector_); - node_->declare_parameter("tip", end_effector_); + rclcpp::Parameter param("tip", tip); node_->set_parameter(param); } }; TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) { - // load robot description and alpha to parameter server - loadURDFParameter(); - loadAlphaParameter(); - loadTipParameter(); + loadTipParameter("link3"); + + // initialize the plugin + ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); + + // calculate end effector transform + Eigen::Matrix pos = Eigen::Matrix::Zero(); + Eigen::Isometry3d end_effector_transform; + ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); + + // convert cartesian delta to joint delta + Eigen::Matrix delta_x = Eigen::Matrix::Zero(); + delta_x[2] = 1; + Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); + ASSERT_TRUE( + ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); + + // convert joint delta to cartesian delta + Eigen::Matrix delta_x_est; + ASSERT_TRUE( + ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); + + // Ensure kinematics math is correct + for (Eigen::Index i = 0; i < delta_x.size(); ++i) + { + ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); + } + + // calculate jacobian + Eigen::Matrix jacobian = Eigen::Matrix::Zero(); + ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian)); + + // calculate jacobian inverse + Eigen::Matrix jacobian_inverse = + jacobian.completeOrthogonalDecomposition().pseudoInverse(); + Eigen::Matrix jacobian_inverse_est = + Eigen::Matrix::Zero(); + ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); + + // ensure jacobian inverse math is correct + for (Eigen::Index i = 0; i < jacobian_inverse.rows(); ++i) + { + for (Eigen::Index j = 0; j < jacobian_inverse.cols(); ++j) + { + ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02); + } + } + // compute the difference between two cartesian frames + Eigen::Matrix x_a, x_b; + x_a << 0, 1, 0, 0, 0, 0, 1; + x_b << 2, 3, 0, 0, 1, 0, 0; + double dt = 1.0; + delta_x = Eigen::Matrix::Zero(); + delta_x_est << 2, 2, 0, 0, 3.14, 0; + ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x)); + + // ensure that difference math is correct + for (Eigen::Index i = 0; i < delta_x.size(); ++i) + { + EXPECT_NEAR(delta_x(i), delta_x_est(i), 0.02) << " for index " << i; + } +} + +TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_reduced_model_tip) +{ // initialize the plugin ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); @@ -146,12 +216,7 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) { - // load robot description and alpha to parameter server - loadURDFParameter(); - loadAlphaParameter(); - loadTipParameter(); - - // initialize the plugin + // initialize the plugin ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); // calculate end effector transform @@ -200,12 +265,7 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) TEST_F(TestPinocchioPlugin, incorrect_input_sizes) { - // load robot description and alpha to parameter server - loadURDFParameter(); - loadAlphaParameter(); - loadTipParameter(); - - // initialize the plugin + // initialize the plugin ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); // define correct values @@ -249,7 +309,12 @@ TEST_F(TestPinocchioPlugin, incorrect_input_sizes) TEST_F(TestPinocchioPlugin, Pinocchio_plugin_no_robot_description) { - // load alpha to parameter server - loadAlphaParameter(); + loadURDFParameter(""); ASSERT_FALSE(ik_->initialize("", node_->get_node_parameters_interface(), "")); } + +TEST_F(TestPinocchioPlugin, Pinocchio_plugin_no_parameter_tip) +{ + loadTipParameter(""); + ASSERT_FALSE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); +} From c6224bca311ecf1c7798fd82f33f88ec4d179ca1 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 20 Oct 2025 14:43:06 +0000 Subject: [PATCH 21/27] Use gmock --- .../test_kinematics_interface_pinocchio.cpp | 75 +++++++++---------- 1 file changed, 37 insertions(+), 38 deletions(-) diff --git a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp index 1142ceb..dd961cb 100644 --- a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp @@ -21,6 +21,11 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "ros2_control_test_assets/descriptions.hpp" +MATCHER_P2(MatrixNear, expected, tol, "Two matrices are approximately equal") +{ + return arg.isApprox(expected, tol); +} + class TestPinocchioPlugin : public ::testing::Test { public: @@ -47,6 +52,7 @@ class TestPinocchioPlugin : public ::testing::Test node_->declare_parameter("alpha", 0.005); node_->declare_parameter("robot_description", urdf_); node_->declare_parameter("tip", end_effector_); + node_->declare_parameter("base", std::string("")); } void TearDown() @@ -82,6 +88,16 @@ class TestPinocchioPlugin : public ::testing::Test rclcpp::Parameter param("tip", tip); node_->set_parameter(param); } + + /** + * \brief Used for testing initialization from parameters. + * Elsewhere, `""` is used. + */ + void loadBaseParameter(std::string base) + { + rclcpp::Parameter param("base", base); + node_->set_parameter(param); + } }; TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) @@ -109,10 +125,7 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); // Ensure kinematics math is correct - for (Eigen::Index i = 0; i < delta_x.size(); ++i) - { - ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); - } + EXPECT_THAT(delta_x, MatrixNear(delta_x_est, 0.02)); // calculate jacobian Eigen::Matrix jacobian = Eigen::Matrix::Zero(); @@ -126,13 +139,7 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); // ensure jacobian inverse math is correct - for (Eigen::Index i = 0; i < jacobian_inverse.rows(); ++i) - { - for (Eigen::Index j = 0; j < jacobian_inverse.cols(); ++j) - { - ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02); - } - } + EXPECT_THAT(jacobian_inverse, MatrixNear(jacobian_inverse_est, 0.02)); // compute the difference between two cartesian frames Eigen::Matrix x_a, x_b; @@ -144,15 +151,12 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x)); // ensure that difference math is correct - for (Eigen::Index i = 0; i < delta_x.size(); ++i) - { - EXPECT_NEAR(delta_x(i), delta_x_est(i), 0.02) << " for index " << i; - } + EXPECT_THAT(delta_x, MatrixNear(delta_x_est, 0.02)); } TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_reduced_model_tip) { - // initialize the plugin + // initialize the plugin ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); // calculate end effector transform @@ -173,10 +177,7 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_reduced_model_tip) ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); // Ensure kinematics math is correct - for (Eigen::Index i = 0; i < delta_x.size(); ++i) - { - ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); - } + EXPECT_THAT(delta_x, MatrixNear(delta_x_est, 0.02)); // calculate jacobian Eigen::Matrix jacobian = Eigen::Matrix::Zero(); @@ -190,13 +191,7 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_reduced_model_tip) ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); // ensure jacobian inverse math is correct - for (Eigen::Index i = 0; i < jacobian_inverse.rows(); ++i) - { - for (Eigen::Index j = 0; j < jacobian_inverse.cols(); ++j) - { - ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02); - } - } + EXPECT_THAT(jacobian_inverse, MatrixNear(jacobian_inverse_est, 0.02)); // compute the difference between two cartesian frames Eigen::Matrix x_a, x_b; @@ -208,6 +203,7 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_reduced_model_tip) ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x)); // ensure that difference math is correct + EXPECT_THAT(delta_x, MatrixNear(delta_x_est, 0.02)); for (Eigen::Index i = 0; i < delta_x.size(); ++i) { EXPECT_NEAR(delta_x(i), delta_x_est(i), 0.02) << " for index " << i; @@ -237,10 +233,7 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); // Ensure kinematics math is correct - for (size_t i = 0; i < static_cast(delta_x.size()); ++i) - { - ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); - } + EXPECT_THAT(delta_x, ::testing::Pointwise(::testing::DoubleNear(0.02), delta_x_est)); // calculate jacobian Eigen::Matrix jacobian = Eigen::Matrix::Zero(); @@ -254,13 +247,19 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); // ensure jacobian inverse math is correct - for (Eigen::Index i = 0; i < jacobian_inverse.rows(); ++i) - { - for (Eigen::Index j = 0; j < jacobian_inverse.cols(); ++j) - { - ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02); - } - } + EXPECT_THAT(jacobian_inverse, MatrixNear(jacobian_inverse_est, 0.02)); + + // compute the difference between two cartesian frames + std::vector x_a(7), x_b(7); + x_a = {0, 1, 0, 0, 0, 0, 1}; + x_b = {2, 3, 0, 0, 1, 0, 0}; + double dt = 1.0; + delta_x = {0, 0, 0, 0, 0, 0}; + delta_x_est = {2, 2, 0, 0, 3.14, 0}; + ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x)); + + // ensure that difference math is correct + EXPECT_THAT(delta_x, ::testing::Pointwise(::testing::DoubleNear(0.02), delta_x_est)); } TEST_F(TestPinocchioPlugin, incorrect_input_sizes) From c07fc10edd1e14f30df54f7d379f92f55bcce7cb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 20 Oct 2025 15:27:03 +0000 Subject: [PATCH 22/27] Add error messages, improve doc and split tests --- .../kinematics_interface.hpp | 8 ++-- .../src/kinematics_interface_pinocchio.cpp | 7 ++- .../test_kinematics_interface_pinocchio.cpp | 43 +++++++++++-------- 3 files changed, 36 insertions(+), 22 deletions(-) diff --git a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp index 795f4e2..6c59b97 100644 --- a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp +++ b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp @@ -109,11 +109,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/src/kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp index b44437e..38568a6 100644 --- a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp @@ -175,6 +175,7 @@ bool KinematicsInterfacePinocchio::convert_joint_deltas_to_cartesian_deltas( !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || !verify_joint_vector(delta_theta)) { + RCLCPP_ERROR(LOGGER, "Input verification failed in convert_joint_deltas_to_cartesian_deltas"); return false; } @@ -199,6 +200,7 @@ bool KinematicsInterfacePinocchio::convert_cartesian_deltas_to_joint_deltas( !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || !verify_joint_vector(delta_theta)) { + RCLCPP_ERROR(LOGGER, "Input verification failed in convert_cartesian_deltas_to_joint_deltas"); return false; } @@ -222,6 +224,7 @@ bool KinematicsInterfacePinocchio::calculate_jacobian( !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || !verify_jacobian(jacobian)) { + RCLCPP_ERROR(LOGGER, "Input verification failed in calculate_jacobian"); return false; } @@ -245,6 +248,7 @@ bool KinematicsInterfacePinocchio::calculate_jacobian_inverse( !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || !verify_jacobian_inverse(jacobian_inverse)) { + RCLCPP_ERROR(LOGGER, "Input verification failed in calculate_jacobian_inverse"); return false; } @@ -309,8 +313,9 @@ bool KinematicsInterfacePinocchio::calculate_frame_difference( Eigen::Matrix & delta_x) { // verify inputs - if (!verify_initialized() || !verify_period(dt)) + if (!verify_period(dt)) { + RCLCPP_ERROR(LOGGER, "Input verification failed in calculate_frame_difference"); return false; } diff --git a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp index dd961cb..f070ba0 100644 --- a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp @@ -114,7 +114,7 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) // convert cartesian delta to joint delta Eigen::Matrix delta_x = Eigen::Matrix::Zero(); - delta_x[2] = 1; + delta_x[2] = 1; // vz Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); ASSERT_TRUE( ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); @@ -140,18 +140,6 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) // ensure jacobian inverse math is correct EXPECT_THAT(jacobian_inverse, MatrixNear(jacobian_inverse_est, 0.02)); - - // compute the difference between two cartesian frames - Eigen::Matrix x_a, x_b; - x_a << 0, 1, 0, 0, 0, 0, 1; - x_b << 2, 3, 0, 0, 1, 0, 0; - double dt = 1.0; - delta_x = Eigen::Matrix::Zero(); - delta_x_est << 2, 2, 0, 0, 3.14, 0; - ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x)); - - // ensure that difference math is correct - EXPECT_THAT(delta_x, MatrixNear(delta_x_est, 0.02)); } TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_reduced_model_tip) @@ -166,7 +154,7 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_reduced_model_tip) // convert cartesian delta to joint delta Eigen::Matrix delta_x = Eigen::Matrix::Zero(); - delta_x[2] = 1; + delta_x[2] = 1; // vz Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); ASSERT_TRUE( ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); @@ -222,7 +210,7 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) // convert cartesian delta to joint delta std::vector delta_x = {0, 0, 0, 0, 0, 0}; - delta_x[2] = 1; + delta_x[2] = 1; // vz std::vector delta_theta = {0, 0}; ASSERT_TRUE( ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); @@ -248,14 +236,33 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) // ensure jacobian inverse math is correct EXPECT_THAT(jacobian_inverse, MatrixNear(jacobian_inverse_est, 0.02)); +} + +TEST_F(TestPinocchioPlugin, Pinocchio_plugin_calculate_frame_difference) +{ + // compute the difference between two cartesian frames + Eigen::Matrix x_a, x_b; + x_a << 0, 1, 0, 0, 0, 0, 1; + x_b << 2, 3, 0, 0, 1, 0, 0; + double dt = 1.0; + Eigen::Matrix delta_x = Eigen::Matrix::Zero(); + Eigen::Matrix delta_x_est; + delta_x_est << 2, 2, 0, 0, 3.14, 0; + ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x)); + // ensure that difference math is correct + EXPECT_THAT(delta_x, MatrixNear(delta_x_est, 0.02)); +} + +TEST_F(TestPinocchioPlugin, Pinocchio_plugin_calculate_frame_difference_std_vector) +{ // compute the difference between two cartesian frames std::vector x_a(7), x_b(7); x_a = {0, 1, 0, 0, 0, 0, 1}; x_b = {2, 3, 0, 0, 1, 0, 0}; double dt = 1.0; - delta_x = {0, 0, 0, 0, 0, 0}; - delta_x_est = {2, 2, 0, 0, 3.14, 0}; + std::vector delta_x = {0, 0, 0, 0, 0, 0}; + std::vector delta_x_est = {2, 2, 0, 0, 3.14, 0}; ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x)); // ensure that difference math is correct @@ -271,7 +278,7 @@ TEST_F(TestPinocchioPlugin, incorrect_input_sizes) Eigen::Matrix pos = Eigen::Matrix::Zero(); Eigen::Isometry3d end_effector_transform; Eigen::Matrix delta_x = Eigen::Matrix::Zero(); - delta_x[2] = 1; + delta_x[2] = 1; // vz Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); Eigen::Matrix delta_x_est; Eigen::Matrix jacobian = Eigen::Matrix::Zero(); From c76a7dac92ad2e9e0637d1c11555027c6735bf81 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 20 Oct 2025 15:50:36 +0000 Subject: [PATCH 23/27] Temp: add failing test for base param --- .../test_kinematics_interface_pinocchio.cpp | 54 ++++++++++++++----- 1 file changed, 40 insertions(+), 14 deletions(-) diff --git a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp index f070ba0..0573bda 100644 --- a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp @@ -180,22 +180,48 @@ TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_reduced_model_tip) // ensure jacobian inverse math is correct EXPECT_THAT(jacobian_inverse, MatrixNear(jacobian_inverse_est, 0.02)); +} - // compute the difference between two cartesian frames - Eigen::Matrix x_a, x_b; - x_a << 0, 1, 0, 0, 0, 0, 1; - x_b << 2, 3, 0, 0, 1, 0, 0; - double dt = 1.0; - delta_x = Eigen::Matrix::Zero(); - delta_x_est << 2, 2, 0, 0, 3.14, 0; - ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x)); +TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_reduced_model_base) +{ + loadTipParameter("link3"); + loadBaseParameter("link1"); - // ensure that difference math is correct - EXPECT_THAT(delta_x, MatrixNear(delta_x_est, 0.02)); - for (Eigen::Index i = 0; i < delta_x.size(); ++i) - { - EXPECT_NEAR(delta_x(i), delta_x_est(i), 0.02) << " for index " << i; - } + // initialize the plugin + ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); + + // calculate end effector transform + Eigen::Matrix pos = Eigen::Matrix::Zero(); + Eigen::Isometry3d end_effector_transform; + ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); + + // convert cartesian delta to joint delta + Eigen::Matrix delta_x = Eigen::Matrix::Zero(); + delta_x[2] = 1; // vz + Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); + ASSERT_TRUE( + ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); + // jacobian inverse for vz is singular in this configuration + EXPECT_THAT(delta_theta, MatrixNear(Eigen::Matrix::Zero(), 0.02)); + + // convert joint delta to cartesian delta + Eigen::Matrix delta_x_est; + // joint deltas from zero should produce zero cartesian deltas + EXPECT_THAT(delta_x_est, MatrixNear(Eigen::Matrix::Zero(), 0.02)); + + // calculate jacobian + Eigen::Matrix jacobian = Eigen::Matrix::Zero(); + ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian)); + + // calculate jacobian inverse + Eigen::Matrix jacobian_inverse = + jacobian.completeOrthogonalDecomposition().pseudoInverse(); + Eigen::Matrix jacobian_inverse_est = + Eigen::Matrix::Zero(); + ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); + + // ensure jacobian inverse math is correct + EXPECT_THAT(jacobian_inverse, MatrixNear(jacobian_inverse_est, 0.02)); } TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) From 0d021a4b7089fa181b27d569c1181991a38d5f0f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 21 Oct 2025 08:17:07 +0000 Subject: [PATCH 24/27] Fix base frame locking --- .../kinematics_interface_common_tests.hpp | 1 + .../src/kinematics_interface_pinocchio.cpp | 77 ++-- .../test_kinematics_interface_pinocchio.cpp | 338 +----------------- 3 files changed, 56 insertions(+), 360 deletions(-) diff --git a/kinematics_interface/test/kinematics_interface_common_tests.hpp b/kinematics_interface/test/kinematics_interface_common_tests.hpp index fecc10c..e381a8c 100644 --- a/kinematics_interface/test/kinematics_interface_common_tests.hpp +++ b/kinematics_interface/test/kinematics_interface_common_tests.hpp @@ -40,6 +40,7 @@ class TestPlugin : public ::testing::Test std::shared_ptr ik_; std::shared_ptr node_; std::string end_effector_ = "link2"; + // base_link -> joint1 -> link1 -> joint2 -> link2 -> joint3 -> link3 std::string urdf_ = std::string(ros2_control_test_assets::urdf_head) + std::string(ros2_control_test_assets::urdf_tail); diff --git a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp index 38568a6..0553a46 100644 --- a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp @@ -86,47 +86,31 @@ bool KinematicsInterfacePinocchio::initialize( parameters_interface->get_parameter(ns + "base", base_param); root_name_ = base_param.as_string(); } - if (!root_name_.empty()) + + // load the model + try { - // TODO(anyone): fix root name usage - try - { - pinocchio::urdf::buildModelFromXML( - robot_description_local, /*root_name_,*/ full_model, verbose, true); - } - catch (const std::exception & e) - { - RCLCPP_ERROR( - LOGGER, "Error parsing URDF to build Pinocchio model from base '%s': %s", - root_name_.c_str(), e.what()); - return false; - } + pinocchio::urdf::buildModelFromXML(robot_description_local, full_model, verbose, true); } - else + catch (const std::exception & e) + { + RCLCPP_ERROR(LOGGER, "Error parsing URDF to build Pinocchio model: %s", e.what()); + return false; + } + if (root_name_.empty()) { - 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; - } root_name_ = full_model.names[full_model.frames[0].parent]; } - if (!full_model.existFrame(end_effector_name)) + if (!full_model.existFrame(root_name_) || !full_model.existFrame(end_effector_name)) { RCLCPP_ERROR( LOGGER, "failed to find chain from robot root '%s' to end effector '%s'", root_name_.c_str(), end_effector_name.c_str()); return false; } - // create reduced model by locking joints after end-effector - pinocchio::FrameIndex frame_id = full_model.getFrameId(end_effector_name); - const pinocchio::Frame & frame = full_model.frames[frame_id]; - pinocchio::JointIndex tool_joint_id = frame.parent; // the joint to which this frame is attached + + // create reduced model by locking joints auto const isChildOf = []( const pinocchio::Model & model, pinocchio::JointIndex ancestor, pinocchio::JointIndex descendant) @@ -139,18 +123,47 @@ bool KinematicsInterfacePinocchio::initialize( } return false; }; - std::vector locked_joints; + std::vector locked_joints_base, locked_joints_tip; + 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]; + pinocchio::JointIndex base_joint_id = + base_frame.parent; // the joint to which this frame is attached + + // Lock all ancestors of base_joint + pinocchio::JointIndex current = base_joint_id; + while (current > 0) + { + locked_joints_base.push_back(current); + current = full_model.parents[current]; + } + RCLCPP_INFO( + LOGGER, "Locked %zu joint(s) before root '%s'", locked_joints_base.size(), + root_name_.c_str()); + } + + 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 for (pinocchio::JointIndex jid = 1; jid < static_cast(full_model.njoints); ++jid) { + // Lock all joints that are descendants of tool frame if (isChildOf(full_model, tool_joint_id, jid) && jid != tool_joint_id) - locked_joints.push_back(jid); + { + locked_joints_tip.push_back(jid); + } } RCLCPP_INFO( - LOGGER, "Locked %zu joint(s) after tool frame %s", locked_joints.size(), + LOGGER, "Locked %zu joint(s) after tool frame '%s'", locked_joints_tip.size(), end_effector_name.c_str()); Eigen::VectorXd q_fixed = Eigen::VectorXd::Zero(full_model.nq); // actual value is not important for kinematics + + std::vector locked_joints = locked_joints_base; + locked_joints.insert(locked_joints.end(), locked_joints_tip.begin(), locked_joints_tip.end()); model_ = pinocchio::buildReducedModel(full_model, locked_joints, q_fixed); // allocate dynamics memory diff --git a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp index 0573bda..1265fa0 100644 --- a/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/test/test_kinematics_interface_pinocchio.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2024, Saif Sidhik. +// 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. @@ -12,341 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. // -/// \author: Saif Sidhik +/// \author: Christoph Froehlich #include -#include -#include "kinematics_interface/kinematics_interface.hpp" -#include "pluginlib/class_loader.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "ros2_control_test_assets/descriptions.hpp" -MATCHER_P2(MatrixNear, expected, tol, "Two matrices are approximately equal") -{ - return arg.isApprox(expected, tol); -} +#include "kinematics_interface_tests/kinematics_interface_common_tests.hpp" -class TestPinocchioPlugin : public ::testing::Test +struct PluginPinocchio { -public: - std::shared_ptr> ik_loader_; - std::shared_ptr ik_; - std::shared_ptr node_; - std::string end_effector_ = "link2"; - std::string urdf_ = std::string(ros2_control_test_assets::urdf_head) + - std::string(ros2_control_test_assets::urdf_tail); - - void SetUp() - { - // init ros - rclcpp::init(0, nullptr); - node_ = std::make_shared("test_node"); - std::string plugin_name = "kinematics_interface_pinocchio/KinematicsInterfacePinocchio"; - ik_loader_ = - std::make_shared>( - "kinematics_interface", "kinematics_interface::KinematicsInterface"); - ik_ = std::unique_ptr( - ik_loader_->createUnmanagedInstance(plugin_name)); - - node_->declare_parameter("verbose", true); - node_->declare_parameter("alpha", 0.005); - node_->declare_parameter("robot_description", urdf_); - node_->declare_parameter("tip", end_effector_); - node_->declare_parameter("base", std::string("")); - } - - void TearDown() - { - node_->shutdown(); - // shutdown ros - rclcpp::shutdown(); - } - - void loadURDFParameter(std::string urdf) + static std::string Name() { - rclcpp::Parameter param("robot_description", urdf); - node_->set_parameter(param); + return "kinematics_interface_pinocchio/KinematicsInterfacePinocchio"; } - - /** - * \brief Used for testing initialization from parameters. - * Elsewhere, default 0.005 is used. - */ - - void loadAlphaParameter(double alpha) - { - rclcpp::Parameter param("alpha", alpha); - node_->set_parameter(param); - } - - /** - * \brief Used for testing initialization from parameters. - * Elsewhere, `end_effector_` member is used. - */ - void loadTipParameter(std::string tip) + static void set_custom_node_parameters(rclcpp_lifecycle::LifecycleNode::SharedPtr node) { - rclcpp::Parameter param("tip", tip); - node_->set_parameter(param); - } - - /** - * \brief Used for testing initialization from parameters. - * Elsewhere, `""` is used. - */ - void loadBaseParameter(std::string base) - { - rclcpp::Parameter param("base", base); - node_->set_parameter(param); + node->declare_parameter("alpha", 0.005); } }; -TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function) -{ - loadTipParameter("link3"); - - // initialize the plugin - ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); - - // calculate end effector transform - Eigen::Matrix pos = Eigen::Matrix::Zero(); - Eigen::Isometry3d end_effector_transform; - ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); - - // convert cartesian delta to joint delta - Eigen::Matrix delta_x = Eigen::Matrix::Zero(); - delta_x[2] = 1; // vz - Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); - ASSERT_TRUE( - ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); - - // convert joint delta to cartesian delta - Eigen::Matrix delta_x_est; - ASSERT_TRUE( - ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); - - // Ensure kinematics math is correct - EXPECT_THAT(delta_x, MatrixNear(delta_x_est, 0.02)); - - // calculate jacobian - Eigen::Matrix jacobian = Eigen::Matrix::Zero(); - ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian)); - - // calculate jacobian inverse - Eigen::Matrix jacobian_inverse = - jacobian.completeOrthogonalDecomposition().pseudoInverse(); - Eigen::Matrix jacobian_inverse_est = - Eigen::Matrix::Zero(); - ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); - - // ensure jacobian inverse math is correct - EXPECT_THAT(jacobian_inverse, MatrixNear(jacobian_inverse_est, 0.02)); -} - -TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_reduced_model_tip) -{ - // initialize the plugin - ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); - - // calculate end effector transform - Eigen::Matrix pos = Eigen::Matrix::Zero(); - Eigen::Isometry3d end_effector_transform; - ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); - - // convert cartesian delta to joint delta - Eigen::Matrix delta_x = Eigen::Matrix::Zero(); - delta_x[2] = 1; // vz - Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); - ASSERT_TRUE( - ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); - - // convert joint delta to cartesian delta - Eigen::Matrix delta_x_est; - ASSERT_TRUE( - ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); - - // Ensure kinematics math is correct - EXPECT_THAT(delta_x, MatrixNear(delta_x_est, 0.02)); - - // calculate jacobian - Eigen::Matrix jacobian = Eigen::Matrix::Zero(); - ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian)); - - // calculate jacobian inverse - Eigen::Matrix jacobian_inverse = - jacobian.completeOrthogonalDecomposition().pseudoInverse(); - Eigen::Matrix jacobian_inverse_est = - Eigen::Matrix::Zero(); - ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); - - // ensure jacobian inverse math is correct - EXPECT_THAT(jacobian_inverse, MatrixNear(jacobian_inverse_est, 0.02)); -} - -TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_reduced_model_base) -{ - loadTipParameter("link3"); - loadBaseParameter("link1"); - - // initialize the plugin - ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); - - // calculate end effector transform - Eigen::Matrix pos = Eigen::Matrix::Zero(); - Eigen::Isometry3d end_effector_transform; - ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); - - // convert cartesian delta to joint delta - Eigen::Matrix delta_x = Eigen::Matrix::Zero(); - delta_x[2] = 1; // vz - Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); - ASSERT_TRUE( - ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); - // jacobian inverse for vz is singular in this configuration - EXPECT_THAT(delta_theta, MatrixNear(Eigen::Matrix::Zero(), 0.02)); - - // convert joint delta to cartesian delta - Eigen::Matrix delta_x_est; - // joint deltas from zero should produce zero cartesian deltas - EXPECT_THAT(delta_x_est, MatrixNear(Eigen::Matrix::Zero(), 0.02)); - - // calculate jacobian - Eigen::Matrix jacobian = Eigen::Matrix::Zero(); - ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian)); - - // calculate jacobian inverse - Eigen::Matrix jacobian_inverse = - jacobian.completeOrthogonalDecomposition().pseudoInverse(); - Eigen::Matrix jacobian_inverse_est = - Eigen::Matrix::Zero(); - ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); - - // ensure jacobian inverse math is correct - EXPECT_THAT(jacobian_inverse, MatrixNear(jacobian_inverse_est, 0.02)); -} - -TEST_F(TestPinocchioPlugin, Pinocchio_plugin_function_std_vector) -{ - // initialize the plugin - ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); - - // calculate end effector transform - std::vector pos = {0, 0}; - Eigen::Isometry3d end_effector_transform; - ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); - - // convert cartesian delta to joint delta - std::vector delta_x = {0, 0, 0, 0, 0, 0}; - delta_x[2] = 1; // vz - std::vector delta_theta = {0, 0}; - ASSERT_TRUE( - ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); - - // convert joint delta to cartesian delta - std::vector delta_x_est(6); - ASSERT_TRUE( - ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est)); - - // Ensure kinematics math is correct - EXPECT_THAT(delta_x, ::testing::Pointwise(::testing::DoubleNear(0.02), delta_x_est)); - - // calculate jacobian - Eigen::Matrix jacobian = Eigen::Matrix::Zero(); - ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian)); - - // calculate jacobian inverse - Eigen::Matrix jacobian_inverse = - jacobian.completeOrthogonalDecomposition().pseudoInverse(); - Eigen::Matrix jacobian_inverse_est = - Eigen::Matrix::Zero(); - ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); - - // ensure jacobian inverse math is correct - EXPECT_THAT(jacobian_inverse, MatrixNear(jacobian_inverse_est, 0.02)); -} - -TEST_F(TestPinocchioPlugin, Pinocchio_plugin_calculate_frame_difference) -{ - // compute the difference between two cartesian frames - Eigen::Matrix x_a, x_b; - x_a << 0, 1, 0, 0, 0, 0, 1; - x_b << 2, 3, 0, 0, 1, 0, 0; - double dt = 1.0; - Eigen::Matrix delta_x = Eigen::Matrix::Zero(); - Eigen::Matrix delta_x_est; - delta_x_est << 2, 2, 0, 0, 3.14, 0; - ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x)); - - // ensure that difference math is correct - EXPECT_THAT(delta_x, MatrixNear(delta_x_est, 0.02)); -} - -TEST_F(TestPinocchioPlugin, Pinocchio_plugin_calculate_frame_difference_std_vector) -{ - // compute the difference between two cartesian frames - std::vector x_a(7), x_b(7); - x_a = {0, 1, 0, 0, 0, 0, 1}; - x_b = {2, 3, 0, 0, 1, 0, 0}; - double dt = 1.0; - std::vector delta_x = {0, 0, 0, 0, 0, 0}; - std::vector delta_x_est = {2, 2, 0, 0, 3.14, 0}; - ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x)); - - // ensure that difference math is correct - EXPECT_THAT(delta_x, ::testing::Pointwise(::testing::DoubleNear(0.02), delta_x_est)); -} - -TEST_F(TestPinocchioPlugin, incorrect_input_sizes) -{ - // initialize the plugin - ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); - - // define correct values - Eigen::Matrix pos = Eigen::Matrix::Zero(); - Eigen::Isometry3d end_effector_transform; - Eigen::Matrix delta_x = Eigen::Matrix::Zero(); - delta_x[2] = 1; // vz - Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); - Eigen::Matrix delta_x_est; - Eigen::Matrix jacobian = Eigen::Matrix::Zero(); - - // wrong size input vector - Eigen::Matrix vec_5 = Eigen::Matrix::Zero(); - // wrong size input jacobian - Eigen::Matrix mat_5_6 = Eigen::Matrix::Zero(); - - // calculate transform - ASSERT_FALSE(ik_->calculate_link_transform(vec_5, end_effector_, end_effector_transform)); - ASSERT_FALSE(ik_->calculate_link_transform(pos, "link_not_in_model", end_effector_transform)); - - // convert cartesian delta to joint delta - ASSERT_FALSE( - ik_->convert_cartesian_deltas_to_joint_deltas(vec_5, delta_x, end_effector_, delta_theta)); - ASSERT_FALSE( - ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, "link_not_in_model", delta_theta)); - ASSERT_FALSE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, vec_5)); - - // convert joint delta to cartesian delta - ASSERT_FALSE( - ik_->convert_joint_deltas_to_cartesian_deltas(vec_5, delta_theta, end_effector_, delta_x_est)); - ASSERT_FALSE( - ik_->convert_joint_deltas_to_cartesian_deltas(pos, vec_5, end_effector_, delta_x_est)); - ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas( - pos, delta_theta, "link_not_in_model", delta_x_est)); - - // calculate jacobian inverse - ASSERT_FALSE(ik_->calculate_jacobian_inverse(vec_5, end_effector_, jacobian)); - ASSERT_FALSE(ik_->calculate_jacobian_inverse(pos, end_effector_, mat_5_6)); - ASSERT_FALSE(ik_->calculate_jacobian_inverse(pos, "link_not_in_model", jacobian)); -} - -TEST_F(TestPinocchioPlugin, Pinocchio_plugin_no_robot_description) -{ - loadURDFParameter(""); - ASSERT_FALSE(ik_->initialize("", node_->get_node_parameters_interface(), "")); -} - -TEST_F(TestPinocchioPlugin, Pinocchio_plugin_no_parameter_tip) -{ - loadTipParameter(""); - ASSERT_FALSE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); -} +using MyTypes = ::testing::Types; +INSTANTIATE_TYPED_TEST_SUITE_P(PluginTestPinocchio, TestPlugin, MyTypes); From a57ef5200fbfe62c8f5d491c4098b266258627e1 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 21 Oct 2025 19:14:48 +0000 Subject: [PATCH 25/27] Fix end_effector variable for tests --- .../test/kinematics_interface_common_tests.hpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/kinematics_interface/test/kinematics_interface_common_tests.hpp b/kinematics_interface/test/kinematics_interface_common_tests.hpp index e381a8c..b1bca76 100644 --- a/kinematics_interface/test/kinematics_interface_common_tests.hpp +++ b/kinematics_interface/test/kinematics_interface_common_tests.hpp @@ -108,6 +108,7 @@ TYPED_TEST_SUITE_P(TestPlugin); TYPED_TEST_P(TestPlugin, basic_plugin_function) { this->loadTipParameter("link3"); + this->end_effector_ = "link3"; // initialize the plugin ASSERT_TRUE(this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); @@ -193,8 +194,9 @@ TYPED_TEST_P(TestPlugin, plugin_function_reduced_model_tip) TYPED_TEST_P(TestPlugin, plugin_function_reduced_model_base) { - this->loadTipParameter("link3"); this->loadBaseParameter("link1"); + this->loadTipParameter("link3"); + this->end_effector_ = "link3"; // initialize the plugin ASSERT_TRUE(this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); @@ -211,15 +213,14 @@ TYPED_TEST_P(TestPlugin, plugin_function_reduced_model_base) Eigen::VectorXd delta_theta = Eigen::VectorXd::Zero(2); ASSERT_TRUE(this->ik_->convert_cartesian_deltas_to_joint_deltas( pos, delta_x, this->end_effector_, delta_theta)); - // jacobian inverse for vz is singular in this configuration - EXPECT_THAT(delta_theta, MatrixNear(Eigen::Vector2d::Zero(), 0.02)); // convert joint delta to cartesian delta kinematics_interface::Vector6d delta_x_est; ASSERT_TRUE(this->ik_->convert_joint_deltas_to_cartesian_deltas( pos, delta_theta, this->end_effector_, delta_x_est)); - // joint deltas from zero should produce zero cartesian deltas - EXPECT_THAT(delta_x_est, MatrixNear(kinematics_interface::Vector6d::Zero(), 0.02)); + + // Ensure kinematics math is correct + EXPECT_THAT(delta_x, MatrixNear(delta_x_est, 0.02)); // calculate jacobian Eigen::Matrix jacobian = Eigen::Matrix::Zero(); From 7d3fa0a73d4477922b9de632c8e361898545576b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 21 Oct 2025 19:24:18 +0000 Subject: [PATCH 26/27] Fix merge artifact --- kinematics_interface/test/kinematics_interface_common_tests.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/kinematics_interface/test/kinematics_interface_common_tests.hpp b/kinematics_interface/test/kinematics_interface_common_tests.hpp index 8ea70bf..c77857d 100644 --- a/kinematics_interface/test/kinematics_interface_common_tests.hpp +++ b/kinematics_interface/test/kinematics_interface_common_tests.hpp @@ -109,7 +109,6 @@ TYPED_TEST_SUITE_P(TestPlugin); TYPED_TEST_P(TestPlugin, basic_plugin_function) { this->loadTipParameter("link3"); - this->end_effector_ = "link3"; // initialize the plugin ASSERT_TRUE(this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); From caa38e17bda1a1c44d22f0692c159b27a81a244b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 22 Oct 2025 19:09:37 +0000 Subject: [PATCH 27/27] Make chain parsing more robust --- .../src/kinematics_interface_pinocchio.cpp | 140 +++++++++++++----- 1 file changed, 99 insertions(+), 41 deletions(-) diff --git a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp index 0553a46..569ce2e 100644 --- a/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp +++ b/kinematics_interface_pinocchio/src/kinematics_interface_pinocchio.cpp @@ -16,6 +16,9 @@ #include "kinematics_interface_pinocchio/kinematics_interface_pinocchio.hpp" +#include +#include + namespace kinematics_interface_pinocchio { rclcpp::Logger LOGGER = rclcpp::get_logger("kinematics_interface_pinocchio"); @@ -99,74 +102,129 @@ bool KinematicsInterfacePinocchio::initialize( } if (root_name_.empty()) { - root_name_ = full_model.names[full_model.frames[0].parent]; - } + // 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; + } + } - if (!full_model.existFrame(root_name_) || !full_model.existFrame(end_effector_name)) + // 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 chain from robot root '%s' to end effector '%s'", root_name_.c_str(), - end_effector_name.c_str()); + 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 isChildOf = []( - const pinocchio::Model & model, pinocchio::JointIndex ancestor, - pinocchio::JointIndex descendant) + auto const get_descendants = [](const pinocchio::Model & model, pinocchio::JointIndex root) + -> std::unordered_set { - pinocchio::JointIndex current = descendant; + 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) { - if (current == ancestor) return true; + chain.push_back(current); + if (current == base_joint) break; current = model.parents[current]; } - return false; + 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; }; - std::vector locked_joints_base, locked_joints_tip; + + 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]; - pinocchio::JointIndex base_joint_id = - base_frame.parent; // the joint to which this frame is attached - - // Lock all ancestors of base_joint - pinocchio::JointIndex current = base_joint_id; - while (current > 0) - { - locked_joints_base.push_back(current); - current = full_model.parents[current]; - } - RCLCPP_INFO( - LOGGER, "Locked %zu joint(s) before root '%s'", locked_joints_base.size(), - root_name_.c_str()); + 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) { - // Lock all joints that are descendants of tool frame - if (isChildOf(full_model, tool_joint_id, jid) && jid != tool_joint_id) + if (chain_set.find(jid) == chain_set.end()) { - locked_joints_tip.push_back(jid); + locked_joints.push_back(jid); } } - RCLCPP_INFO( - LOGGER, "Locked %zu joint(s) after tool frame '%s'", locked_joints_tip.size(), - end_effector_name.c_str()); Eigen::VectorXd q_fixed = Eigen::VectorXd::Zero(full_model.nq); // actual value is not important for kinematics - - std::vector locked_joints = locked_joints_base; - locked_joints.insert(locked_joints.end(), locked_joints_tip.begin(), locked_joints_tip.end()); model_ = pinocchio::buildReducedModel(full_model, locked_joints, q_fixed); - // allocate dynamics memory + // allocate dynamic memory data_ = std::make_shared(model_); num_joints_ = static_cast(model_.nq); q_.resize(num_joints_); @@ -188,7 +246,7 @@ bool KinematicsInterfacePinocchio::convert_joint_deltas_to_cartesian_deltas( !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || !verify_joint_vector(delta_theta)) { - RCLCPP_ERROR(LOGGER, "Input verification failed in convert_joint_deltas_to_cartesian_deltas"); + RCLCPP_ERROR(LOGGER, "Input verification failed in '%s'", FUNCTION_SIGNATURE); return false; } @@ -213,7 +271,7 @@ bool KinematicsInterfacePinocchio::convert_cartesian_deltas_to_joint_deltas( !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || !verify_joint_vector(delta_theta)) { - RCLCPP_ERROR(LOGGER, "Input verification failed in convert_cartesian_deltas_to_joint_deltas"); + RCLCPP_ERROR(LOGGER, "Input verification failed in '%s'", FUNCTION_SIGNATURE); return false; } @@ -237,7 +295,7 @@ bool KinematicsInterfacePinocchio::calculate_jacobian( !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || !verify_jacobian(jacobian)) { - RCLCPP_ERROR(LOGGER, "Input verification failed in calculate_jacobian"); + RCLCPP_ERROR(LOGGER, "Input verification failed in '%s'", FUNCTION_SIGNATURE); return false; } @@ -261,7 +319,7 @@ bool KinematicsInterfacePinocchio::calculate_jacobian_inverse( !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || !verify_jacobian_inverse(jacobian_inverse)) { - RCLCPP_ERROR(LOGGER, "Input verification failed in calculate_jacobian_inverse"); + RCLCPP_ERROR(LOGGER, "Input verification failed in '%s'", FUNCTION_SIGNATURE); return false; } @@ -328,7 +386,7 @@ bool KinematicsInterfacePinocchio::calculate_frame_difference( // verify inputs if (!verify_period(dt)) { - RCLCPP_ERROR(LOGGER, "Input verification failed in calculate_frame_difference"); + RCLCPP_ERROR(LOGGER, "Input verification failed in '%s'", FUNCTION_SIGNATURE); return false; }