From d5bd5daf8308cb09457cc9594fec5544eb68d0c6 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 19:23:29 +0100 Subject: [PATCH 01/16] use new realtime_tools headers --- .../include/dynamics_interface_fd/dynamics_interface_fd.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dynamics_interface_fd/include/dynamics_interface_fd/dynamics_interface_fd.hpp b/dynamics_interface_fd/include/dynamics_interface_fd/dynamics_interface_fd.hpp index d4ec6be..4fda026 100644 --- a/dynamics_interface_fd/include/dynamics_interface_fd/dynamics_interface_fd.hpp +++ b/dynamics_interface_fd/include/dynamics_interface_fd/dynamics_interface_fd.hpp @@ -40,7 +40,7 @@ #include #include "rclcpp/node_interfaces/node_parameters_interface.hpp" -#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_buffer.hpp" #include "std_msgs/msg/float64_multi_array.hpp" From e87349cca2f2bce244d7375849ccd641864070f2 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 19:23:45 +0100 Subject: [PATCH 02/16] fix str format error --- dynamics_interface_fd/src/dynamics_interface_fd.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dynamics_interface_fd/src/dynamics_interface_fd.cpp b/dynamics_interface_fd/src/dynamics_interface_fd.cpp index bd29109..e572fc1 100644 --- a/dynamics_interface_fd/src/dynamics_interface_fd.cpp +++ b/dynamics_interface_fd/src/dynamics_interface_fd.cpp @@ -349,8 +349,8 @@ bool DynamicsInterfaceFd::calculate_inertia( if (fd_inertia_msg_->data.size() != static_cast(36)) { RCLCPP_ERROR( - LOGGER, "The size of the inertia matrix msg (%zu) does not match the required size of (6x6)", - fd_inertia_msg_->data.size(), num_joints_ * num_joints_); + LOGGER, "The size of the inertia matrix msg (%zu) does not match the required size of (6 x 6)", + fd_inertia_msg_->data.size()); return false; } fromMsg(*fd_inertia_msg_, fd_inertia_); From e6023bd2562d2e26afb20929ece19cd980f66703 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 19:26:51 +0100 Subject: [PATCH 03/16] lint --- dynamics_interface_fd/src/dynamics_interface_fd.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dynamics_interface_fd/src/dynamics_interface_fd.cpp b/dynamics_interface_fd/src/dynamics_interface_fd.cpp index e572fc1..ef72b38 100644 --- a/dynamics_interface_fd/src/dynamics_interface_fd.cpp +++ b/dynamics_interface_fd/src/dynamics_interface_fd.cpp @@ -349,7 +349,8 @@ bool DynamicsInterfaceFd::calculate_inertia( if (fd_inertia_msg_->data.size() != static_cast(36)) { RCLCPP_ERROR( - LOGGER, "The size of the inertia matrix msg (%zu) does not match the required size of (6 x 6)", + LOGGER, + "The size of the inertia matrix msg (%zu) does not match the required size of (6 x 6)", fd_inertia_msg_->data.size()); return false; } From e40839acdc395ec1857c31576de4346dbe155aae Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 19:44:57 +0100 Subject: [PATCH 04/16] add jacobian_inverse to track kinematics interface changes --- .../dynamics_interface_kdl.hpp | 6 + .../src/dynamics_interface_kdl.cpp | 140 +++++++++--------- .../test/test_dynamics_interface_kdl.cpp | 11 +- 3 files changed, 84 insertions(+), 73 deletions(-) diff --git a/dynamics_interface_kdl/include/dynamics_interface_kdl/dynamics_interface_kdl.hpp b/dynamics_interface_kdl/include/dynamics_interface_kdl/dynamics_interface_kdl.hpp index 6855d32..d7fb5d9 100644 --- a/dynamics_interface_kdl/include/dynamics_interface_kdl/dynamics_interface_kdl.hpp +++ b/dynamics_interface_kdl/include/dynamics_interface_kdl/dynamics_interface_kdl.hpp @@ -67,6 +67,10 @@ class DynamicsInterfaceKDL : public dynamics_interface::DynamicsInterface const std::string & link_name, Eigen::Matrix & jacobian_derivative) override; + bool calculate_jacobian_inverse( + const Eigen::VectorXd & joint_pos, const std::string & link_name, + Eigen::Matrix & jacobian_inverse) override; + bool calculate_inertia( const Eigen::VectorXd & joint_pos, Eigen::Matrix & inertia) override; @@ -91,6 +95,7 @@ class DynamicsInterfaceKDL : public dynamics_interface::DynamicsInterface bool verify_link_name(const std::string & link_name); bool verify_joint_vector(const Eigen::VectorXd & joint_vector); bool verify_jacobian(const Eigen::Matrix & jacobian); + bool verify_jacobian_inverse(const Eigen::Matrix & jacobian); bool verify_inertia(const Eigen::Matrix & inertia); bool verify_coriolis(const Eigen::VectorXd & coriolis); bool verify_gravity(const Eigen::VectorXd & gravity); @@ -105,6 +110,7 @@ class DynamicsInterfaceKDL : public dynamics_interface::DynamicsInterface KDL::JntArrayVel q_array_vel_; KDL::Frame frame_; std::shared_ptr jacobian_, jacobian_derivative_; + std::shared_ptr> jacobian_inverse_; std::shared_ptr inertia_; KDL::JntArray coriolis_, gravity_; std::shared_ptr jac_solver_; diff --git a/dynamics_interface_kdl/src/dynamics_interface_kdl.cpp b/dynamics_interface_kdl/src/dynamics_interface_kdl.cpp index ee324fc..ec785e0 100644 --- a/dynamics_interface_kdl/src/dynamics_interface_kdl.cpp +++ b/dynamics_interface_kdl/src/dynamics_interface_kdl.cpp @@ -34,31 +34,24 @@ bool DynamicsInterfaceKDL::initialize( std::string ns = !param_namespace.empty() ? param_namespace + "." : ""; std::string robot_description_local; - if (robot_description.empty()) - { + if (robot_description.empty()) { // 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("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 - { + } else { robot_description_local = robot_description; } // get end-effector name auto end_effector_name_param = rclcpp::Parameter("tip"); - if (parameters_interface->has_parameter(ns + "tip")) - { + if (parameters_interface->has_parameter(ns + "tip")) { parameters_interface->get_parameter(ns + "tip", end_effector_name_param); - } - else - { + } else { RCLCPP_ERROR(LOGGER, "Failed to find end effector name parameter [tip]."); return false; } @@ -70,26 +63,21 @@ bool DynamicsInterfaceKDL::initialize( // get root name auto base_param = rclcpp::Parameter(); - if (parameters_interface->has_parameter(ns + "base")) - { + if (parameters_interface->has_parameter(ns + "base")) { parameters_interface->get_parameter(ns + "base", base_param); root_name_ = base_param.as_string(); - } - else - { + } else { 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, 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()); return false; } - if (!robot_tree.getChain(root_name_, end_effector_name, chain_)) - { + 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(), end_effector_name.c_str()); @@ -97,19 +85,16 @@ bool DynamicsInterfaceKDL::initialize( } // create map from link names to their index - for (size_t i = 0; i < chain_.getNrOfSegments(); ++i) - { + for (size_t i = 0; i < chain_.getNrOfSegments(); ++i) { link_name_map_[chain_.getSegment(i).getName()] = i + 1; } // get gravity vector in base frame auto gravity_param = rclcpp::Parameter(); - if (parameters_interface->has_parameter(ns + "gravity")) - { + if (parameters_interface->has_parameter(ns + "gravity")) { parameters_interface->get_parameter(ns + "gravity", gravity_param); std::vector gravity_vec = gravity_param.as_double_array(); - if (gravity_vec.size() != 3) - { + if (gravity_vec.size() != 3) { RCLCPP_ERROR( LOGGER, "The size of the gravity vector (%zu) does not match the required size of 3", gravity_vec.size()); @@ -118,9 +103,7 @@ bool DynamicsInterfaceKDL::initialize( gravity_in_base_frame_[0] = gravity_vec[0]; gravity_in_base_frame_[1] = gravity_vec[1]; gravity_in_base_frame_[2] = gravity_vec[2]; - } - else - { + } else { RCLCPP_ERROR(LOGGER, "Please specify a gravity vector in base frame '%s'.", root_name_.c_str()); return false; /* @@ -144,6 +127,7 @@ bool DynamicsInterfaceKDL::initialize( jacobian_ = std::make_shared(num_joints_); jacobian_derivative_ = std::make_shared(num_joints_); + jacobian_inverse_ = std::make_shared>(num_joints_, 6); inertia_ = std::make_shared(num_joints_); coriolis_ = KDL::JntArray(num_joints_); gravity_ = KDL::JntArray(num_joints_); @@ -165,8 +149,7 @@ bool DynamicsInterfaceKDL::calculate_link_transform( Eigen::Isometry3d & transform) { // verify inputs - if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name)) - { + if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name)) { return false; } @@ -177,8 +160,7 @@ bool DynamicsInterfaceKDL::calculate_link_transform( transform.setIdentity(); // special case: since the root is not in the robot tree, need to return identity transform - if (link_name == root_name_) - { + if (link_name == root_name_) { return true; } @@ -210,6 +192,34 @@ bool DynamicsInterfaceKDL::calculate_jacobian( return true; } +bool DynamicsInterfaceKDL::calculate_jacobian_inverse( + const Eigen::Matrix & joint_pos, const std::string & link_name, + Eigen::Matrix & jacobian_inverse) +{ + // verify inputs + if ( + !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || + !verify_jacobian_inverse(jacobian_inverse)) + { + return false; + } + + // get joint array + q_.data = joint_pos; + + // calculate Jacobian + jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]); + Eigen::Matrix jacobian = jacobian_->data; + + // damped inverse + *jacobian_inverse_ = + (jacobian.transpose() * jacobian + alpha * I).inverse() * jacobian.transpose(); + + jacobian_inverse = *jacobian_inverse_; + + return true; +} + bool DynamicsInterfaceKDL::calculate_jacobian_derivative( const Eigen::Matrix & joint_pos, const Eigen::Matrix & joint_vel, const std::string & link_name, @@ -245,8 +255,7 @@ bool DynamicsInterfaceKDL::calculate_inertia( Eigen::Matrix & inertia) { // verify inputs - if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_inertia(inertia)) - { + if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_inertia(inertia)) { return false; } @@ -286,8 +295,7 @@ bool DynamicsInterfaceKDL::calculate_gravity( const Eigen::VectorXd & joint_pos, Eigen::VectorXd & gravity) { // verify inputs - if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_gravity(gravity)) - { + if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_gravity(gravity)) { return false; } @@ -340,17 +348,12 @@ bool DynamicsInterfaceKDL::convert_cartesian_deltas_to_joint_deltas( return false; } - // get joint array - q_.data = joint_pos; + // calculate Jacobian inverse + if (!calculate_jacobian_inverse(joint_pos, link_name, *jacobian_inverse_)) { + return false; + } - // calculate Jacobian - jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]); - // TODO(anyone): this dynamic allocation needs to be replaced - Eigen::Matrix J = jacobian_->data; - // damped inverse - Eigen::Matrix J_inverse = - (J.transpose() * J + alpha * I).inverse() * J.transpose(); - delta_theta = J_inverse * delta_x; + delta_theta = *jacobian_inverse_ * delta_x; return true; } @@ -360,15 +363,12 @@ bool DynamicsInterfaceKDL::convert_cartesian_deltas_to_joint_deltas( bool DynamicsInterfaceKDL::verify_link_name(const std::string & link_name) { - if (link_name == root_name_) - { + if (link_name == root_name_) { return true; } - if (link_name_map_.find(link_name) == link_name_map_.end()) - { + if (link_name_map_.find(link_name) == link_name_map_.end()) { std::string links; - for (size_t i = 0; i < chain_.getNrOfSegments(); ++i) - { + for (size_t i = 0; i < chain_.getNrOfSegments(); ++i) { links += "\n" + chain_.getSegment(i).getName(); } RCLCPP_ERROR( @@ -381,8 +381,7 @@ bool DynamicsInterfaceKDL::verify_link_name(const std::string & link_name) bool DynamicsInterfaceKDL::verify_joint_vector(const Eigen::VectorXd & joint_vector) { - if (static_cast(joint_vector.size()) != num_joints_) - { + if (static_cast(joint_vector.size()) != num_joints_) { RCLCPP_ERROR( LOGGER, "Invalid joint vector size (%zu). Expected size is %zu.", joint_vector.size(), num_joints_); @@ -394,8 +393,7 @@ bool DynamicsInterfaceKDL::verify_joint_vector(const Eigen::VectorXd & joint_vec bool DynamicsInterfaceKDL::verify_initialized() { // check if interface is initialized - if (!initialized) - { + if (!initialized) { RCLCPP_ERROR( LOGGER, "The KDL kinematics plugin was not initialized. Ensure you called the initialize method."); @@ -407,8 +405,7 @@ bool DynamicsInterfaceKDL::verify_initialized() bool DynamicsInterfaceKDL::verify_jacobian( const Eigen::Matrix & jacobian) { - if (jacobian.rows() != jacobian_->rows() || jacobian.cols() != jacobian_->columns()) - { + if (jacobian.rows() != jacobian_->rows() || jacobian.cols() != jacobian_->columns()) { RCLCPP_ERROR( LOGGER, "The size of the jacobian (%zu, %zu) does not match the required size of (%u, %u)", jacobian.rows(), jacobian.cols(), jacobian_->rows(), jacobian_->columns()); @@ -417,11 +414,24 @@ bool DynamicsInterfaceKDL::verify_jacobian( return true; } +bool DynamicsInterfaceKDL::verify_jacobian_inverse( + const Eigen::Matrix & jacobian_inverse) +{ + if ( + jacobian_inverse.rows() != jacobian_->columns() || jacobian_inverse.cols() != jacobian_->rows()) + { + RCLCPP_ERROR( + LOGGER, "The size of the jacobian (%zu, %zu) does not match the required size of (%u, %u)", + jacobian_inverse.rows(), jacobian_inverse.cols(), jacobian_->columns(), jacobian_->rows()); + return false; + } + return true; +} + bool DynamicsInterfaceKDL::verify_inertia( const Eigen::Matrix & inertia) { - if (inertia.rows() != inertia_->rows() || inertia.cols() != inertia_->columns()) - { + if (inertia.rows() != inertia_->rows() || inertia.cols() != inertia_->columns()) { RCLCPP_ERROR( LOGGER, "The size of the inertia matrix (%zu, %zu) does not match the required size of (%u, %u)", @@ -433,8 +443,7 @@ bool DynamicsInterfaceKDL::verify_inertia( bool DynamicsInterfaceKDL::verify_coriolis(const Eigen::VectorXd & coriolis) { - if (static_cast(coriolis.size()) != num_joints_) - { + if (static_cast(coriolis.size()) != num_joints_) { RCLCPP_ERROR( LOGGER, "The size of the coriolis vector (%zu) does not match the required size of (%lu)", coriolis.size(), num_joints_); @@ -445,8 +454,7 @@ bool DynamicsInterfaceKDL::verify_coriolis(const Eigen::VectorXd & coriolis) bool DynamicsInterfaceKDL::verify_gravity(const Eigen::VectorXd & gravity) { - if (static_cast(gravity.size()) != num_joints_) - { + if (static_cast(gravity.size()) != num_joints_) { RCLCPP_ERROR( LOGGER, "The size of the gravity vector (%zu) does not match the required size of (%lu)", gravity.size(), num_joints_); diff --git a/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp b/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp index a5f9798..70f473a 100644 --- a/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp +++ b/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp @@ -64,7 +64,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); + std::string(ros2_control_test_assets::urdf_tail); rclcpp::Parameter param_urdf("robot_description", urdf); node_->declare_parameter("robot_description", ""); @@ -153,8 +153,7 @@ TEST_F(TestKDLPlugin, KDL_plugin_function) dyn_->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) - { + for (size_t i = 0; i < static_cast(delta_x.size()); ++i) { ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); } } @@ -208,8 +207,7 @@ TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector) dyn_->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) - { + for (size_t i = 0; i < static_cast(delta_x.size()); ++i) { ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); } } @@ -336,8 +334,7 @@ TEST_F(TestKDLPlugin, KDL_plugin_as_kinematics_interface_only) kyn_->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) - { + for (size_t i = 0; i < static_cast(delta_x.size()); ++i) { ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); } } From 8493635e6e82cd3a119917959eecc1f3444e2207 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 19:45:08 +0100 Subject: [PATCH 05/16] fix clang format --- .clang-format | 15 --------------- 1 file changed, 15 deletions(-) delete mode 100644 .clang-format diff --git a/.clang-format b/.clang-format deleted file mode 100644 index 9c21e1e..0000000 --- a/.clang-format +++ /dev/null @@ -1,15 +0,0 @@ ---- -Language: Cpp -BasedOnStyle: Google - -ColumnLimit: 100 -AccessModifierOffset: -2 -AlignAfterOpenBracket: AlwaysBreak -BreakBeforeBraces: Allman -ConstructorInitializerIndentWidth: 0 -ContinuationIndentWidth: 2 -DerivePointerAlignment: false -PointerAlignment: Middle -ReflowComments: false -IncludeBlocks: Preserve -... From d0725f7b59463cd107a20d2265845571354ffc89 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 19:57:38 +0100 Subject: [PATCH 06/16] change private DynamicsInterfaceKDL stuff to protected --- .../include/dynamics_interface_kdl/dynamics_interface_kdl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dynamics_interface_kdl/include/dynamics_interface_kdl/dynamics_interface_kdl.hpp b/dynamics_interface_kdl/include/dynamics_interface_kdl/dynamics_interface_kdl.hpp index d7fb5d9..ae1eb9d 100644 --- a/dynamics_interface_kdl/include/dynamics_interface_kdl/dynamics_interface_kdl.hpp +++ b/dynamics_interface_kdl/include/dynamics_interface_kdl/dynamics_interface_kdl.hpp @@ -89,7 +89,7 @@ class DynamicsInterfaceKDL : public dynamics_interface::DynamicsInterface const Eigen::VectorXd & joint_pos, const Eigen::VectorXd & delta_theta, const std::string & link_name, Eigen::Matrix & delta_x) override; -private: +protected: // verification methods bool verify_initialized(); bool verify_link_name(const std::string & link_name); From 0e4a56e4fb358962fc90cdcc5cb86d306112cdbd Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 19:57:56 +0100 Subject: [PATCH 07/16] use DynamicsInterfaceKDL as a base class for DynamicsInterfaceFd --- dynamics_interface_fd/CMakeLists.txt | 2 +- .../dynamics_interface_fd.hpp | 67 +-- dynamics_interface_fd/package.xml | 2 +- .../src/dynamics_interface_fd.cpp | 410 +----------------- .../test/test_dynamics_interface_fd.cpp | 24 +- 5 files changed, 34 insertions(+), 471 deletions(-) diff --git a/dynamics_interface_fd/CMakeLists.txt b/dynamics_interface_fd/CMakeLists.txt index 04730fb..cfbc472 100644 --- a/dynamics_interface_fd/CMakeLists.txt +++ b/dynamics_interface_fd/CMakeLists.txt @@ -7,7 +7,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS kdl_parser - dynamics_interface + dynamics_interface_kdl pluginlib tf2_eigen_kdl realtime_tools diff --git a/dynamics_interface_fd/include/dynamics_interface_fd/dynamics_interface_fd.hpp b/dynamics_interface_fd/include/dynamics_interface_fd/dynamics_interface_fd.hpp index 4fda026..8f6cde4 100644 --- a/dynamics_interface_fd/include/dynamics_interface_fd/dynamics_interface_fd.hpp +++ b/dynamics_interface_fd/include/dynamics_interface_fd/dynamics_interface_fd.hpp @@ -24,19 +24,8 @@ #include #include -#include "dynamics_interface/dynamics_interface.hpp" +#include "dynamics_interface_kdl/dynamics_interface_kdl.hpp" -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" - -#include "kdl/chaindynparam.hpp" -#include "kdl/chainfksolverpos_recursive.hpp" -#include "kdl/chainfksolvervel_recursive.hpp" -#include "kdl/chainjnttojacdotsolver.hpp" -#include "kdl/chainjnttojacsolver.hpp" -#include "kdl/treejnttojacsolver.hpp" -#include "kdl_parser/kdl_parser.hpp" -#include "tf2_eigen_kdl/tf2_eigen_kdl.hpp" #include #include "rclcpp/node_interfaces/node_parameters_interface.hpp" @@ -46,7 +35,7 @@ namespace dynamics_interface_fd { -class DynamicsInterfaceFd : public dynamics_interface::DynamicsInterface +class DynamicsInterfaceFd : public dynamics_interface_kdl::DynamicsInterfaceKDL { public: DynamicsInterfaceFd(); @@ -65,19 +54,6 @@ class DynamicsInterfaceFd : public dynamics_interface::DynamicsInterface std::shared_ptr parameters_interface, const std::string & param_namespace) override; - bool calculate_link_transform( - const Eigen::VectorXd & joint_pos, const std::string & link_name, - Eigen::Isometry3d & transform) override; - - bool calculate_jacobian( - const Eigen::VectorXd & joint_pos, const std::string & link_name, - Eigen::Matrix & jacobian) override; - - bool calculate_jacobian_derivative( - const Eigen::VectorXd & joint_pos, const Eigen::VectorXd & joint_vel, - const std::string & link_name, - Eigen::Matrix & jacobian_derivative) override; - bool calculate_inertia( const Eigen::VectorXd & joint_pos, Eigen::Matrix & inertia) override; @@ -88,37 +64,12 @@ class DynamicsInterfaceFd : public dynamics_interface::DynamicsInterface bool calculate_gravity(const Eigen::VectorXd & joint_pos, Eigen::VectorXd & gravity) override; - bool convert_cartesian_deltas_to_joint_deltas( - const Eigen::VectorXd & joint_pos, const Eigen::Matrix & delta_x, - const std::string & link_name, Eigen::VectorXd & delta_theta) override; - - bool convert_joint_deltas_to_cartesian_deltas( - const Eigen::VectorXd & joint_pos, const Eigen::VectorXd & delta_theta, - const std::string & link_name, Eigen::Matrix & delta_x) override; - -private: +protected: // methods bool fill_joints_if_missing( const Eigen::VectorXd & joint_values, Eigen::VectorXd & joint_values_filled); - // verification methods - bool verify_initialized(); - bool verify_link_name(const std::string & link_name); - bool verify_joint_vector(const Eigen::VectorXd & joint_vector); - bool verify_jacobian(const Eigen::Matrix & jacobian); - bool verify_inertia(const Eigen::Matrix & inertia); - bool verify_coriolis(const Eigen::VectorXd & coriolis); - bool verify_gravity(const Eigen::VectorXd & gravity); - - bool initialized = false; - std::string root_name_; - size_t num_joints_; - - std::shared_ptr parameters_interface_; - std::unordered_map link_name_map_; - double alpha; // damping term for Jacobian inverse std::string fd_inertia_topic_name_; - Eigen::MatrixXd I; // async node for inertia rclcpp::Node::SharedPtr async_node_; @@ -130,17 +81,7 @@ class DynamicsInterfaceFd : public dynamics_interface::DynamicsInterface // Incoming fd_inertia data realtime_tools::RealtimeBuffer> - rt_fd_inertia_subscriber_ptr_; - - // KDL - KDL::Chain chain_; - std::shared_ptr fk_pos_solver_; - KDL::JntArray q_, q_dot_; - KDL::JntArrayVel q_array_vel_; - KDL::Frame frame_; - std::shared_ptr jacobian_, jacobian_derivative_; - std::shared_ptr jac_solver_; - std::shared_ptr jac_dot_solver_; + rt_fd_inertia_subscriber_ptr_; }; } // namespace dynamics_interface_fd diff --git a/dynamics_interface_fd/package.xml b/dynamics_interface_fd/package.xml index 6c73428..97b04a6 100644 --- a/dynamics_interface_fd/package.xml +++ b/dynamics_interface_fd/package.xml @@ -13,7 +13,7 @@ eigen kdl_parser - dynamics_interface + dynamics_interface_kdl pluginlib tf2_eigen_kdl realtime_tools diff --git a/dynamics_interface_fd/src/dynamics_interface_fd.cpp b/dynamics_interface_fd/src/dynamics_interface_fd.cpp index ef72b38..71f76af 100644 --- a/dynamics_interface_fd/src/dynamics_interface_fd.cpp +++ b/dynamics_interface_fd/src/dynamics_interface_fd.cpp @@ -28,10 +28,8 @@ rclcpp::Logger LOGGER = rclcpp::get_logger("dynamics_interface_fd"); bool fromMsg(const std_msgs::msg::Float64MultiArray & m, Eigen::Matrix & e) { int ii = 0; - for (int i = 0; i < e.rows(); ++i) - { - for (int j = 0; j < e.cols(); ++j) - { + for (int i = 0; i < e.rows(); ++i) { + for (int j = 0; j < e.cols(); ++j) { e(i, j) = m.data[ii++]; } } @@ -39,9 +37,9 @@ bool fromMsg(const std_msgs::msg::Float64MultiArray & m, Eigen::Matrixjoin(); node_thread_.reset(); async_node_.reset(); } + // Call the destructor of the base class + DynamicsInterfaceKDL::~DynamicsInterfaceKDL(); RCLCPP_INFO(LOGGER, "Successfully deactivated!"); } @@ -67,113 +66,10 @@ bool DynamicsInterfaceFd::initialize( // track initialization plugin initialized = true; - // get parameters - std::string ns = !param_namespace.empty() ? param_namespace + "." : ""; - - std::string robot_description_local; - if (robot_description.empty()) - { - // 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; - } - - // get 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_param); - } - else - { - RCLCPP_ERROR(LOGGER, "Failed to find end effector name parameter [tip]."); + if (!DynamicsInterfaceKDL::initialize(robot_description, parameters_interface, param_namespace)) { + RCLCPP_ERROR(LOGGER, "Failed to initialize DynamicsInterfaceKDL"); 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); - - // get root name - auto base_param = rclcpp::Parameter(); - if (parameters_interface->has_parameter(ns + "base")) - { - parameters_interface->get_parameter(ns + "base", base_param); - root_name_ = base_param.as_string(); - } - else - { - 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(), - end_effector_name.c_str()); - return false; - } - - // get root name - auto fd_inertia_topic_name_param = rclcpp::Parameter(); - if (parameters_interface->has_parameter(ns + "fd_inertia_topic_name")) - { - parameters_interface->get_parameter(ns + "fd_inertia_topic_name", fd_inertia_topic_name_param); - fd_inertia_topic_name_ = fd_inertia_topic_name_param.as_string(); - } - else - { - fd_inertia_topic_name_ = "fd_inertia"; - } - - 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(), - end_effector_name.c_str()); - return false; - } - - // create map from link names to their index - for (size_t i = 0; i < chain_.getNrOfSegments(); ++i) - { - link_name_map_[chain_.getSegment(i).getName()] = i + 1; - } - - // get alpha damping term - auto alpha_param = rclcpp::Parameter("alpha", 0.000005); - if (parameters_interface->has_parameter(ns + "alpha")) - { - parameters_interface->get_parameter(ns + "alpha", alpha_param); - } - alpha = alpha_param.as_double(); - - // allocate dynamics memory - num_joints_ = chain_.getNrOfJoints(); - q_ = KDL::JntArray(num_joints_); - q_dot_ = KDL::JntArray(num_joints_); - q_array_vel_ = KDL::JntArrayVel(num_joints_); // container for for q AND q_dot_ - I = Eigen::MatrixXd(num_joints_, num_joints_); - I.setIdentity(); - - jacobian_ = std::make_shared(num_joints_); - jacobian_derivative_ = std::make_shared(num_joints_); - - // create KDL solvers - fk_pos_solver_ = std::make_shared(chain_); - jac_solver_ = std::make_shared(chain_); - jac_dot_solver_ = std::make_shared(chain_); // Setup internal inertia subscriber RCLCPP_INFO(LOGGER, "Setting up internal inertia subscriber... please wait..."); @@ -187,7 +83,7 @@ bool DynamicsInterfaceFd::initialize( fd_inertia_subscriber_ptr_ = async_node_->create_subscription( fd_inertia_topic_name_, rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Float64MultiArray::SharedPtr msg) - { rt_fd_inertia_subscriber_ptr_.writeFromNonRT(msg); }); + {rt_fd_inertia_subscriber_ptr_.writeFromNonRT(msg);}); node_thread_ = std::make_unique( [&]() @@ -206,21 +102,18 @@ bool DynamicsInterfaceFd::initialize( bool DynamicsInterfaceFd::fill_joints_if_missing( const Eigen::VectorXd & joint_values, Eigen::VectorXd & joint_values_filled) { - if (joint_values.size() == 0 || static_cast(joint_values.size()) > num_joints_) - { + if (joint_values.size() == 0 || static_cast(joint_values.size()) > num_joints_) { // TODO(tpoignonec) add warning return false; } - if (joint_values.size() != 3 && joint_values.size() != 6) - { + if (joint_values.size() != 3 && joint_values.size() != 6) { // should be either 3 or 6! // TODO(tpoignonec) add warning return false; } - if (static_cast(joint_values.size()) == num_joints_) - { + if (static_cast(joint_values.size()) == num_joints_) { joint_values_filled = joint_values; return true; } @@ -230,98 +123,6 @@ bool DynamicsInterfaceFd::fill_joints_if_missing( return true; } -// FK, Jacobian, and Jacobian time derivative -// -------------------------------------------------------------------- - -bool DynamicsInterfaceFd::calculate_link_transform( - const Eigen::Matrix & joint_pos, const std::string & link_name, - Eigen::Isometry3d & transform) -{ - // verify inputs - if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name)) - { - return false; - } - - // get joint array - if (!fill_joints_if_missing(joint_pos, q_.data)) - { - return false; - } - - // reset transform_vec - transform.setIdentity(); - - // special case: since the root is not in the robot tree, need to return identity transform - if (link_name == root_name_) - { - return true; - } - - // create forward kinematics solver - fk_pos_solver_->JntToCart(q_, frame_, link_name_map_[link_name]); - tf2::transformKDLToEigen(frame_, transform); - return true; -} - -bool DynamicsInterfaceFd::calculate_jacobian( - const Eigen::Matrix & joint_pos, const std::string & link_name, - Eigen::Matrix & jacobian) -{ - // verify inputs - if ( - !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || - !verify_jacobian(jacobian)) - { - return false; - } - - // get joint array - if (!fill_joints_if_missing(joint_pos, q_.data)) - { - return false; - } - - // calculate Jacobian - jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]); - jacobian = jacobian_->data; - - return true; -} - -bool DynamicsInterfaceFd::calculate_jacobian_derivative( - const Eigen::Matrix & joint_pos, - const Eigen::Matrix & joint_vel, const std::string & link_name, - Eigen::Matrix & jacobian_derivative) -{ - // verify inputs - if ( - !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_joint_vector(joint_vel) || - !verify_link_name(link_name) || !verify_jacobian(jacobian_derivative)) - { - return false; - } - - // get joint array - if (!fill_joints_if_missing(joint_pos, q_.data)) - { - return false; - } - if (!fill_joints_if_missing(joint_vel, q_dot_.data)) - { - return false; - } - - q_array_vel_.q = q_; - q_array_vel_.qdot = q_dot_; - - // calculate Jacobian - jac_dot_solver_->JntToJacDot(q_array_vel_, *jacobian_derivative_, link_name_map_[link_name]); - jacobian_derivative = jacobian_derivative_->data; - - return true; -} - // Dynamics // -------------------------------------------------------------------- @@ -330,8 +131,7 @@ bool DynamicsInterfaceFd::calculate_inertia( Eigen::Matrix & inertia) { // verify inputs - if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_inertia(inertia)) - { + if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_inertia(inertia)) { return false; } @@ -339,15 +139,13 @@ bool DynamicsInterfaceFd::calculate_inertia( fd_inertia_msg_ = *rt_fd_inertia_subscriber_ptr_.readFromRT(); // if message exists, load values into references - if (!fd_inertia_msg_.get()) - { + if (!fd_inertia_msg_.get()) { RCLCPP_ERROR(LOGGER, "No message received from fd_inertia real-time subscriber."); return false; } // check dimension of message - if (fd_inertia_msg_->data.size() != static_cast(36)) - { + if (fd_inertia_msg_->data.size() != static_cast(36)) { RCLCPP_ERROR( LOGGER, "The size of the inertia matrix msg (%zu) does not match the required size of (6 x 6)", @@ -357,16 +155,11 @@ bool DynamicsInterfaceFd::calculate_inertia( fromMsg(*fd_inertia_msg_, fd_inertia_); // load values from message - if (num_joints_ == 3) - { + if (num_joints_ == 3) { inertia = fd_inertia_.block<3, 3>(0, 0); - } - else if (num_joints_ == 6) - { + } else if (num_joints_ == 6) { inertia = fd_inertia_; - } - else - { + } else { RCLCPP_ERROR(LOGGER, "Invalid number of joints (%lu).", num_joints_); return false; } @@ -393,8 +186,7 @@ bool DynamicsInterfaceFd::calculate_gravity( const Eigen::VectorXd & joint_pos, Eigen::VectorXd & gravity) { // verify inputs - if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_gravity(gravity)) - { + if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_gravity(gravity)) { return false; } @@ -403,168 +195,6 @@ bool DynamicsInterfaceFd::calculate_gravity( return true; } -// Kinematics -// -------------------------------------------------------------------- - -bool DynamicsInterfaceFd::convert_joint_deltas_to_cartesian_deltas( - const Eigen::Matrix & joint_pos, - const Eigen::Matrix & delta_theta, const std::string & link_name, - Eigen::Matrix & delta_x) -{ - // verify inputs - if ( - !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || - !verify_joint_vector(delta_theta)) - { - return false; - } - - // get joint array - if (!fill_joints_if_missing(joint_pos, q_.data)) - { - return false; - } - - // calculate Jacobian - jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]); - delta_x = jacobian_->data * delta_theta; - - return true; -} - -bool DynamicsInterfaceFd::convert_cartesian_deltas_to_joint_deltas( - const Eigen::Matrix & joint_pos, - const Eigen::Matrix & delta_x, const std::string & link_name, - Eigen::Matrix & delta_theta) -{ - // verify inputs - if ( - !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || - !verify_joint_vector(delta_theta)) - { - return false; - } - - // get joint array - if (!fill_joints_if_missing(joint_pos, q_.data)) - { - return false; - } - - // calculate Jacobian - jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]); - // TODO(anyone): this dynamic allocation needs to be replaced - Eigen::Matrix J = jacobian_->data; - // damped inverse - Eigen::Matrix J_inverse = - (J.transpose() * J + alpha * I).inverse() * J.transpose(); - delta_theta = J_inverse * delta_x; - - return true; -} - -// Verification functions -// -------------------------------------------------------------------- - -bool DynamicsInterfaceFd::verify_link_name(const std::string & link_name) -{ - if (link_name == root_name_) - { - return true; - } - if (link_name_map_.find(link_name) == link_name_map_.end()) - { - std::string links; - for (size_t i = 0; i < chain_.getNrOfSegments(); ++i) - { - links += "\n" + chain_.getSegment(i).getName(); - } - RCLCPP_ERROR( - LOGGER, "The link %s was not found in the robot chain. Available links are: %s", - link_name.c_str(), links.c_str()); - return false; - } - return true; -} - -bool DynamicsInterfaceFd::verify_joint_vector(const Eigen::VectorXd & joint_vector) -{ - if (static_cast(joint_vector.size()) != num_joints_) - { - RCLCPP_ERROR( - LOGGER, "Invalid joint vector size (%zu). Expected size is %zu.", joint_vector.size(), - num_joints_); - return false; - } - return true; -} - -bool DynamicsInterfaceFd::verify_initialized() -{ - // check if interface is initialized - if (!initialized) - { - RCLCPP_ERROR( - LOGGER, - "The KDL kinematics plugin was not initialized. Ensure you called the initialize method."); - return false; - } - return true; -} - -bool DynamicsInterfaceFd::verify_jacobian(const Eigen::Matrix & jacobian) -{ - if (jacobian.rows() != jacobian_->rows() || jacobian.cols() != jacobian_->columns()) - { - RCLCPP_ERROR( - LOGGER, "The size of the jacobian (%zu, %zu) does not match the required size of (%u, %u)", - jacobian.rows(), jacobian.cols(), jacobian_->rows(), jacobian_->columns()); - return false; - } - return true; -} - -bool DynamicsInterfaceFd::verify_inertia( - const Eigen::Matrix & inertia) -{ - if ( - static_cast(inertia.rows()) != num_joints_ || - static_cast(inertia.cols()) != num_joints_) - { - RCLCPP_ERROR( - LOGGER, - "The size of the inertia matrix (%zu, %zu) does not match the required size of (%lu, %lu)", - inertia.rows(), inertia.cols(), num_joints_, num_joints_); - return false; - } - return true; -} - -bool DynamicsInterfaceFd::verify_coriolis(const Eigen::VectorXd & coriolis) -{ - if (static_cast(coriolis.size()) != num_joints_) - { - RCLCPP_ERROR( - LOGGER, "The size of the coriolis vector (%zu) does not match the required size of (%lu)", - coriolis.size(), num_joints_); - return false; - } - return true; -} - -bool DynamicsInterfaceFd::verify_gravity(const Eigen::VectorXd & gravity) -{ - if (static_cast(gravity.size()) != num_joints_) - { - RCLCPP_ERROR( - LOGGER, "The size of the gravity vector (%zu) does not match the required size of (%lu)", - gravity.size(), num_joints_); - return false; - } - - return true; -} - } // namespace dynamics_interface_fd #include "pluginlib/class_list_macros.hpp" diff --git a/dynamics_interface_fd/test/test_dynamics_interface_fd.cpp b/dynamics_interface_fd/test/test_dynamics_interface_fd.cpp index 4b9205f..bb99d69 100644 --- a/dynamics_interface_fd/test/test_dynamics_interface_fd.cpp +++ b/dynamics_interface_fd/test/test_dynamics_interface_fd.cpp @@ -75,10 +75,8 @@ class TestDynamicsFdPlugin : public ::testing::Test { // wait for subscriber size_t wait_count = 0; - while (node_->count_subscribers(fd_inertia_topic_name) == 0) - { - if (wait_count >= 5) - { + while (node_->count_subscribers(fd_inertia_topic_name) == 0) { + if (wait_count >= 5) { auto error_msg = std::string("publishing to ") + fd_inertia_topic_name + " but no node subscribes to it"; throw std::runtime_error(error_msg); @@ -90,10 +88,8 @@ class TestDynamicsFdPlugin : public ::testing::Test // publish std_msgs::msg::Float64MultiArray msg; msg.data.resize(36); - for (size_t i = 0; i < 6; i++) - { - for (size_t j = 0; j < 6; j++) - { + for (size_t i = 0; i < 6; i++) { + for (size_t j = 0; j < 6; j++) { msg.data[i * 6 + j] = inertia(i, j); } } @@ -214,8 +210,7 @@ TEST_F(TestDynamicsFdPlugin, FD_plugin_function_with_omega6_urdf) dyn_->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) - { + for (size_t i = 0; i < static_cast(delta_x.size()); ++i) { ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); } RCLCPP_INFO(node_->get_logger(), "All good so far..."); @@ -293,8 +288,7 @@ TEST_F(TestDynamicsFdPlugin, FD_plugin_function_with_omega3_urdf) dyn_->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) - { + for (size_t i = 0; i < static_cast(delta_x.size()); ++i) { ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); } } @@ -352,8 +346,7 @@ TEST_F(TestDynamicsFdPlugin, FD_plugin_function_std_vector) dyn_->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) - { + for (size_t i = 0; i < static_cast(delta_x.size()); ++i) { ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); } } @@ -490,8 +483,7 @@ TEST_F(TestDynamicsFdPlugin, FD_plugin_as_kinematics_interface_only) kyn_->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) - { + for (size_t i = 0; i < static_cast(delta_x.size()); ++i) { ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); } } From 31e7e1f2243e3f8fecd00498612922e4ef7905f8 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 20:23:50 +0100 Subject: [PATCH 08/16] fix KDL_plugin_no_robot_description test --- dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp b/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp index 70f473a..f5b890c 100644 --- a/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp +++ b/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp @@ -259,6 +259,7 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes) TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description) { // load alpha to parameter server + loadURDFParameter(); loadAlphaParameter(); loadEndEffectorNameParameter(end_effector_); loadGravityParameter(); From 912674d81a01de37b5a66cfa952c1cdb2b1b9443 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 20:24:01 +0100 Subject: [PATCH 09/16] fix KDL_plugin_no_gravity test --- dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp b/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp index f5b890c..9adddc0 100644 --- a/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp +++ b/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp @@ -294,7 +294,7 @@ TEST_F(TestKDLPlugin, KDL_plugin_no_gravity) ASSERT_TRUE(node_->get_parameter("robot_description", robot_param)); auto robot_description_str = robot_param.as_string(); - ASSERT_TRUE( + ASSERT_FALSE( dyn_->initialize(robot_description_str, node_->get_node_parameters_interface(), "dynamics")); } From ec5d63249c79a2006bc5fbfcb475e4c00d390fcc Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 20:24:14 +0100 Subject: [PATCH 10/16] add calculate_jacobian_inverse test --- .../test/test_dynamics_interface_kdl.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp b/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp index 9adddc0..e25541f 100644 --- a/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp +++ b/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp @@ -321,6 +321,24 @@ TEST_F(TestKDLPlugin, KDL_plugin_as_kinematics_interface_only) // calculate jacobian Eigen::Matrix jacobian = Eigen::Matrix::Zero(); ASSERT_TRUE(kyn_->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(kyn_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); + + // ensure jacobian inverse math is correct + for (size_t i = 0; i < static_cast(jacobian_inverse.rows()); ++i) + { + for (size_t j = 0; j < static_cast(jacobian_inverse.cols()); ++j) + { + ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02); + } + } // convert cartesian delta to joint delta Eigen::Matrix delta_x = Eigen::Matrix::Zero(); From d5d144e295c321af49bf411488132e50008b874b Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 20:47:51 +0100 Subject: [PATCH 11/16] add missing alpha initialization --- dynamics_interface_kdl/src/dynamics_interface_kdl.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/dynamics_interface_kdl/src/dynamics_interface_kdl.cpp b/dynamics_interface_kdl/src/dynamics_interface_kdl.cpp index ec785e0..ce05d54 100644 --- a/dynamics_interface_kdl/src/dynamics_interface_kdl.cpp +++ b/dynamics_interface_kdl/src/dynamics_interface_kdl.cpp @@ -46,6 +46,14 @@ bool DynamicsInterfaceKDL::initialize( } else { robot_description_local = robot_description; } + + // get alpha damping term + auto alpha_param = rclcpp::Parameter("alpha", 0.000005); + if (parameters_interface->has_parameter(ns + "alpha")) + { + parameters_interface->get_parameter(ns + "alpha", alpha_param); + } + alpha = alpha_param.as_double(); // get end-effector name auto end_effector_name_param = rclcpp::Parameter("tip"); From 9be34ebc457787e32f481d409972225676b73182 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 20:52:42 +0100 Subject: [PATCH 12/16] fix deleted fd_inertia_topic_name initialization --- .../src/dynamics_interface_fd.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/dynamics_interface_fd/src/dynamics_interface_fd.cpp b/dynamics_interface_fd/src/dynamics_interface_fd.cpp index 71f76af..f26593c 100644 --- a/dynamics_interface_fd/src/dynamics_interface_fd.cpp +++ b/dynamics_interface_fd/src/dynamics_interface_fd.cpp @@ -71,6 +71,21 @@ bool DynamicsInterfaceFd::initialize( return false; } + // get parameters + std::string ns = !param_namespace.empty() ? param_namespace + "." : ""; + + // get inertia topic name + auto fd_inertia_topic_name_param = rclcpp::Parameter(); + if (parameters_interface->has_parameter(ns + "fd_inertia_topic_name")) + { + parameters_interface->get_parameter(ns + "fd_inertia_topic_name", fd_inertia_topic_name_param); + fd_inertia_topic_name_ = fd_inertia_topic_name_param.as_string(); + } + else + { + fd_inertia_topic_name_ = "fd_inertia"; + } + // Setup internal inertia subscriber RCLCPP_INFO(LOGGER, "Setting up internal inertia subscriber... please wait..."); rclcpp::NodeOptions options; From 8715b0eb2054137e0cda3f43a11629e1a1c8dc05 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 21:00:46 +0100 Subject: [PATCH 13/16] fix DynamicsInterfaceFd::initialize() --- dynamics_interface_fd/src/dynamics_interface_fd.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/dynamics_interface_fd/src/dynamics_interface_fd.cpp b/dynamics_interface_fd/src/dynamics_interface_fd.cpp index f26593c..2fe0d09 100644 --- a/dynamics_interface_fd/src/dynamics_interface_fd.cpp +++ b/dynamics_interface_fd/src/dynamics_interface_fd.cpp @@ -63,9 +63,6 @@ bool DynamicsInterfaceFd::initialize( std::shared_ptr parameters_interface, const std::string & param_namespace) { - // track initialization plugin - initialized = true; - if (!DynamicsInterfaceKDL::initialize(robot_description, parameters_interface, param_namespace)) { RCLCPP_ERROR(LOGGER, "Failed to initialize DynamicsInterfaceKDL"); return false; From b7112ba29b16057e484b7101084aa91ebbacde89 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 21:05:05 +0100 Subject: [PATCH 14/16] fix "double free or corruption (!prev)" error --- dynamics_interface_fd/src/dynamics_interface_fd.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/dynamics_interface_fd/src/dynamics_interface_fd.cpp b/dynamics_interface_fd/src/dynamics_interface_fd.cpp index 2fe0d09..f1efaaa 100644 --- a/dynamics_interface_fd/src/dynamics_interface_fd.cpp +++ b/dynamics_interface_fd/src/dynamics_interface_fd.cpp @@ -53,8 +53,6 @@ DynamicsInterfaceFd::~DynamicsInterfaceFd() node_thread_.reset(); async_node_.reset(); } - // Call the destructor of the base class - DynamicsInterfaceKDL::~DynamicsInterfaceKDL(); RCLCPP_INFO(LOGGER, "Successfully deactivated!"); } From cca5714fd8c6e82b508dae94e01a1ec19a9594f3 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 21:06:37 +0100 Subject: [PATCH 15/16] fix broken tests --- .../test/test_dynamics_interface_fd.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/dynamics_interface_fd/test/test_dynamics_interface_fd.cpp b/dynamics_interface_fd/test/test_dynamics_interface_fd.cpp index bb99d69..0fe5a05 100644 --- a/dynamics_interface_fd/test/test_dynamics_interface_fd.cpp +++ b/dynamics_interface_fd/test/test_dynamics_interface_fd.cpp @@ -151,6 +151,9 @@ TEST_F(TestDynamicsFdPlugin, FD_plugin_function_with_omega6_urdf) // initialize the plugin auto robot_param = rclcpp::Parameter(); ASSERT_TRUE(node_->get_parameter("robot_description", robot_param)); + auto robot_description_str = robot_param.as_string(); + ASSERT_TRUE( + dyn_->initialize(robot_description_str, node_->get_node_parameters_interface(), "dynamics")); RCLCPP_INFO(node_->get_logger(), "Plugin instantiated successfully!"); // publish inertia matrix @@ -229,6 +232,9 @@ TEST_F(TestDynamicsFdPlugin, FD_plugin_function_with_omega3_urdf) // initialize the plugin auto robot_param = rclcpp::Parameter(); ASSERT_TRUE(node_->get_parameter("robot_description", robot_param)); + auto robot_description_str = robot_param.as_string(); + ASSERT_TRUE( + dyn_->initialize(robot_description_str, node_->get_node_parameters_interface(), "dynamics")); RCLCPP_INFO(node_->get_logger(), "Plugin instantiated successfully!"); // publish inertia matrix @@ -301,6 +307,10 @@ TEST_F(TestDynamicsFdPlugin, FD_plugin_function_std_vector) // initialize the plugin auto robot_param = rclcpp::Parameter(); ASSERT_TRUE(node_->get_parameter("robot_description", robot_param)); + auto robot_description_str = robot_param.as_string(); + ASSERT_TRUE( + dyn_->initialize(robot_description_str, node_->get_node_parameters_interface(), "dynamics")); + RCLCPP_INFO(node_->get_logger(), "Plugin instantiated successfully!"); // publish inertia matrix Eigen::Matrix mock_inertia = 2 * Eigen::Matrix::Identity(); @@ -361,6 +371,9 @@ TEST_F(TestDynamicsFdPlugin, no_inertia_published) // initialize the plugin auto robot_param = rclcpp::Parameter(); ASSERT_TRUE(node_->get_parameter("robot_description", robot_param)); + auto robot_description_str = robot_param.as_string(); + ASSERT_TRUE( + dyn_->initialize(robot_description_str, node_->get_node_parameters_interface(), "dynamics")); RCLCPP_INFO(node_->get_logger(), "Plugin instantiated successfully!"); // dummy joint position and velocity @@ -381,9 +394,9 @@ TEST_F(TestDynamicsFdPlugin, incorrect_input_sizes) auto robot_param = rclcpp::Parameter(); ASSERT_TRUE(node_->get_parameter("robot_description", robot_param)); auto robot_description_str = robot_param.as_string(); - ASSERT_TRUE( dyn_->initialize(robot_description_str, node_->get_node_parameters_interface(), "dynamics")); + RCLCPP_INFO(node_->get_logger(), "Plugin instantiated successfully!"); // define correct values Eigen::Matrix pos = From fee24f64b1f13fb007cfa5e47e28dc9940d6113a Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 21:06:50 +0100 Subject: [PATCH 16/16] linting --- dynamics_interface_fd/src/dynamics_interface_fd.cpp | 7 ++----- .../src/dynamics_interface_kdl.cpp | 5 ++--- .../test/test_dynamics_interface_kdl.cpp | 12 +++++------- 3 files changed, 9 insertions(+), 15 deletions(-) diff --git a/dynamics_interface_fd/src/dynamics_interface_fd.cpp b/dynamics_interface_fd/src/dynamics_interface_fd.cpp index f1efaaa..0b197e5 100644 --- a/dynamics_interface_fd/src/dynamics_interface_fd.cpp +++ b/dynamics_interface_fd/src/dynamics_interface_fd.cpp @@ -71,13 +71,10 @@ bool DynamicsInterfaceFd::initialize( // get inertia topic name auto fd_inertia_topic_name_param = rclcpp::Parameter(); - if (parameters_interface->has_parameter(ns + "fd_inertia_topic_name")) - { + if (parameters_interface->has_parameter(ns + "fd_inertia_topic_name")) { parameters_interface->get_parameter(ns + "fd_inertia_topic_name", fd_inertia_topic_name_param); fd_inertia_topic_name_ = fd_inertia_topic_name_param.as_string(); - } - else - { + } else { fd_inertia_topic_name_ = "fd_inertia"; } diff --git a/dynamics_interface_kdl/src/dynamics_interface_kdl.cpp b/dynamics_interface_kdl/src/dynamics_interface_kdl.cpp index ce05d54..f122012 100644 --- a/dynamics_interface_kdl/src/dynamics_interface_kdl.cpp +++ b/dynamics_interface_kdl/src/dynamics_interface_kdl.cpp @@ -46,11 +46,10 @@ bool DynamicsInterfaceKDL::initialize( } else { robot_description_local = robot_description; } - + // get alpha damping term auto alpha_param = rclcpp::Parameter("alpha", 0.000005); - if (parameters_interface->has_parameter(ns + "alpha")) - { + if (parameters_interface->has_parameter(ns + "alpha")) { parameters_interface->get_parameter(ns + "alpha", alpha_param); } alpha = alpha_param.as_double(); diff --git a/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp b/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp index e25541f..936d501 100644 --- a/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp +++ b/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp @@ -321,21 +321,19 @@ TEST_F(TestKDLPlugin, KDL_plugin_as_kinematics_interface_only) // calculate jacobian Eigen::Matrix jacobian = Eigen::Matrix::Zero(); ASSERT_TRUE(kyn_->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(kyn_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est)); // ensure jacobian inverse math is correct - for (size_t i = 0; i < static_cast(jacobian_inverse.rows()); ++i) - { - for (size_t j = 0; j < static_cast(jacobian_inverse.cols()); ++j) - { + for (size_t i = 0; i < static_cast(jacobian_inverse.rows()); ++i) { + for (size_t j = 0; j < static_cast(jacobian_inverse.cols()); ++j) { ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02); } }