From bbcea681000073f3ffbd2d5cebed79fd7d5ba6ef Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Tue, 9 Apr 2024 13:51:27 -0400 Subject: [PATCH 1/9] Pass the robot_description as an arg in initialize() with fallback --- .../kinematics_interface.hpp | 3 ++- .../kinematics_interface_kdl.hpp | 3 ++- .../src/kinematics_interface_kdl.cpp | 26 +++++++++++++------ 3 files changed, 22 insertions(+), 10 deletions(-) diff --git a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp index b3d4cee..87d2b29 100644 --- a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp +++ b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp @@ -42,7 +42,8 @@ class KinematicsInterface */ virtual bool initialize( std::shared_ptr parameters_interface, - const std::string & end_effector_name) = 0; + const std::string & end_effector_name, + const std::string & robot_description = "") = 0; /** * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. diff --git a/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp b/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp index 9ed343d..c568757 100644 --- a/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp +++ b/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp @@ -41,7 +41,8 @@ class KinematicsInterfaceKDL : public kinematics_interface::KinematicsInterface public: bool initialize( std::shared_ptr parameters_interface, - const std::string & end_effector_name) override; + const std::string & end_effector_name, + const std::string & robot_description = "") override; bool convert_cartesian_deltas_to_joint_deltas( const Eigen::VectorXd & joint_pos, const Eigen::Matrix & delta_x, diff --git a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp index af60266..9a71389 100644 --- a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp @@ -22,19 +22,29 @@ rclcpp::Logger LOGGER = rclcpp::get_logger("kinematics_interface_kdl"); bool KinematicsInterfaceKDL::initialize( std::shared_ptr parameters_interface, - const std::string & end_effector_name) + const std::string & end_effector_name, + const std::string & robot_description) { // track initialization plugin initialized = true; - // get robot description - auto robot_param = rclcpp::Parameter(); - if (!parameters_interface->get_parameter("robot_description", robot_param)) + std::string robot_description_local; + if (robot_description.empty()) { - RCLCPP_ERROR(LOGGER, "parameter robot_description not set"); - return false; + // If the robot_description input argument is empty, try to get the + // robot_description from the node's parameters. + auto robot_param = rclcpp::Parameter(); + if (!parameters_interface->get_parameter("robot_description", robot_param)) + { + RCLCPP_ERROR(LOGGER, "parameter robot_description not set in kinematics_interface_kdl"); + return false; + } + robot_description_local = robot_param.as_string(); + } + else + { + robot_description_local = robot_description; } - auto robot_description = robot_param.as_string(); // get alpha damping term auto alpha_param = rclcpp::Parameter("alpha", 0.000005); if (parameters_interface->has_parameter("alpha")) @@ -44,7 +54,7 @@ bool KinematicsInterfaceKDL::initialize( alpha = alpha_param.as_double(); // create kinematic chain KDL::Tree robot_tree; - kdl_parser::treeFromString(robot_description, robot_tree); + kdl_parser::treeFromString(robot_description_local, robot_tree); root_name_ = robot_tree.getRootSegment()->first; if (!robot_tree.getChain(root_name_, end_effector_name, chain_)) { From 24ba1d60534382274ef5b45c903a2dee8af5f979 Mon Sep 17 00:00:00 2001 From: Nikola Banovic Date: Thu, 1 Aug 2024 11:53:23 +0200 Subject: [PATCH 2/9] apply API change discussed in PR#66 --- .../kinematics_interface.hpp | 8 +++++-- .../kinematics_interface_kdl.hpp | 4 ++-- .../src/kinematics_interface_kdl.cpp | 24 +++++++++++++------ 3 files changed, 25 insertions(+), 11 deletions(-) diff --git a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp index 87d2b29..a4f50b8 100644 --- a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp +++ b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp @@ -39,11 +39,15 @@ class KinematicsInterface /** * \brief Initialize plugin. This method must be called before any other. + * \param[in] robot_description robot URDF in string format + * \param[in] parameters_interface + * \param[in] param_namespace namespace for kinematics parameters - defaults to "kinematics" + * \return true if successful */ virtual bool initialize( + const std::string & robot_description, std::shared_ptr parameters_interface, - const std::string & end_effector_name, - const std::string & robot_description = "") = 0; + const std::string & param_namespace = "kinematics") = 0; /** * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. diff --git a/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp b/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp index c568757..62bba5e 100644 --- a/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp +++ b/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp @@ -40,9 +40,9 @@ class KinematicsInterfaceKDL : public kinematics_interface::KinematicsInterface { public: bool initialize( + const std::string & robot_description, std::shared_ptr parameters_interface, - const std::string & end_effector_name, - const std::string & robot_description = "") override; + const std::string & param_namespace = "kinematics") override; bool convert_cartesian_deltas_to_joint_deltas( const Eigen::VectorXd & joint_pos, const Eigen::Matrix & delta_x, diff --git a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp index 9a71389..56518a5 100644 --- a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp @@ -21,9 +21,9 @@ namespace kinematics_interface_kdl rclcpp::Logger LOGGER = rclcpp::get_logger("kinematics_interface_kdl"); bool KinematicsInterfaceKDL::initialize( + const std::string & robot_description, std::shared_ptr parameters_interface, - const std::string & end_effector_name, - const std::string & robot_description) + const std::string & param_namespace) { // track initialization plugin initialized = true; @@ -34,7 +34,7 @@ bool KinematicsInterfaceKDL::initialize( // If the robot_description input argument is empty, try to get the // robot_description from the node's parameters. auto robot_param = rclcpp::Parameter(); - if (!parameters_interface->get_parameter("robot_description", robot_param)) + if (!parameters_interface->get_parameter(param_namespace + "robot_description", robot_param)) { RCLCPP_ERROR(LOGGER, "parameter robot_description not set in kinematics_interface_kdl"); return false; @@ -45,22 +45,32 @@ bool KinematicsInterfaceKDL::initialize( { robot_description_local = robot_description; } + + // get parameters + std::string ns = !param_namespace.empty() ? param_namespace + "." : ""; // get alpha damping term auto alpha_param = rclcpp::Parameter("alpha", 0.000005); - if (parameters_interface->has_parameter("alpha")) + if (parameters_interface->has_parameter(ns + "alpha")) { - parameters_interface->get_parameter("alpha", alpha_param); + parameters_interface->get_parameter(ns + "alpha", alpha_param); } alpha = alpha_param.as_double(); + // get end-effector name + auto end_effector_name = rclcpp::Parameter("tip", "MISSING_END_EFFECTOR_NAME"); + if (parameters_interface->has_parameter(ns + "tip")) + { + parameters_interface->get_parameter(ns + "tip", end_effector_name); + } + // create kinematic chain KDL::Tree robot_tree; kdl_parser::treeFromString(robot_description_local, robot_tree); root_name_ = robot_tree.getRootSegment()->first; - if (!robot_tree.getChain(root_name_, end_effector_name, chain_)) + if (!robot_tree.getChain(root_name_, end_effector_name.as_string(), chain_)) { RCLCPP_ERROR( LOGGER, "failed to find chain from robot root %s to end effector %s", root_name_.c_str(), - end_effector_name.c_str()); + end_effector_name.as_string().c_str()); return false; } // create map from link names to their index From daf0f5581391a4ef31e4594325d81b7d4b1d2077 Mon Sep 17 00:00:00 2001 From: Nikola Banovic Date: Thu, 1 Aug 2024 11:53:50 +0200 Subject: [PATCH 3/9] adjust API in tests as well --- .../test/test_kinematics_interface_kdl.cpp | 30 ++++++++++++++----- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp index b78ae43..258ddfc 100644 --- a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp @@ -28,6 +28,8 @@ class TestKDLPlugin : public ::testing::Test 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() { @@ -50,9 +52,7 @@ class TestKDLPlugin : public ::testing::Test void loadURDFParameter() { - auto urdf = std::string(ros2_control_test_assets::urdf_head) + - std::string(ros2_control_test_assets::urdf_tail); - rclcpp::Parameter param("robot_description", urdf); + rclcpp::Parameter param("robot_description", urdf_); node_->declare_parameter("robot_description", ""); node_->set_parameter(param); } @@ -63,6 +63,18 @@ class TestKDLPlugin : public ::testing::Test node_->declare_parameter("alpha", 0.005); node_->set_parameter(param); } + + /** + * \brief Used for testing initialization from parameters. + * Elsewhere, `end_effector_` member is used. + */ + void loadTipParameter() + { + + rclcpp::Parameter param("tip", "link2"); + node_->declare_parameter("tip", "link2"); + node_->set_parameter(param); + } }; TEST_F(TestKDLPlugin, KDL_plugin_function) @@ -70,9 +82,10 @@ TEST_F(TestKDLPlugin, KDL_plugin_function) // load robot description and alpha to parameter server loadURDFParameter(); loadAlphaParameter(); + loadTipParameter(); // initialize the plugin - ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); // calculate end effector transform Eigen::Matrix pos = Eigen::Matrix::Zero(); @@ -103,9 +116,10 @@ TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector) // load robot description and alpha to parameter server loadURDFParameter(); loadAlphaParameter(); + loadTipParameter(); // initialize the plugin - ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); // calculate end effector transform std::vector pos = {0, 0}; @@ -136,9 +150,10 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes) // load robot description and alpha to parameter server loadURDFParameter(); loadAlphaParameter(); + loadTipParameter(); // initialize the plugin - ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); // define correct values Eigen::Matrix pos = Eigen::Matrix::Zero(); @@ -175,5 +190,6 @@ TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description) { // load alpha to parameter server loadAlphaParameter(); - ASSERT_FALSE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + loadTipParameter(); + ASSERT_FALSE(ik_->initialize("", node_->get_node_parameters_interface(), "")); } From ec63d876131c8e30489dfc9b885b8b5507d283c8 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 14 Aug 2024 16:09:38 +0200 Subject: [PATCH 4/9] Update kinematics_interface_kdl/src/kinematics_interface_kdl.cpp --- kinematics_interface_kdl/src/kinematics_interface_kdl.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp index 6f06fa1..5b68ddf 100644 --- a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp @@ -66,7 +66,6 @@ bool KinematicsInterfaceKDL::initialize( // create kinematic chain KDL::Tree robot_tree; kdl_parser::treeFromString(robot_description_local, robot_tree); - // get root name auto base_param = rclcpp::Parameter(); if (parameters_interface->has_parameter(ns + "base")) From c59d396372abc3b22d8ffffdd546013021bb784a Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 16 Aug 2024 19:21:42 +0200 Subject: [PATCH 5/9] Update kinematics_interface/include/kinematics_interface/kinematics_interface.hpp --- .../include/kinematics_interface/kinematics_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp index a4f50b8..0602246 100644 --- a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp +++ b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp @@ -47,7 +47,7 @@ class KinematicsInterface virtual bool initialize( const std::string & robot_description, std::shared_ptr parameters_interface, - const std::string & param_namespace = "kinematics") = 0; + const std::string & param_namespace) = 0; /** * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. From 3ac190ec2b9654c13149b31c4377e0eaff9fc087 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 16 Aug 2024 19:22:10 +0200 Subject: [PATCH 6/9] Update kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp --- .../kinematics_interface_kdl/kinematics_interface_kdl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp b/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp index 62bba5e..645a5fa 100644 --- a/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp +++ b/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp @@ -42,7 +42,7 @@ class KinematicsInterfaceKDL : public kinematics_interface::KinematicsInterface bool initialize( const std::string & robot_description, std::shared_ptr parameters_interface, - const std::string & param_namespace = "kinematics") override; + const std::string & param_namespace) override; bool convert_cartesian_deltas_to_joint_deltas( const Eigen::VectorXd & joint_pos, const Eigen::Matrix & delta_x, From 0c48ef39f1fd17defb86ee201d7bd952b90fdac1 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 16 Aug 2024 19:23:34 +0200 Subject: [PATCH 7/9] Update kinematics_interface_kdl/src/kinematics_interface_kdl.cpp Co-authored-by: Sai Kishor Kothakota --- kinematics_interface_kdl/src/kinematics_interface_kdl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp index 5b68ddf..f870202 100644 --- a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp @@ -58,7 +58,7 @@ bool KinematicsInterfaceKDL::initialize( alpha = alpha_param.as_double(); // get end-effector name auto end_effector_name = rclcpp::Parameter("tip", "MISSING_END_EFFECTOR_NAME"); - if (parameters_interface->has_parameter(param_namespace + "tip")) + if (parameters_interface->has_parameter(ns + "tip")) { parameters_interface->get_parameter(ns + "tip", end_effector_name); } From 209d67d33e606d010898224f5b8620505fabcd22 Mon Sep 17 00:00:00 2001 From: Nikola Banovic Date: Mon, 19 Aug 2024 09:57:47 +0200 Subject: [PATCH 8/9] initialize() return false on missing `tip` parameter --- .../src/kinematics_interface_kdl.cpp | 14 +++++++++++--- .../test/test_kinematics_interface_kdl.cpp | 5 +++++ 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp index f870202..7bf3c9a 100644 --- a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp @@ -57,11 +57,19 @@ bool KinematicsInterfaceKDL::initialize( } alpha = alpha_param.as_double(); // get end-effector name - auto end_effector_name = rclcpp::Parameter("tip", "MISSING_END_EFFECTOR_NAME"); + auto end_effector_name_param = rclcpp::Parameter("tip"); if (parameters_interface->has_parameter(ns + "tip")) { - parameters_interface->get_parameter(ns + "tip", end_effector_name); + parameters_interface->get_parameter(ns + "tip", end_effector_name_param); } + else + { + RCLCPP_ERROR( + LOGGER, "Failed to find end effector name parameter [tip]."); + return false; + } + std::string end_effector_name = end_effector_name_param.as_string(); + // create kinematic chain KDL::Tree robot_tree; @@ -82,7 +90,7 @@ bool KinematicsInterfaceKDL::initialize( { RCLCPP_ERROR( LOGGER, "failed to find chain from robot root %s to end effector %s", root_name_.c_str(), - end_effector_name.as_string().c_str()); + end_effector_name.c_str()); return false; } // create map from link names to their index diff --git a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp index 258ddfc..c899901 100644 --- a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp @@ -193,3 +193,8 @@ TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description) loadTipParameter(); ASSERT_FALSE(ik_->initialize("", node_->get_node_parameters_interface(), "")); } + +TEST_F(TestKDLPlugin, KDL_plugin_no_parameter_tip) +{ + ASSERT_FALSE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); +} From 00676388bc83757b7aa9cd44a9f2a6a4393e747f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 4 Nov 2024 12:52:58 +0000 Subject: [PATCH 9/9] Fix whitespaces --- .../include/kinematics_interface/kinematics_interface.hpp | 2 +- kinematics_interface_kdl/src/kinematics_interface_kdl.cpp | 6 ++---- .../test/test_kinematics_interface_kdl.cpp | 3 +-- 3 files changed, 4 insertions(+), 7 deletions(-) diff --git a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp index 0602246..9e4f10c 100644 --- a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp +++ b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp @@ -40,7 +40,7 @@ class KinematicsInterface /** * \brief Initialize plugin. This method must be called before any other. * \param[in] robot_description robot URDF in string format - * \param[in] parameters_interface + * \param[in] parameters_interface * \param[in] param_namespace namespace for kinematics parameters - defaults to "kinematics" * \return true if successful */ diff --git a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp index 7bf3c9a..62dca65 100644 --- a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp @@ -27,7 +27,7 @@ bool KinematicsInterfaceKDL::initialize( { // track initialization plugin initialized = true; - + // get parameters std::string ns = !param_namespace.empty() ? param_namespace + "." : ""; @@ -64,13 +64,11 @@ bool KinematicsInterfaceKDL::initialize( } else { - RCLCPP_ERROR( - LOGGER, "Failed to find end effector name parameter [tip]."); + RCLCPP_ERROR(LOGGER, "Failed to find end effector name parameter [tip]."); return false; } std::string end_effector_name = end_effector_name_param.as_string(); - // create kinematic chain KDL::Tree robot_tree; kdl_parser::treeFromString(robot_description_local, robot_tree); diff --git a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp index c899901..9176c3a 100644 --- a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp @@ -69,8 +69,7 @@ class TestKDLPlugin : public ::testing::Test * Elsewhere, `end_effector_` member is used. */ void loadTipParameter() - { - + { rclcpp::Parameter param("tip", "link2"); node_->declare_parameter("tip", "link2"); node_->set_parameter(param);