Skip to content

Commit 629bca4

Browse files
authored
Add pinocchio plugin (backport #199) (#225)
1 parent a11188c commit 629bca4

File tree

9 files changed

+736
-6
lines changed

9 files changed

+736
-6
lines changed

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,5 +24,5 @@ jobs:
2424
uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-win-build.yml@master
2525
with:
2626
ros_distro: rolling
27-
pixi_dependencies: typeguard jinja2 boost compilers
27+
pixi_dependencies: typeguard jinja2 boost compilers octomap
2828
ninja_packages: rsl

README.md

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0)
44
[![codecov](https://codecov.io/gh/ros-controls/kinematics_interface/graph/badge.svg?token=NS73VKPG9V)](https://codecov.io/gh/ros-controls/kinematics_interface)
55

6-
This is a ROS 2 package for using C++ kinematics frameworks in the context of ROS 2 control. A kinematics interface is designed to allow ROS 2 controllers to control robots in Cartesian space. This package also contains a basic implementation of the interface using KDL.
6+
This is a ROS 2 package for using C++ kinematics frameworks in the context of ROS 2 control. A kinematics interface is designed to allow ROS 2 controllers to control robots in Cartesian space. This package also contains a basic implementation of the interface using KDL and a pinocchio-based implementation. Use `kinematics_interface_pinocchio` plugin when you need high-performance kinematics computations for complex robots, require efficient handling of large kinematic chains, or want to leverage Pinocchio's advanced features such as automatic differentiation and collision detection integration. The plugin supports damped least-squares Jacobian inversion for robust singularity handling.
77

88
## Contributing
99

@@ -14,10 +14,10 @@ If you are new to the project, please read the [contributing guide](https://cont
1414

1515
ROS2 Distro | Branch | Build status | Documentation | Package Build
1616
:---------: | :----: | :----------: | :-----------: | :---------------:
17-
**Rolling** | [`master`](https://github.com/ros-controls/kinematics_interface/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-binary-build.yml) <br> [![Rolling Semi-Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-semi-binary-build.yml) <br> [![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Rdev__kinematics_interface__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Rdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/rolling/p/kinematics_interface/) <br> [API kdl](http://docs.ros.org/en/rolling/p/kinematics_interface_kdl/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary/)
18-
**Kilted** | [`master`](https://github.com/ros-controls/kinematics_interface/tree/master) | see above <br> [![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Kdev__kinematics_interface__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Kdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/kilted/p/kinematics_interface/) <br> [API kdl](http://docs.ros.org/en/kilted/p/kinematics_interface_kdl/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Kbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Kbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary/)
19-
**Jazzy** | [`jazzy`](https://github.com/ros-controls/kinematics_interface/tree/jazzy) | [![Jazzy Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-binary-build.yml) <br> [![Jazzy Semi-Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-semi-binary-build.yml) <br> [![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Jdev__kinematics_interface__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Jdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/jazzy/p/kinematics_interface/) <br> [API kdl](http://docs.ros.org/en/jazzy/p/kinematics_interface_kdl/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary/)
20-
**Humble** | [`humble`](https://github.com/ros-controls/kinematics_interface/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-binary-build.yml/badge.svg?branch=humble)](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-binary-build.yml) <br> [![Humble Semi-Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=humble)](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-semi-binary-build.yml) <br> [![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Hdev__kinematics_interface__ubuntu_jammy_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Hdev__kinematics_interface__ubuntu_jammy_amd64/) | [API](http://docs.ros.org/en/humble/p/kinematics_interface/) <br> [API kdl](http://docs.ros.org/en/humble/p/kinematics_interface_kdl/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__kinematics_interface__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__kinematics_interface__ubuntu_jammy_amd64__binary/)
17+
**Rolling** | [`master`](https://github.com/ros-controls/kinematics_interface/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-binary-build.yml) <br> [![Rolling Semi-Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/kinematics_interface/actions/workflows/rolling-semi-binary-build.yml) <br> [![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Rdev__kinematics_interface__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Rdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/rolling/p/kinematics_interface/) <br> [API kdl](http://docs.ros.org/en/rolling/p/kinematics_interface_kdl/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary/) <br> [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__kinematics_interface_kdl__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__kinematics_interface_kdl__ubuntu_noble_amd64__binary/) <br> [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__kinematics_interface_pinocchio__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__kinematics_interface_pinocchio__ubuntu_noble_amd64__binary/)
18+
**Kilted** | [`master`](https://github.com/ros-controls/kinematics_interface/tree/master) | see above <br> [![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Kdev__kinematics_interface__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Kdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/kilted/p/kinematics_interface/) <br> [API kdl](http://docs.ros.org/en/kilted/p/kinematics_interface_kdl/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Kbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Kbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary/) <br> [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__kinematics_interface_kdl__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__kinematics_interface_kdl__ubuntu_noble_amd64__binary/) <br> [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__kinematics_interface_pinocchio__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__kinematics_interface_pinocchio__ubuntu_noble_amd64__binary/)
19+
**Jazzy** | [`jazzy`](https://github.com/ros-controls/kinematics_interface/tree/jazzy) | [![Jazzy Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-binary-build.yml) <br> [![Jazzy Semi-Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/kinematics_interface/actions/workflows/jazzy-semi-binary-build.yml) <br> [![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Jdev__kinematics_interface__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Jdev__kinematics_interface__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/jazzy/p/kinematics_interface/) <br> [API kdl](http://docs.ros.org/en/jazzy/p/kinematics_interface_kdl/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__kinematics_interface__ubuntu_noble_amd64__binary/) <br> [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__kinematics_interface_kdl__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__kinematics_interface_kdl__ubuntu_noble_amd64__binary/)
20+
**Humble** | [`humble`](https://github.com/ros-controls/kinematics_interface/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-binary-build.yml/badge.svg?branch=humble)](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-binary-build.yml) <br> [![Humble Semi-Binary Build](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=humble)](https://github.com/ros-controls/kinematics_interface/actions/workflows/humble-semi-binary-build.yml) <br> [![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Hdev__kinematics_interface__ubuntu_jammy_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Hdev__kinematics_interface__ubuntu_jammy_amd64/) | [API](http://docs.ros.org/en/humble/p/kinematics_interface/) <br> [API kdl](http://docs.ros.org/en/humble/p/kinematics_interface_kdl/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__kinematics_interface__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__kinematics_interface__ubuntu_jammy_amd64__binary/) <br> [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__kinematics_interface_kdl__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__kinematics_interface_kdl__ubuntu_jammy_amd64__binary/)
2121

2222
## Acknowledgements
2323

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2+
Changelog for package kinematics_interface_pinocchio
3+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4+
5+
0.0.2 (2025-06-28)
6+
------------------
7+
* fixes to adapt to upstream changes in kinematics_interface
8+
* Contributors: David V. Lu, Saif Sidhik
9+
10+
0.0.1 (2024-08-05)
11+
------------------
12+
* create a pinocchio kinematics plugin following ros2 kinematics_interface format
13+
* Contributors: Saif Sidhik
Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
cmake_minimum_required(VERSION 3.16)
2+
project(kinematics_interface_pinocchio LANGUAGES CXX)
3+
4+
find_package(ros2_control_cmake REQUIRED)
5+
set_compiler_options()
6+
export_windows_symbols()
7+
8+
set(THIS_PACKAGE_INCLUDE_DEPENDS
9+
kinematics_interface
10+
pluginlib
11+
eigen3_cmake_module
12+
pinocchio
13+
)
14+
15+
# find dependencies
16+
find_package(ament_cmake REQUIRED)
17+
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
18+
find_package(${Dependency} REQUIRED)
19+
endforeach()
20+
find_package(Eigen3 REQUIRED NO_MODULE)
21+
22+
add_library(
23+
kinematics_interface_pinocchio
24+
SHARED
25+
src/kinematics_interface_pinocchio.cpp
26+
)
27+
target_include_directories(kinematics_interface_pinocchio PUBLIC
28+
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
29+
$<INSTALL_INTERFACE:include/kinematics_interface_pinocchio>
30+
)
31+
target_compile_features(kinematics_interface_pinocchio PUBLIC cxx_std_17)
32+
target_link_libraries(kinematics_interface_pinocchio PUBLIC
33+
kinematics_interface::kinematics_interface
34+
pluginlib::pluginlib
35+
Eigen3::Eigen
36+
pinocchio::pinocchio
37+
)
38+
39+
pluginlib_export_plugin_description_file(kinematics_interface kinematics_interface_pinocchio.xml)
40+
41+
if(BUILD_TESTING)
42+
find_package(ament_cmake_gmock REQUIRED)
43+
find_package(ros2_control_test_assets REQUIRED)
44+
45+
ament_add_gmock(
46+
test_kinematics_interface_pinocchio
47+
test/test_kinematics_interface_pinocchio.cpp
48+
)
49+
target_link_libraries(test_kinematics_interface_pinocchio
50+
kinematics_interface_pinocchio
51+
ros2_control_test_assets::ros2_control_test_assets
52+
)
53+
endif()
54+
55+
install(
56+
DIRECTORY include/
57+
DESTINATION include/kinematics_interface_pinocchio
58+
)
59+
install(
60+
TARGETS kinematics_interface_pinocchio
61+
EXPORT export_kinematics_interface_pinocchio
62+
ARCHIVE DESTINATION lib
63+
LIBRARY DESTINATION lib
64+
RUNTIME DESTINATION bin
65+
)
66+
67+
ament_export_targets(export_kinematics_interface_pinocchio HAS_LIBRARY_TARGET)
68+
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
69+
ament_package()
Lines changed: 95 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,95 @@
1+
// Copyright (c) 2024, Saif Sidhik.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
//
15+
/// \author: Saif Sidhik
16+
/// \description: Pinocchio plugin for kinematics interface
17+
18+
#ifndef KINEMATICS_INTERFACE_PINOCCHIO__KINEMATICS_INTERFACE_PINOCCHIO_HPP_
19+
#define KINEMATICS_INTERFACE_PINOCCHIO__KINEMATICS_INTERFACE_PINOCCHIO_HPP_
20+
21+
#include <memory>
22+
#include <string>
23+
#include <unordered_map>
24+
#include <vector>
25+
26+
#include "eigen3/Eigen/Core"
27+
#include "eigen3/Eigen/LU"
28+
#include "kinematics_interface/kinematics_interface.hpp"
29+
#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
30+
31+
#include "pinocchio/algorithm/frames.hpp"
32+
#include "pinocchio/algorithm/geometry.hpp"
33+
#include "pinocchio/algorithm/jacobian.hpp"
34+
#include "pinocchio/algorithm/joint-configuration.hpp"
35+
#include "pinocchio/parsers/urdf.hpp"
36+
37+
namespace kinematics_interface_pinocchio
38+
{
39+
class KinematicsInterfacePinocchio : public kinematics_interface::KinematicsInterface
40+
{
41+
public:
42+
bool initialize(
43+
const std::string & robot_description,
44+
std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
45+
const std::string & param_namespace) override;
46+
47+
bool convert_cartesian_deltas_to_joint_deltas(
48+
const Eigen::VectorXd & joint_pos, const Eigen::Matrix<double, 6, 1> & delta_x,
49+
const std::string & link_name, Eigen::VectorXd & delta_theta) override;
50+
51+
bool convert_joint_deltas_to_cartesian_deltas(
52+
const Eigen::VectorXd & joint_pos, const Eigen::VectorXd & delta_theta,
53+
const std::string & link_name, Eigen::Matrix<double, 6, 1> & delta_x) override;
54+
55+
bool calculate_link_transform(
56+
const Eigen::VectorXd & joint_pos, const std::string & link_name,
57+
Eigen::Isometry3d & transform) override;
58+
59+
bool calculate_jacobian(
60+
const Eigen::VectorXd & joint_pos, const std::string & link_name,
61+
Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian) override;
62+
63+
bool calculate_jacobian_inverse(
64+
const Eigen::VectorXd & joint_pos, const std::string & link_name,
65+
Eigen::Matrix<double, Eigen::Dynamic, 6> & jacobian_inverse) override;
66+
67+
private:
68+
// verification methods
69+
bool verify_initialized();
70+
bool verify_link_name(const std::string & link_name);
71+
bool verify_joint_vector(const Eigen::VectorXd & joint_vector);
72+
bool verify_jacobian(const Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian);
73+
bool verify_jacobian_inverse(const Eigen::Matrix<double, Eigen::Dynamic, 6> & jacobian_inverse);
74+
bool verify_period(const double dt);
75+
76+
bool initialized = false;
77+
std::string root_name_;
78+
Eigen::Index num_joints_;
79+
80+
pinocchio::Model model_;
81+
std::shared_ptr<pinocchio::Data> data_;
82+
Eigen::VectorXd q_;
83+
Eigen::MatrixXd jacobian_;
84+
Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian_inverse_;
85+
Eigen::MatrixXd frame_tf_;
86+
87+
std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface_;
88+
std::unordered_map<std::string, int> link_name_map_;
89+
double alpha; // damping term for Jacobian inverse
90+
Eigen::MatrixXd I;
91+
};
92+
93+
} // namespace kinematics_interface_pinocchio
94+
95+
#endif // KINEMATICS_INTERFACE_PINOCCHIO__KINEMATICS_INTERFACE_PINOCCHIO_HPP_
Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
<library path="kinematics_interface_pinocchio">
2+
<class name="kinematics_interface_pinocchio/KinematicsInterfacePinocchio"
3+
type="kinematics_interface_pinocchio::KinematicsInterfacePinocchio"
4+
base_class_type="kinematics_interface::KinematicsInterface">
5+
<description>
6+
Pinocchio plugin for ros2_control kinematics interface
7+
</description>
8+
</class>
9+
</library>
Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>kinematics_interface_pinocchio</name>
5+
<version>0.0.2</version>
6+
<description>Pinocchio-based implementation of ros2_control kinematics interface</description>
7+
8+
<maintainer email="[email protected]">Bence Magyar</maintainer>
9+
<maintainer email="[email protected]">Denis Štogl</maintainer>
10+
<maintainer email="[email protected]">Christoph Froehlich</maintainer>
11+
<maintainer email="[email protected]">Sai Kishor Kothakota</maintainer>
12+
13+
<license>Apache License 2.0</license>
14+
15+
<url type="website">https://control.ros.org</url>
16+
<url type="bugtracker">https://github.com/ros-controls/ros2_controllers/issues</url>
17+
<url type="repository">https://github.com/ros-controls/ros2_controllers/</url>
18+
19+
<author email="[email protected]">Saif Sidhik</author>
20+
21+
<buildtool_depend>ament_cmake</buildtool_depend>
22+
<buildtool_depend>eigen3_cmake_module</buildtool_depend>
23+
24+
<build_depend>ros2_control_cmake</build_depend>
25+
26+
<depend>eigen</depend>
27+
<depend>kinematics_interface</depend>
28+
<depend>eigen3_cmake_module</depend>
29+
<depend>pinocchio</depend>
30+
<depend>pluginlib</depend>
31+
32+
<test_depend>ament_cmake_gmock</test_depend>
33+
<test_depend>ros2_control_test_assets</test_depend>
34+
35+
<export>
36+
<build_type>ament_cmake</build_type>
37+
</export>
38+
</package>

0 commit comments

Comments
 (0)