diff --git a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp index 795f4e2..b55d669 100644 --- a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp +++ b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp @@ -29,8 +29,18 @@ #include "rclcpp/logging.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" +// TODO(anyone): Use std::source_location::function_name() once we require C++20 +#ifdef _MSC_VER +#define FUNCTION_SIGNATURE __FUNCSIG__ +#else +#define FUNCTION_SIGNATURE __PRETTY_FUNCTION__ +#endif + namespace kinematics_interface { + +using Vector6d = Eigen::Matrix; + class KinematicsInterface { public: diff --git a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp index fb6875c..3ce7a41 100644 --- a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp @@ -79,7 +79,7 @@ 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; } @@ -87,7 +87,7 @@ bool KinematicsInterfaceKDL::initialize( 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; } @@ -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 '%s'", FUNCTION_SIGNATURE); return false; } @@ -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 '%s'", FUNCTION_SIGNATURE); return false; } @@ -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 '%s'", FUNCTION_SIGNATURE); return false; } @@ -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 '%s'", FUNCTION_SIGNATURE); return false; } @@ -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 '%s'", FUNCTION_SIGNATURE); return false; } @@ -241,8 +246,9 @@ bool KinematicsInterfaceKDL::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 '%s'", FUNCTION_SIGNATURE); return false; } diff --git a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp index 48ead26..a6f3bd3 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 @@ #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: @@ -42,6 +46,12 @@ class TestKDLPlugin : public ::testing::Test "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() @@ -50,17 +60,25 @@ class TestKDLPlugin : public ::testing::Test rclcpp::shutdown(); } - void loadURDFParameter() + /** + * \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_->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); } @@ -68,46 +86,90 @@ class TestKDLPlugin : public ::testing::Test * \brief Used for testing initialization from parameters. * Elsewhere, `end_effector_` member is used. */ - void loadTipParameter() + void loadTipParameter(const std::string & tip) { - rclcpp::Parameter param("tip", "link2"); - node_->declare_parameter("tip", "link2"); + rclcpp::Parameter param("tip", tip); + node_->set_parameter(param); + this->end_effector_ = tip; + } + + /** + * \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) { - // load robot description and alpha to parameter server - loadURDFParameter(); - loadAlphaParameter(); - loadTipParameter(); + loadTipParameter("link3"); - // initialize the plugin + // initialize the plugin ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); // calculate end effector transform - Eigen::Matrix pos = Eigen::Matrix::Zero(); + 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 - Eigen::Matrix delta_x = Eigen::Matrix::Zero(); - delta_x[2] = 1; - Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); + 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 - Eigen::Matrix delta_x_est; + 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 - 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(); + 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(); @@ -121,38 +183,55 @@ 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 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(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 + 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_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 @@ -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(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(); @@ -190,52 +266,75 @@ 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 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) +{ // 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 - 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)); } -TEST_F(TestKDLPlugin, incorrect_input_sizes) +TEST_F(TestKDLPlugin, KDL_incorrect_parameters) { - // load robot description and alpha to parameter server - loadURDFParameter(); - loadAlphaParameter(); - loadTipParameter(); + loadTipParameter(""); + EXPECT_FALSE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); + + loadTipParameter("unknown"); + EXPECT_FALSE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); - // initialize the plugin + loadBaseParameter("unknown"); + loadTipParameter("link2"); + EXPECT_FALSE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); + + // but this should work + loadBaseParameter("link2"); + loadTipParameter("link1"); + EXPECT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); +} + +TEST_F(TestKDLPlugin, 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(); + 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::Matrix::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(); @@ -273,13 +372,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(), "")); }