diff --git a/kinematics_interface/CMakeLists.txt b/kinematics_interface/CMakeLists.txt index a89f256..5675cae 100644 --- a/kinematics_interface/CMakeLists.txt +++ b/kinematics_interface/CMakeLists.txt @@ -33,6 +33,36 @@ install( DIRECTORY include/ DESTINATION include/kinematics_interface ) + +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_tests + ) + 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 @@ -40,6 +70,7 @@ install( LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) +endif() ament_export_targets(export_kinematics_interface HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} Eigen3) diff --git a/kinematics_interface/package.xml b/kinematics_interface/package.xml index e96899d..8bf692f 100644 --- a/kinematics_interface/package.xml +++ b/kinematics_interface/package.xml @@ -26,6 +26,10 @@ eigen rclcpp_lifecycle + ament_cmake_gmock + pluginlib + ros2_control_test_assets + ament_cmake diff --git a/kinematics_interface/test/kinematics_interface_common_tests.hpp b/kinematics_interface/test/kinematics_interface_common_tests.hpp new file mode 100644 index 0000000..7a3f67e --- /dev/null +++ b/kinematics_interface/test/kinematics_interface_common_tests.hpp @@ -0,0 +1,402 @@ +// 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"; + // 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); + + 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("robot_description", urdf_); + node_->declare_parameter("tip", end_effector_); + node_->declare_parameter("base", std::string("")); + + PluginUnderTest::set_custom_node_parameters(node_); + } + + void TearDown() + { + node_->shutdown(); + // 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, `end_effector_` member is used. + */ + void loadTipParameter(const std::string & tip) + { + 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); + } +}; + +TYPED_TEST_SUITE_P(TestPlugin); + +TYPED_TEST_P(TestPlugin, plugin_function_basic) +{ + this->loadTipParameter("link3"); + + // initialize the plugin + ASSERT_TRUE(this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); + + // calculate end effector transform + 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)); + + // 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(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 + 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 + kinematics_interface::Vector6d delta_x = kinematics_interface::Vector6d::Zero(); + delta_x[2] = 1; // vz + 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 + 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_base) +{ + this->loadBaseParameter("link1"); + this->loadTipParameter("link3"); + + // initialize the plugin + ASSERT_TRUE(this->ik_->initialize(this->urdf_, this->node_->get_node_parameters_interface(), "")); + + // calculate end effector transform + 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 + kinematics_interface::Vector6d delta_x = kinematics_interface::Vector6d::Zero(); + delta_x[2] = 1; // vz + 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 + 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_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_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 + ASSERT_TRUE(this->ik_->initialize(this->urdf_, this->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( + 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(), "")); +} + +REGISTER_TYPED_TEST_SUITE_P( + 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_parameters, incorrect_input_sizes, + plugin_no_robot_description); + +#endif // KINEMATICS_INTERFACE_COMMON_TESTS_HPP_ diff --git a/kinematics_interface_kdl/CMakeLists.txt b/kinematics_interface_kdl/CMakeLists.txt index d8322f5..abc486e 100644 --- a/kinematics_interface_kdl/CMakeLists.txt +++ b/kinematics_interface_kdl/CMakeLists.txt @@ -38,13 +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 ) - 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( 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 diff --git a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp index a6f3bd3..1f154a2 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,372 +12,20 @@ // 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("")); - } +#include "kinematics_interface_tests/kinematics_interface_common_tests.hpp" - 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); - this->end_effector_ = tip; - } - - /** - * \brief Used for testing initialization from parameters. - * Elsewhere, `""` is used. - */ - void loadBaseParameter(const std::string & base) +struct PluginKDL +{ + static std::string Name() { return "kinematics_interface_kdl/KinematicsInterfaceKDL"; } + static void set_custom_node_parameters(rclcpp_lifecycle::LifecycleNode::SharedPtr node) { - rclcpp::Parameter param("base", base); - node_->set_parameter(param); + node->declare_parameter("alpha", 0.005); } }; -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(), "")); - - // 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) -{ - // 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) -{ - // 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, KDL_incorrect_parameters) -{ - loadTipParameter(""); - EXPECT_FALSE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); - - loadTipParameter("unknown"); - EXPECT_FALSE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); - - 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 - 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(), "")); -} - -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);