Skip to content

Refactor and extend tests #210

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 12 commits into
base: master
Choose a base branch
from
Open

Refactor and extend tests #210

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
c54b5b5
Improve error messages
christophfroehlich Oct 20, 2025
bbb642c
Split and add new tests, use gmock matcher
christophfroehlich Oct 20, 2025
7e18816
std::source_location::function_name()
christophfroehlich Oct 20, 2025
f83c633
const&
christophfroehlich Oct 20, 2025
aec6d5b
Eigen::Vector
christophfroehlich Oct 20, 2025
c1dc67f
Eigen::Vector
christophfroehlich Oct 20, 2025
effc965
Use c++17 compatible version
christophfroehlich Oct 20, 2025
918e07e
Add Vector6d
christophfroehlich Oct 20, 2025
3bc4fbd
Use correct macro
christophfroehlich Oct 20, 2025
97a45a7
Specify Eigen Vector sizes
christophfroehlich Oct 21, 2025
f5c5fa7
Fix end_effector variable for tests
christophfroehlich Oct 21, 2025
1f9c887
Add tests for wrong frame strings
christophfroehlich Oct 22, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 9 additions & 3 deletions kinematics_interface_kdl/src/kinematics_interface_kdl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,15 +79,15 @@ bool KinematicsInterfaceKDL::initialize(
parameters_interface->get_parameter(ns + "base", base_param);
root_name_ = base_param.as_string();
}
else
if (root_name_.empty())
{
root_name_ = robot_tree.getRootSegment()->first;
}

if (!robot_tree.getChain(root_name_, end_effector_name, chain_))
{
RCLCPP_ERROR(
LOGGER, "failed to find chain from robot root %s to end effector %s", root_name_.c_str(),
LOGGER, "failed to find chain from robot root '%s' to end effector '%s'", root_name_.c_str(),
end_effector_name.c_str());
return false;
}
Expand Down Expand Up @@ -121,6 +121,7 @@ bool KinematicsInterfaceKDL::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;
}

Expand All @@ -144,6 +145,7 @@ bool KinematicsInterfaceKDL::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;
}

Expand All @@ -167,6 +169,7 @@ bool KinematicsInterfaceKDL::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;
}

Expand All @@ -189,6 +192,7 @@ bool KinematicsInterfaceKDL::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;
}

Expand All @@ -215,6 +219,7 @@ bool KinematicsInterfaceKDL::calculate_link_transform(
// verify inputs
if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name))
{
RCLCPP_ERROR(LOGGER, "Input verification failed in calculate_link_transform");
return false;
}

Expand All @@ -241,8 +246,9 @@ bool KinematicsInterfaceKDL::calculate_frame_difference(
Eigen::Matrix<double, 6, 1> & 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;
}

Expand Down
222 changes: 151 additions & 71 deletions kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,10 @@
#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:
Expand All @@ -42,6 +46,12 @@ class TestKDLPlugin : public ::testing::Test
"kinematics_interface", "kinematics_interface::KinematicsInterface");
ik_ = std::unique_ptr<kinematics_interface::KinematicsInterface>(
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()
Expand All @@ -50,40 +60,94 @@ class TestKDLPlugin : public ::testing::Test
rclcpp::shutdown();
}

void loadURDFParameter()
/**
* \brief Used for testing initialization from parameters.
* Elsewhere, `urdf_` member is used.
*/

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);
}

/**
* \brief Used for testing initialization from parameters.
* Elsewhere, `end_effector_` member is used.
*/
void loadTipParameter()
void loadTipParameter(std::string tip)
{
rclcpp::Parameter param("tip", "link2");
node_->declare_parameter("tip", "link2");
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(TestKDLPlugin, KDL_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<double, Eigen::Dynamic, 1> pos = Eigen::Matrix<double, 3, 1>::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<double, 6, 1> delta_x = Eigen::Matrix<double, 6, 1>::Zero();
delta_x[2] = 1; // vz
Eigen::Matrix<double, Eigen::Dynamic, 1> delta_theta = Eigen::Matrix<double, 3, 1>::Zero();
ASSERT_TRUE(
ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta));

// initialize the plugin
// convert joint delta to cartesian delta
Eigen::Matrix<double, 6, 1> 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<double, 6, Eigen::Dynamic> jacobian = Eigen::Matrix<double, 6, 3>::Zero();
ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian));

