From 255fb098baef6bf737c175875972ad980f01161e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 21 Oct 2025 07:23:53 +0000 Subject: [PATCH 01/11] Type-Parameterized Tests --- .../kinematics_interface_common_tests.hpp | 395 ++++++++++++++++++ .../test/test_kinematics_interface_kdl.cpp | 354 +--------------- 2 files changed, 403 insertions(+), 346 deletions(-) create mode 100644 kinematics_interface_kdl/test/kinematics_interface_common_tests.hpp diff --git a/kinematics_interface_kdl/test/kinematics_interface_common_tests.hpp b/kinematics_interface_kdl/test/kinematics_interface_common_tests.hpp new file mode 100644 index 0000000..c868a95 --- /dev/null +++ b/kinematics_interface_kdl/test/kinematics_interface_common_tests.hpp @@ -0,0 +1,395 @@ +// 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, Christoph Froehlich + +#ifndef KINEMATICS_INTERFACE_COMMON_TESTS_HPP_ +#define KINEMATICS_INTERFACE_COMMON_TESTS_HPP_ + +#include +#include +#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); +} + +template +class TestPlugin : 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 = PluginUnderTest::Name(); + 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() + { + // shutdown ros + rclcpp::shutdown(); + } + + /** + * \brief Used for testing initialization from parameters. + * Elsewhere, `urdf_` member is used. + */ + + void loadURDFParameter(const std::string & urdf) + { + rclcpp::Parameter param("robot_description", urdf); + node_->set_parameter(param); + } + + /** + * \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(const std::string & tip) + { + rclcpp::Parameter param("tip", tip); + node_->set_parameter(param); + } + + /** + * \brief Used for testing initialization from parameters. + * Elsewhere, `""` is used. + */ + void loadBaseParameter(const std::string & base) + { + rclcpp::Parameter param("base", base); + node_->set_parameter(param); + } +}; + +TYPED_TEST_SUITE_P(TestPlugin); + +TYPED_TEST_P(TestPlugin, basic_plugin_function) +{ + this->loadTipParameter("link3"); + + // initialize the plugin + ASSERT_TRUE(this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); + + // calculate end effector transform + Eigen::VectorXd pos = Eigen::Vector3d::Zero(); + Eigen::Isometry3d end_effector_transform; + ASSERT_TRUE( + this->ik_->calculate_link_transform(pos, this->end_effector_, end_effector_transform)); + + // convert cartesian delta to joint delta + kinematics_interface::Vector6d delta_x = kinematics_interface::Vector6d::Zero(); + delta_x[2] = 1; // vz + Eigen::VectorXd delta_theta = Eigen::Vector3d::Zero(); + ASSERT_TRUE(this->ik_->convert_cartesian_deltas_to_joint_deltas( + pos, delta_x, this->end_effector_, delta_theta)); + + // 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)); + + // 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(this->ik_->calculate_jacobian(pos, this->end_effector_, jacobian)); + + // calculate jacobian inverse + Eigen::Matrix jacobian_inverse = + jacobian.completeOrthogonalDecomposition().pseudoInverse(); + Eigen::Matrix jacobian_inverse_est = + Eigen::Matrix::Zero(); + ASSERT_TRUE( + this->ik_->calculate_jacobian_inverse(pos, this->end_effector_, jacobian_inverse_est)); + + // ensure jacobian inverse math is correct + EXPECT_THAT(jacobian_inverse, MatrixNear(jacobian_inverse_est, 0.02)); +} + +TYPED_TEST_P(TestPlugin, plugin_function_reduced_model_tip) +{ + // initialize the plugin + ASSERT_TRUE(this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); + + // calculate end effector transform + Eigen::Matrix pos = Eigen::Vector2d::Zero(); + Eigen::Isometry3d end_effector_transform; + ASSERT_TRUE( + this->ik_->calculate_link_transform(pos, this->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::Vector2d::Zero(); + ASSERT_TRUE(this->ik_->convert_cartesian_deltas_to_joint_deltas( + pos, delta_x, this->end_effector_, delta_theta)); + + // convert joint delta to cartesian delta + Eigen::Matrix delta_x_est; + ASSERT_TRUE(this->ik_->convert_joint_deltas_to_cartesian_deltas( + pos, delta_theta, this->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(this->ik_->calculate_jacobian(pos, this->end_effector_, jacobian)); + + // calculate jacobian inverse + Eigen::Matrix jacobian_inverse = + jacobian.completeOrthogonalDecomposition().pseudoInverse(); + Eigen::Matrix jacobian_inverse_est = + Eigen::Matrix::Zero(); + ASSERT_TRUE( + this->ik_->calculate_jacobian_inverse(pos, this->end_effector_, jacobian_inverse_est)); + + // ensure jacobian inverse math is correct + EXPECT_THAT(jacobian_inverse, MatrixNear(jacobian_inverse_est, 0.02)); +} + +TYPED_TEST_P(TestPlugin, plugin_function_reduced_model_base) +{ + this->loadTipParameter("link3"); + this->loadBaseParameter("link1"); + + // initialize the plugin + ASSERT_TRUE(this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); + + // calculate end effector transform + Eigen::VectorXd pos = Eigen::Vector2d::Zero(); + Eigen::Isometry3d end_effector_transform; + ASSERT_TRUE( + this->ik_->calculate_link_transform(pos, this->end_effector_, end_effector_transform)); + + // convert cartesian delta to joint delta + kinematics_interface::Vector6d delta_x = kinematics_interface::Vector6d::Zero(); + delta_x[2] = 1; // vz + Eigen::VectorXd delta_theta = Eigen::Vector2d::Zero(); + 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)); + + // calculate jacobian + Eigen::Matrix jacobian = Eigen::Matrix::Zero(); + ASSERT_TRUE(this->ik_->calculate_jacobian(pos, this->end_effector_, jacobian)); + + // calculate jacobian inverse + Eigen::Matrix jacobian_inverse = + jacobian.completeOrthogonalDecomposition().pseudoInverse(); + Eigen::Matrix jacobian_inverse_est = + Eigen::Matrix::Zero(); + ASSERT_TRUE( + this->ik_->calculate_jacobian_inverse(pos, this->end_effector_, jacobian_inverse_est)); + + // ensure jacobian inverse math is correct + EXPECT_THAT(jacobian_inverse, MatrixNear(jacobian_inverse_est, 0.02)); +} + +TYPED_TEST_P(TestPlugin, plugin_function_std_vector) +{ + // initialize the plugin + ASSERT_TRUE(this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); + + // calculate end effector transform + std::vector pos = {0, 0}; + Eigen::Isometry3d end_effector_transform; + ASSERT_TRUE( + this->ik_->calculate_link_transform(pos, this->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(this->ik_->convert_cartesian_deltas_to_joint_deltas( + pos, delta_x, this->end_effector_, delta_theta)); + + // convert joint delta to cartesian delta + std::vector delta_x_est(6); + ASSERT_TRUE(this->ik_->convert_joint_deltas_to_cartesian_deltas( + pos, delta_theta, this->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(this->ik_->calculate_jacobian(pos, this->end_effector_, jacobian)); + + // calculate jacobian inverse + Eigen::Matrix jacobian_inverse = + jacobian.completeOrthogonalDecomposition().pseudoInverse(); + Eigen::Matrix jacobian_inverse_est = + Eigen::Matrix::Zero(); + ASSERT_TRUE( + this->ik_->calculate_jacobian_inverse(pos, this->end_effector_, jacobian_inverse_est)); + + // ensure jacobian inverse math is correct + EXPECT_THAT(jacobian_inverse, MatrixNear(jacobian_inverse_est, 0.02)); +} + +TYPED_TEST_P(TestPlugin, 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; + kinematics_interface::Vector6d delta_x = kinematics_interface::Vector6d::Zero(); + kinematics_interface::Vector6d delta_x_est; + delta_x_est << 2, 2, 0, 0, 3.14, 0; + ASSERT_TRUE(this->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)); +} + +TYPED_TEST_P(TestPlugin, 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(this->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)); +} + +TYPED_TEST_P(TestPlugin, incorrect_input_sizes) +{ + // initialize the plugin + ASSERT_TRUE(this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); + + // define correct values + Eigen::Matrix pos = Eigen::Vector2d::Zero(); + Eigen::Isometry3d end_effector_transform; + Eigen::Matrix delta_x = Eigen::Matrix::Zero(); + delta_x[2] = 1; + Eigen::Matrix delta_theta = Eigen::Vector2d::Zero(); + Eigen::Matrix delta_x_est; + Eigen::Matrix jacobian = Eigen::Matrix::Zero(); + Eigen::Matrix x_a, x_b; + + // wrong size input vector + Eigen::Matrix vec_5 = Eigen::Matrix::Zero(); + + // wrong size input jacobian + Eigen::Matrix mat_5_6 = Eigen::Matrix::Zero(); + + // wrong value for period + double dt = -0.1; + + // calculate transform + ASSERT_FALSE( + this->ik_->calculate_link_transform(vec_5, this->end_effector_, end_effector_transform)); + ASSERT_FALSE( + this->ik_->calculate_link_transform(pos, "link_not_in_model", end_effector_transform)); + + // convert cartesian delta to joint delta + ASSERT_FALSE(this->ik_->convert_cartesian_deltas_to_joint_deltas( + vec_5, delta_x, this->end_effector_, delta_theta)); + ASSERT_FALSE(this->ik_->convert_cartesian_deltas_to_joint_deltas( + pos, delta_x, "link_not_in_model", delta_theta)); + ASSERT_FALSE( + this->ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, this->end_effector_, vec_5)); + + // convert joint delta to cartesian delta + ASSERT_FALSE(this->ik_->convert_joint_deltas_to_cartesian_deltas( + vec_5, delta_theta, this->end_effector_, delta_x_est)); + ASSERT_FALSE(this->ik_->convert_joint_deltas_to_cartesian_deltas( + pos, vec_5, this->end_effector_, delta_x_est)); + ASSERT_FALSE(this->ik_->convert_joint_deltas_to_cartesian_deltas( + pos, delta_theta, "link_not_in_model", delta_x_est)); + + // calculate jacobian inverse + ASSERT_FALSE(this->ik_->calculate_jacobian_inverse(vec_5, this->end_effector_, jacobian)); + ASSERT_FALSE(this->ik_->calculate_jacobian_inverse(pos, this->end_effector_, mat_5_6)); + ASSERT_FALSE(this->ik_->calculate_jacobian_inverse(pos, "link_not_in_model", jacobian)); + + // compute the difference between two cartesian frames + ASSERT_FALSE(this->ik_->calculate_frame_difference(x_a, x_b, dt, delta_x)); +} + +TYPED_TEST_P(TestPlugin, plugin_no_robot_description) +{ + this->loadURDFParameter(""); + ASSERT_FALSE(this->ik_->initialize("", this->node_->get_node_parameters_interface(), "")); +} + +TYPED_TEST_P(TestPlugin, plugin_no_parameter_tip) +{ + this->loadTipParameter(""); + ASSERT_FALSE( + this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); +} + +REGISTER_TYPED_TEST_SUITE_P( + TestPlugin, basic_plugin_function, plugin_function_reduced_model_tip, + plugin_function_reduced_model_base, plugin_function_std_vector, plugin_calculate_frame_difference, + plugin_calculate_frame_difference_std_vector, incorrect_input_sizes, plugin_no_robot_description, + plugin_no_parameter_tip); + +#endif // KINEMATICS_INTERFACE_COMMON_TESTS_HPP_ diff --git a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp index 778bc92..90066e9 100644 --- a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, PickNik, Inc. +// 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,354 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. // -/// \author: Paul Gesel +/// \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); -} -class TestKDLPlugin : 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_kdl/KinematicsInterfaceKDL"; - 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() - { - // shutdown ros - rclcpp::shutdown(); - } - - /** - * \brief Used for testing initialization from parameters. - * Elsewhere, `urdf_` member is used. - */ - - void loadURDFParameter(const std::string & urdf) - { - rclcpp::Parameter param("robot_description", urdf); - node_->set_parameter(param); - } - - /** - * \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(const std::string & tip) - { - rclcpp::Parameter param("tip", tip); - node_->set_parameter(param); - } - - /** - * \brief Used for testing initialization from parameters. - * Elsewhere, `""` is used. - */ - void loadBaseParameter(const std::string & base) - { - rclcpp::Parameter param("base", base); - node_->set_parameter(param); - } -}; - -TEST_F(TestKDLPlugin, KDL_plugin_function) -{ - loadTipParameter("link3"); - - // initialize the plugin - ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); - - // calculate end effector transform - auto pos = Eigen::VectorXd::Zero(3); - Eigen::Isometry3d end_effector_transform; - ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); - - // convert cartesian delta to joint delta - kinematics_interface::Vector6d delta_x = kinematics_interface::Vector6d::Zero(); - delta_x[2] = 1; // vz - Eigen::VectorXd delta_theta = Eigen::VectorXd::Zero(3); - ASSERT_TRUE( - ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); - - // convert joint delta to cartesian delta - kinematics_interface::Vector6d 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(TestKDLPlugin, KDL_plugin_function_reduced_model_tip) -{ - // initialize the plugin - ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); - - // calculate end effector transform - auto pos = Eigen::VectorXd::Zero(2); - Eigen::Isometry3d end_effector_transform; - ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); - - // convert cartesian delta to joint delta - kinematics_interface::Vector6d delta_x = kinematics_interface::Vector6d::Zero(); - delta_x[2] = 1; // vz - Eigen::VectorXd delta_theta = Eigen::VectorXd::Zero(2); - ASSERT_TRUE( - ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta)); - - // convert joint delta to cartesian delta - kinematics_interface::Vector6d 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(TestKDLPlugin, KDL_plugin_function_reduced_model_base) -{ - loadTipParameter("link3"); - loadBaseParameter("link1"); - - // initialize the plugin - ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); +#include "kinematics_interface_common_tests.hpp" - // calculate end effector transform - auto pos = Eigen::VectorXd::Zero(2); - Eigen::Isometry3d end_effector_transform; - ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector_, end_effector_transform)); - - // convert cartesian delta to joint delta - kinematics_interface::Vector6d delta_x = kinematics_interface::Vector6d::Zero(); - delta_x[2] = 1; // vz - Eigen::VectorXd delta_theta = Eigen::VectorXd::Zero(2); - 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::Vector2d::Zero(), 0.02)); - - // convert joint delta to cartesian delta - kinematics_interface::Vector6d delta_x_est; - ASSERT_TRUE( - ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, 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)); - - // 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(TestKDLPlugin, KDL_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; - 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(TestKDLPlugin, KDL_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; - kinematics_interface::Vector6d delta_x = kinematics_interface::Vector6d::Zero(); - kinematics_interface::Vector6d 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(TestKDLPlugin, KDL_plugin_calculate_frame_difference_std_vector) +struct PluginKDL { - // 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(TestKDLPlugin, incorrect_input_sizes) -{ - // initialize the plugin - ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); - - // define correct values - auto pos = Eigen::VectorXd::Zero(2); - Eigen::Isometry3d end_effector_transform; - kinematics_interface::Vector6d delta_x = kinematics_interface::Vector6d::Zero(); - delta_x[2] = 1; - Eigen::VectorXd delta_theta = Eigen::VectorXd::Zero(2); - kinematics_interface::Vector6d delta_x_est; - Eigen::Matrix jacobian = Eigen::Matrix::Zero(); - Eigen::Matrix x_a, x_b; - - // wrong size input vector - Eigen::VectorXd vec_5 = Eigen::VectorXd::Zero(5); - - // wrong size input jacobian - Eigen::Matrix mat_5_6 = Eigen::Matrix::Zero(); - - // wrong value for period - double dt = -0.1; - - // 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)); - - // compute the difference between two cartesian frames - ASSERT_FALSE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x)); -} - -TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description) -{ - loadURDFParameter(""); - ASSERT_FALSE(ik_->initialize("", node_->get_node_parameters_interface(), "")); -} + static std::string Name() { return "kinematics_interface_kdl/KinematicsInterfaceKDL"; } +}; -TEST_F(TestKDLPlugin, KDL_plugin_no_parameter_tip) -{ - loadTipParameter(""); - ASSERT_FALSE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); -} +using MyTypes = ::testing::Types; +INSTANTIATE_TYPED_TEST_SUITE_P(PluginTestKDL, TestPlugin, MyTypes); From f021e31a3526e26398c5049faef6f0c8698a2686 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 21 Oct 2025 07:23:53 +0000 Subject: [PATCH 02/11] Explicitely call shutdown --- .../test/kinematics_interface_common_tests.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/kinematics_interface_kdl/test/kinematics_interface_common_tests.hpp b/kinematics_interface_kdl/test/kinematics_interface_common_tests.hpp index c868a95..ce47e24 100644 --- a/kinematics_interface_kdl/test/kinematics_interface_common_tests.hpp +++ b/kinematics_interface_kdl/test/kinematics_interface_common_tests.hpp @@ -64,6 +64,7 @@ class TestPlugin : public ::testing::Test void TearDown() { + node_->shutdown(); // shutdown ros rclcpp::shutdown(); } From aa8f635777c1e9fcbd9b05c39ce256f07f18dc03 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 21 Oct 2025 07:23:53 +0000 Subject: [PATCH 03/11] Move to base class --- .../kinematics_interface_common_tests.hpp | 6 +++--- .../test/test_kinematics_interface_kdl.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) rename {kinematics_interface_kdl/test => kinematics_interface/include/kinematics_interface}/kinematics_interface_common_tests.hpp (98%) diff --git a/kinematics_interface_kdl/test/kinematics_interface_common_tests.hpp b/kinematics_interface/include/kinematics_interface/kinematics_interface_common_tests.hpp similarity index 98% rename from kinematics_interface_kdl/test/kinematics_interface_common_tests.hpp rename to kinematics_interface/include/kinematics_interface/kinematics_interface_common_tests.hpp index ce47e24..ad07b0a 100644 --- a/kinematics_interface_kdl/test/kinematics_interface_common_tests.hpp +++ b/kinematics_interface/include/kinematics_interface/kinematics_interface_common_tests.hpp @@ -14,8 +14,8 @@ // /// \author: Paul Gesel, Christoph Froehlich -#ifndef KINEMATICS_INTERFACE_COMMON_TESTS_HPP_ -#define KINEMATICS_INTERFACE_COMMON_TESTS_HPP_ +#ifndef KINEMATICS_INTERFACE__KINEMATICS_INTERFACE_COMMON_TESTS_HPP_ +#define KINEMATICS_INTERFACE__KINEMATICS_INTERFACE_COMMON_TESTS_HPP_ #include #include @@ -393,4 +393,4 @@ REGISTER_TYPED_TEST_SUITE_P( plugin_calculate_frame_difference_std_vector, incorrect_input_sizes, plugin_no_robot_description, plugin_no_parameter_tip); -#endif // KINEMATICS_INTERFACE_COMMON_TESTS_HPP_ +#endif // KINEMATICS_INTERFACE__KINEMATICS_INTERFACE_COMMON_TESTS_HPP_ diff --git a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp index 90066e9..691c4f5 100644 --- a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp @@ -16,7 +16,7 @@ #include -#include "kinematics_interface_common_tests.hpp" +#include "kinematics_interface/kinematics_interface_common_tests.hpp" struct PluginKDL { From 2f7470a1a7d307846532a26fe9035b63a620f870 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 21 Oct 2025 07:23:53 +0000 Subject: [PATCH 04/11] Only install it in case of BUILD_TESTING --- kinematics_interface/CMakeLists.txt | 11 +++++++++++ kinematics_interface/package.xml | 3 +++ .../kinematics_interface_common_tests.hpp | 6 +++--- kinematics_interface_kdl/CMakeLists.txt | 2 +- kinematics_interface_kdl/package.xml | 2 -- 5 files changed, 18 insertions(+), 6 deletions(-) rename kinematics_interface/{include/kinematics_interface => test}/kinematics_interface_common_tests.hpp (98%) diff --git a/kinematics_interface/CMakeLists.txt b/kinematics_interface/CMakeLists.txt index a89f256..165cca7 100644 --- a/kinematics_interface/CMakeLists.txt +++ b/kinematics_interface/CMakeLists.txt @@ -41,6 +41,17 @@ install( RUNTIME DESTINATION bin ) +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + install( + DIRECTORY test/ + DESTINATION include/kinematics_interface/kinematics_interface + ) + ament_export_dependencies(ament_cmake_gmock ros2_control_test_assets) +endif() + ament_export_targets(export_kinematics_interface HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} Eigen3) ament_package() diff --git a/kinematics_interface/package.xml b/kinematics_interface/package.xml index e96899d..1b0a797 100644 --- a/kinematics_interface/package.xml +++ b/kinematics_interface/package.xml @@ -26,6 +26,9 @@ eigen rclcpp_lifecycle + ament_cmake_gmock + ros2_control_test_assets + ament_cmake diff --git a/kinematics_interface/include/kinematics_interface/kinematics_interface_common_tests.hpp b/kinematics_interface/test/kinematics_interface_common_tests.hpp similarity index 98% rename from kinematics_interface/include/kinematics_interface/kinematics_interface_common_tests.hpp rename to kinematics_interface/test/kinematics_interface_common_tests.hpp index ad07b0a..ce47e24 100644 --- a/kinematics_interface/include/kinematics_interface/kinematics_interface_common_tests.hpp +++ b/kinematics_interface/test/kinematics_interface_common_tests.hpp @@ -14,8 +14,8 @@ // /// \author: Paul Gesel, Christoph Froehlich -#ifndef KINEMATICS_INTERFACE__KINEMATICS_INTERFACE_COMMON_TESTS_HPP_ -#define KINEMATICS_INTERFACE__KINEMATICS_INTERFACE_COMMON_TESTS_HPP_ +#ifndef KINEMATICS_INTERFACE_COMMON_TESTS_HPP_ +#define KINEMATICS_INTERFACE_COMMON_TESTS_HPP_ #include #include @@ -393,4 +393,4 @@ REGISTER_TYPED_TEST_SUITE_P( plugin_calculate_frame_difference_std_vector, incorrect_input_sizes, plugin_no_robot_description, plugin_no_parameter_tip); -#endif // KINEMATICS_INTERFACE__KINEMATICS_INTERFACE_COMMON_TESTS_HPP_ +#endif // KINEMATICS_INTERFACE_COMMON_TESTS_HPP_ diff --git a/kinematics_interface_kdl/CMakeLists.txt b/kinematics_interface_kdl/CMakeLists.txt index d8322f5..7366008 100644 --- a/kinematics_interface_kdl/CMakeLists.txt +++ b/kinematics_interface_kdl/CMakeLists.txt @@ -38,12 +38,12 @@ pluginlib_export_plugin_description_file(kinematics_interface kinematics_interfa if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - find_package(ros2_control_test_assets REQUIRED) ament_add_gmock( test_kinematics_interface_kdl test/test_kinematics_interface_kdl.cpp ) + # TODO(christophfroehlich): Remove dependency on ros2_control_test_assets target_link_libraries(test_kinematics_interface_kdl kinematics_interface_kdl ros2_control_test_assets::ros2_control_test_assets) endif() diff --git a/kinematics_interface_kdl/package.xml b/kinematics_interface_kdl/package.xml index 8d538ae..ad6d5df 100644 --- a/kinematics_interface_kdl/package.xml +++ b/kinematics_interface_kdl/package.xml @@ -31,8 +31,6 @@ tf2_eigen_kdl ament_cmake_gmock - ros2_control_test_assets - ament_cmake From 0da20404e2316731e57103f90ba32987a775f9bb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 21 Oct 2025 07:23:53 +0000 Subject: [PATCH 05/11] Export as INTERFACE library --- kinematics_interface/CMakeLists.txt | 36 +++++++++++++++++++------ kinematics_interface/package.xml | 1 + kinematics_interface_kdl/CMakeLists.txt | 3 +-- 3 files changed, 30 insertions(+), 10 deletions(-) diff --git a/kinematics_interface/CMakeLists.txt b/kinematics_interface/CMakeLists.txt index 165cca7..11263de 100644 --- a/kinematics_interface/CMakeLists.txt +++ b/kinematics_interface/CMakeLists.txt @@ -33,23 +33,43 @@ install( DIRECTORY include/ DESTINATION include/kinematics_interface ) -install( - TARGETS kinematics_interface - EXPORT export_kinematics_interface - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) + find_package(pluginlib REQUIRED) + + add_library(kinematics_interface_tests INTERFACE) + target_include_directories(kinematics_interface_tests INTERFACE + $ + $ + ) + target_link_libraries(kinematics_interface_tests INTERFACE + kinematics_interface + ros2_control_test_assets::ros2_control_test_assets + pluginlib::pluginlib + ) install( DIRECTORY test/ DESTINATION include/kinematics_interface/kinematics_interface ) - ament_export_dependencies(ament_cmake_gmock ros2_control_test_assets) + ament_export_dependencies(ros2_control_test_assets pluginlib) +install( + TARGETS kinematics_interface kinematics_interface_tests + EXPORT export_kinematics_interface + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +else() +install( + TARGETS kinematics_interface + EXPORT export_kinematics_interface + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) endif() ament_export_targets(export_kinematics_interface HAS_LIBRARY_TARGET) diff --git a/kinematics_interface/package.xml b/kinematics_interface/package.xml index 1b0a797..8bf692f 100644 --- a/kinematics_interface/package.xml +++ b/kinematics_interface/package.xml @@ -27,6 +27,7 @@ rclcpp_lifecycle ament_cmake_gmock + pluginlib ros2_control_test_assets diff --git a/kinematics_interface_kdl/CMakeLists.txt b/kinematics_interface_kdl/CMakeLists.txt index 7366008..abc486e 100644 --- a/kinematics_interface_kdl/CMakeLists.txt +++ b/kinematics_interface_kdl/CMakeLists.txt @@ -43,8 +43,7 @@ if(BUILD_TESTING) test_kinematics_interface_kdl test/test_kinematics_interface_kdl.cpp ) - # TODO(christophfroehlich): Remove dependency on ros2_control_test_assets - target_link_libraries(test_kinematics_interface_kdl kinematics_interface_kdl ros2_control_test_assets::ros2_control_test_assets) + target_link_libraries(test_kinematics_interface_kdl kinematics_interface::kinematics_interface_tests) endif() install( From 0bd94a694deee2891e618cf21cb9899322380b9e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 21 Oct 2025 07:23:53 +0000 Subject: [PATCH 06/11] Install to different subfolder --- kinematics_interface/CMakeLists.txt | 2 +- kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/kinematics_interface/CMakeLists.txt b/kinematics_interface/CMakeLists.txt index 11263de..5675cae 100644 --- a/kinematics_interface/CMakeLists.txt +++ b/kinematics_interface/CMakeLists.txt @@ -52,7 +52,7 @@ if(BUILD_TESTING) install( DIRECTORY test/ - DESTINATION include/kinematics_interface/kinematics_interface + DESTINATION include/kinematics_interface/kinematics_interface_tests ) ament_export_dependencies(ros2_control_test_assets pluginlib) install( diff --git a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp index 691c4f5..105e2cd 100644 --- a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp @@ -16,7 +16,7 @@ #include -#include "kinematics_interface/kinematics_interface_common_tests.hpp" +#include "kinematics_interface_tests/kinematics_interface_common_tests.hpp" struct PluginKDL { From bcaf5ab6178dd41bf2212b58861ad6aa79eb9f36 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 21 Oct 2025 07:23:53 +0000 Subject: [PATCH 07/11] Update Eigen Vector sizes --- .../kinematics_interface_common_tests.hpp | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/kinematics_interface/test/kinematics_interface_common_tests.hpp b/kinematics_interface/test/kinematics_interface_common_tests.hpp index ce47e24..0b55c61 100644 --- a/kinematics_interface/test/kinematics_interface_common_tests.hpp +++ b/kinematics_interface/test/kinematics_interface_common_tests.hpp @@ -122,7 +122,7 @@ TYPED_TEST_P(TestPlugin, basic_plugin_function) ASSERT_TRUE(this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); // calculate end effector transform - Eigen::VectorXd pos = Eigen::Vector3d::Zero(); + auto pos = Eigen::VectorXd::Zero(3); Eigen::Isometry3d end_effector_transform; ASSERT_TRUE( this->ik_->calculate_link_transform(pos, this->end_effector_, end_effector_transform)); @@ -130,7 +130,7 @@ TYPED_TEST_P(TestPlugin, basic_plugin_function) // convert cartesian delta to joint delta kinematics_interface::Vector6d delta_x = kinematics_interface::Vector6d::Zero(); delta_x[2] = 1; // vz - Eigen::VectorXd delta_theta = Eigen::Vector3d::Zero(); + Eigen::VectorXd delta_theta = Eigen::VectorXd::Zero(3); ASSERT_TRUE(this->ik_->convert_cartesian_deltas_to_joint_deltas( pos, delta_x, this->end_effector_, delta_theta)); @@ -164,20 +164,20 @@ TYPED_TEST_P(TestPlugin, plugin_function_reduced_model_tip) ASSERT_TRUE(this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); // calculate end effector transform - Eigen::Matrix pos = Eigen::Vector2d::Zero(); + auto pos = Eigen::VectorXd::Zero(2); Eigen::Isometry3d end_effector_transform; ASSERT_TRUE( this->ik_->calculate_link_transform(pos, this->end_effector_, end_effector_transform)); // convert cartesian delta to joint delta - Eigen::Matrix delta_x = Eigen::Matrix::Zero(); + kinematics_interface::Vector6d delta_x = kinematics_interface::Vector6d::Zero(); delta_x[2] = 1; // vz - Eigen::Matrix delta_theta = Eigen::Vector2d::Zero(); + 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)); // convert joint delta to cartesian delta - Eigen::Matrix delta_x_est; + 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)); @@ -209,7 +209,7 @@ TYPED_TEST_P(TestPlugin, plugin_function_reduced_model_base) ASSERT_TRUE(this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); // calculate end effector transform - Eigen::VectorXd pos = Eigen::Vector2d::Zero(); + auto pos = Eigen::VectorXd::Zero(2); Eigen::Isometry3d end_effector_transform; ASSERT_TRUE( this->ik_->calculate_link_transform(pos, this->end_effector_, end_effector_transform)); @@ -217,7 +217,7 @@ TYPED_TEST_P(TestPlugin, plugin_function_reduced_model_base) // convert cartesian delta to joint delta kinematics_interface::Vector6d delta_x = kinematics_interface::Vector6d::Zero(); delta_x[2] = 1; // vz - Eigen::VectorXd delta_theta = Eigen::Vector2d::Zero(); + 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 @@ -325,17 +325,17 @@ TYPED_TEST_P(TestPlugin, incorrect_input_sizes) ASSERT_TRUE(this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); // define correct values - Eigen::Matrix pos = Eigen::Vector2d::Zero(); + auto pos = Eigen::VectorXd::Zero(2); Eigen::Isometry3d end_effector_transform; - Eigen::Matrix delta_x = Eigen::Matrix::Zero(); + kinematics_interface::Vector6d delta_x = kinematics_interface::Vector6d::Zero(); delta_x[2] = 1; - Eigen::Matrix delta_theta = Eigen::Vector2d::Zero(); - Eigen::Matrix delta_x_est; + Eigen::VectorXd delta_theta = Eigen::VectorXd::Zero(2); + kinematics_interface::Vector6d delta_x_est; Eigen::Matrix jacobian = Eigen::Matrix::Zero(); Eigen::Matrix x_a, x_b; // wrong size input vector - Eigen::Matrix vec_5 = Eigen::Matrix::Zero(); + Eigen::VectorXd vec_5 = Eigen::VectorXd::Zero(5); // wrong size input jacobian Eigen::Matrix mat_5_6 = Eigen::Matrix::Zero(); From 7a9ce64497629b0507a198e228e9fa271ef22566 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 21 Oct 2025 07:23:53 +0000 Subject: [PATCH 08/11] Add set_custom_node_parameters --- .../test/kinematics_interface_common_tests.hpp | 14 ++------------ .../test/test_kinematics_interface_kdl.cpp | 4 ++++ 2 files changed, 6 insertions(+), 12 deletions(-) diff --git a/kinematics_interface/test/kinematics_interface_common_tests.hpp b/kinematics_interface/test/kinematics_interface_common_tests.hpp index 0b55c61..fecc10c 100644 --- a/kinematics_interface/test/kinematics_interface_common_tests.hpp +++ b/kinematics_interface/test/kinematics_interface_common_tests.hpp @@ -56,10 +56,11 @@ class TestPlugin : 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_); node_->declare_parameter("base", std::string("")); + + PluginUnderTest::set_custom_node_parameters(node_); } void TearDown() @@ -80,17 +81,6 @@ class TestPlugin : public ::testing::Test node_->set_parameter(param); } - /** - * \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. diff --git a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp index 105e2cd..1f154a2 100644 --- a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp @@ -21,6 +21,10 @@ struct PluginKDL { static std::string Name() { return "kinematics_interface_kdl/KinematicsInterfaceKDL"; } + static void set_custom_node_parameters(rclcpp_lifecycle::LifecycleNode::SharedPtr node) + { + node->declare_parameter("alpha", 0.005); + } }; using MyTypes = ::testing::Types; From 91aa0e1a93d4dcd38295f864cad2c11e6533547b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 21 Oct 2025 19:14:48 +0000 Subject: [PATCH 09/11] 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 fecc10c..e4b1634 100644 --- a/kinematics_interface/test/kinematics_interface_common_tests.hpp +++ b/kinematics_interface/test/kinematics_interface_common_tests.hpp @@ -107,6 +107,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(), "")); @@ -192,8 +193,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(), "")); @@ -210,15 +212,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 a43cc9234112df94fdc3106e2fa250564e0488e2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 21 Oct 2025 19:20:51 +0000 Subject: [PATCH 10/11] Move to loadTip method --- .../test/kinematics_interface_common_tests.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/kinematics_interface/test/kinematics_interface_common_tests.hpp b/kinematics_interface/test/kinematics_interface_common_tests.hpp index e4b1634..d90a066 100644 --- a/kinematics_interface/test/kinematics_interface_common_tests.hpp +++ b/kinematics_interface/test/kinematics_interface_common_tests.hpp @@ -89,6 +89,7 @@ class TestPlugin : public ::testing::Test { rclcpp::Parameter param("tip", tip); node_->set_parameter(param); + this->end_effector_ = tip; } /** @@ -107,7 +108,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(), "")); @@ -195,7 +195,6 @@ TYPED_TEST_P(TestPlugin, plugin_function_reduced_model_base) { 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(), "")); From a87c50e6e09cbe6ea79221de6b3df79f3d72d00a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 22 Oct 2025 19:06:28 +0000 Subject: [PATCH 11/11] Add tests for wrong frame strings --- .../kinematics_interface_common_tests.hpp | 38 +++++++++++++------ 1 file changed, 27 insertions(+), 11 deletions(-) diff --git a/kinematics_interface/test/kinematics_interface_common_tests.hpp b/kinematics_interface/test/kinematics_interface_common_tests.hpp index d90a066..7a3f67e 100644 --- a/kinematics_interface/test/kinematics_interface_common_tests.hpp +++ b/kinematics_interface/test/kinematics_interface_common_tests.hpp @@ -40,6 +40,8 @@ class TestPlugin : public ::testing::Test std::shared_ptr ik_; std::shared_ptr node_; std::string end_effector_ = "link2"; + // world -> base_joint(fixed) -> 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); @@ -105,7 +107,7 @@ class TestPlugin : public ::testing::Test TYPED_TEST_SUITE_P(TestPlugin); -TYPED_TEST_P(TestPlugin, basic_plugin_function) +TYPED_TEST_P(TestPlugin, plugin_function_basic) { this->loadTipParameter("link3"); @@ -309,6 +311,27 @@ TYPED_TEST_P(TestPlugin, plugin_calculate_frame_difference_std_vector) EXPECT_THAT(delta_x, ::testing::Pointwise(::testing::DoubleNear(0.02), delta_x_est)); } +TYPED_TEST_P(TestPlugin, incorrect_parameters) +{ + this->loadTipParameter(""); + EXPECT_FALSE( + this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); + + this->loadTipParameter("unknown"); + EXPECT_FALSE( + this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); + + this->loadBaseParameter("unknown"); + this->loadTipParameter("link2"); + EXPECT_FALSE( + this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); + + // but this should work + this->loadBaseParameter("link2"); + this->loadTipParameter("link1"); + EXPECT_TRUE(this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); +} + TYPED_TEST_P(TestPlugin, incorrect_input_sizes) { // initialize the plugin @@ -370,17 +393,10 @@ TYPED_TEST_P(TestPlugin, plugin_no_robot_description) ASSERT_FALSE(this->ik_->initialize("", this->node_->get_node_parameters_interface(), "")); } -TYPED_TEST_P(TestPlugin, plugin_no_parameter_tip) -{ - this->loadTipParameter(""); - ASSERT_FALSE( - this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); -} - REGISTER_TYPED_TEST_SUITE_P( - TestPlugin, basic_plugin_function, plugin_function_reduced_model_tip, + TestPlugin, plugin_function_basic, plugin_function_reduced_model_tip, plugin_function_reduced_model_base, plugin_function_std_vector, plugin_calculate_frame_difference, - plugin_calculate_frame_difference_std_vector, incorrect_input_sizes, plugin_no_robot_description, - plugin_no_parameter_tip); + plugin_calculate_frame_difference_std_vector, incorrect_parameters, incorrect_input_sizes, + plugin_no_robot_description); #endif // KINEMATICS_INTERFACE_COMMON_TESTS_HPP_