// calculate jacobian inverse
Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian_inverse =
jacobian.completeOrthogonalDecomposition().pseudoInverse();
Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian_inverse_est =
Eigen::Matrix<double, 3, 6>::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
Expand All @@ -93,7 +157,7 @@ TEST_F(TestKDLPlugin, KDL_plugin_function)

// convert cartesian delta to joint delta
Eigen::Matrix<double, 6, 1> delta_x = Eigen::Matrix<double, 6, 1>::Zero();
delta_x[2] = 1;
delta_x[2] = 1; // vz
Eigen::Matrix<double, Eigen::Dynamic, 1> delta_theta = Eigen::Matrix<double, 2, 1>::Zero();
ASSERT_TRUE(
ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector_, delta_theta));
Expand All @@ -104,10 +168,7 @@ TEST_F(TestKDLPlugin, KDL_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<double, 6, Eigen::Dynamic> jacobian = Eigen::Matrix<double, 6, 2>::Zero();
Expand All @@ -121,38 +182,56 @@ TEST_F(TestKDLPlugin, KDL_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<double, 7, 1> 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<double, 6, 1>::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(TestKDLPlugin, KDL_plugin_function_reduced_model_base)
{
loadTipParameter("link3");
loadBaseParameter("link1");

// ensure that difference math is correct
for (Eigen::Index i = 0; i < delta_x.size(); ++i)
{
ASSERT_NEAR(delta_x(i), delta_x_est(i), 0.02);
}
// initialize the plugin
ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), ""));

// calculate end effector transform
Eigen::Matrix<double, Eigen::Dynamic, 1> pos = Eigen::Matrix<double, 2, 1>::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<double, 6, 1> delta_x = Eigen::Matrix<double, 6, 1>::Zero();
delta_x[2] = 1; // vz
Eigen::Matrix<double, Eigen::Dynamic, 1> delta_theta = Eigen::Matrix<double, 2, 1>::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<double, 2, 1>::Zero(), 0.02));

// convert joint delta to cartesian delta
Eigen::Matrix<double, 6, 1> 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(Eigen::Matrix<double, 6, 1>::Zero(), 0.02));

// calculate jacobian
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian = Eigen::Matrix<double, 6, 2>::Zero();
ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian));

// calculate jacobian inverse
Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian_inverse =
jacobian.completeOrthogonalDecomposition().pseudoInverse();
Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian_inverse_est =
Eigen::Matrix<double, 2, 6>::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)
{
// 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
Expand All @@ -173,10 +252,7 @@ TEST_F(TestKDLPlugin, KDL_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<size_t>(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<double, 6, Eigen::Dynamic> jacobian = Eigen::Matrix<double, 6, 2>::Zero();
Expand All @@ -190,38 +266,43 @@ TEST_F(TestKDLPlugin, KDL_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));
}

TEST_F(TestKDLPlugin, KDL_plugin_calculate_frame_difference)
{
// compute the difference between two cartesian frames
Eigen::Matrix<double, 7, 1> 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<double, 6, 1> delta_x = Eigen::Matrix<double, 6, 1>::Zero();
Eigen::Matrix<double, 6, 1> 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)
{
// compute the difference between two cartesian frames
std::vector<double> 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<double> delta_x = {0, 0, 0, 0, 0, 0};
std::vector<double> 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 (size_t i = 0; i < static_cast<size_t>(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));
}

TEST_F(TestKDLPlugin, 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
Expand Down Expand Up @@ -273,13 +354,12 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes)

TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description)
{
// load alpha to parameter server
loadAlphaParameter();
loadTipParameter();
loadURDFParameter("");
ASSERT_FALSE(ik_->initialize("", node_->get_node_parameters_interface(), ""));
}

TEST_F(TestKDLPlugin, KDL_plugin_no_parameter_tip)
{
loadTipParameter("");
ASSERT_FALSE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), ""));
}