From fa0a67fdda0ad88844a9e60802e2ae194dbad4f0 Mon Sep 17 00:00:00 2001 From: francescodonofrio <116165932+francescodonofrio@users.noreply.github.com> Date: Thu, 14 Nov 2024 18:16:34 +0100 Subject: [PATCH 1/2] Add methods for computing frame differences --- .../kinematics_interface.hpp | 16 +++++++++ .../src/kinematics_interface.cpp | 33 +++++++++++++++++++ 2 files changed, 49 insertions(+) diff --git a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp index 9e4f10c..de5d47c 100644 --- a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp +++ b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp @@ -95,6 +95,18 @@ class KinematicsInterface const Eigen::VectorXd & joint_pos, const std::string & link_name, Eigen::Matrix & jacobian) = 0; + /** + * \brief Calculates the difference between two Cartesian frames + * \param[in] x_a first Cartesian frame (x, y, z, wx, wy, wz, ww) + * \param[in] x_b second Cartesian frame (x, y, z, wx, wy, wz, ww) + * \param[in] dt time interval over which the numerical differentiation takes place + * \param[out] delta_x Cartesian deltas (x, y, z, wx, wy, wz) + * \return true if successful + */ + virtual bool calculate_frame_difference( + Eigen::Matrix & x_a, Eigen::Matrix & x_b, double dt, + Eigen::Matrix & delta_x) = 0; + bool convert_cartesian_deltas_to_joint_deltas( std::vector & joint_pos_vec, const std::vector & delta_x_vec, const std::string & link_name, std::vector & delta_theta_vec); @@ -110,6 +122,10 @@ class KinematicsInterface bool calculate_jacobian( const std::vector & joint_pos_vec, const std::string & link_name, Eigen::Matrix & jacobian); + + bool calculate_frame_difference( + std::vector & x_a_vec, std::vector & x_b_vec, double dt, + std::vector & delta_x_vec); }; } // namespace kinematics_interface diff --git a/kinematics_interface/src/kinematics_interface.cpp b/kinematics_interface/src/kinematics_interface.cpp index a3b94b5..d392fb1 100644 --- a/kinematics_interface/src/kinematics_interface.cpp +++ b/kinematics_interface/src/kinematics_interface.cpp @@ -80,4 +80,37 @@ bool KinematicsInterface::calculate_jacobian( return calculate_jacobian(joint_pos, link_name, jacobian); } +bool KinematicsInterface::calculate_frame_difference( + std::vector & x_a_vec, std::vector & x_b_vec, double dt, + std::vector & delta_x_vec) +{ + if (x_a_vec.size() != 7) + { + RCLCPP_ERROR( + LOGGER, "The length of the first cartesian vector (%zu) must be 7.", x_a_vec.size()); + return false; + } + Eigen::Matrix x_a(x_a_vec.data()); + if (x_b_vec.size() != 7) + { + RCLCPP_ERROR( + LOGGER, "The length of the second cartesian vector (%zu) must be 7.", x_b_vec.size()); + return false; + } + Eigen::Matrix x_b(x_b_vec.data()); + if (delta_x_vec.size() != 6) + { + RCLCPP_ERROR( + LOGGER, "The length of the cartesian delta vector (%zu) must be 6.", delta_x_vec.size()); + return false; + } + Eigen::Matrix delta_x(delta_x_vec.data()); + bool ret = calculate_frame_difference(x_a, x_b, dt, delta_x); + for (auto i = 0ul; i < delta_x_vec.size(); i++) + { + delta_x_vec[i] = delta_x[i]; + } + return ret; +} + } // namespace kinematics_interface From 210a57faff329d26decc567dfda647552d6348f3 Mon Sep 17 00:00:00 2001 From: francescodonofrio <116165932+francescodonofrio@users.noreply.github.com> Date: Thu, 14 Nov 2024 18:17:50 +0100 Subject: [PATCH 2/2] Add KDL implementation for computing frame differences --- .../kinematics_interface_kdl.hpp | 8 +++- .../src/kinematics_interface_kdl.cpp | 40 ++++++++++++++++++- .../test/test_kinematics_interface_kdl.cpp | 37 +++++++++++++++++ 3 files changed, 82 insertions(+), 3 deletions(-) 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 645a5fa..ec9c1e3 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 @@ -60,12 +60,17 @@ class KinematicsInterfaceKDL : public kinematics_interface::KinematicsInterface const Eigen::VectorXd & joint_pos, const std::string & link_name, Eigen::Matrix & jacobian) override; + bool calculate_frame_difference( + Eigen::Matrix & x_a, Eigen::Matrix & x_b, double dt, + Eigen::Matrix & delta_x) override; + private: // 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_period(const double dt); bool initialized = false; std::string root_name_; @@ -73,7 +78,8 @@ class KinematicsInterfaceKDL : public kinematics_interface::KinematicsInterface KDL::Chain chain_; std::shared_ptr fk_pos_solver_; KDL::JntArray q_; - KDL::Frame frame_; + Eigen::Matrix frames_; + KDL::Twist delta_x_; std::shared_ptr jacobian_; std::shared_ptr jac_solver_; std::shared_ptr parameters_interface_; diff --git a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp index 8c86d00..a8f4e10 100644 --- a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp @@ -205,8 +205,34 @@ bool KinematicsInterfaceKDL::calculate_link_transform( } // create forward kinematics solver - fk_pos_solver_->JntToCart(q_, frame_, link_name_map_[link_name]); - tf2::transformKDLToEigen(frame_, transform); + fk_pos_solver_->JntToCart(q_, frames_(0), link_name_map_[link_name]); + tf2::transformKDLToEigen(frames_(0), transform); + return true; +} + +bool KinematicsInterfaceKDL::calculate_frame_difference( + Eigen::Matrix & x_a, Eigen::Matrix & x_b, double dt, + Eigen::Matrix & delta_x) +{ + // verify inputs + if (!verify_initialized() || !verify_period(dt)) + { + return false; + } + + // get frames + frames_(0) = KDL::Frame( + KDL::Rotation::Quaternion(x_a(3), x_a(4), x_a(5), x_a(6)), KDL::Vector(x_a(0), x_a(1), x_a(2))); + frames_(1) = KDL::Frame( + KDL::Rotation::Quaternion(x_b(3), x_b(4), x_b(5), x_b(6)), KDL::Vector(x_b(0), x_b(1), x_b(2))); + + // compute the difference + delta_x_ = KDL::diff(frames_(0), frames_(1), dt); + for (size_t i = 0; i < 6; ++i) + { + delta_x(i) = delta_x_[i]; + } + return true; } @@ -269,6 +295,16 @@ bool KinematicsInterfaceKDL::verify_jacobian( return true; } +bool KinematicsInterfaceKDL::verify_period(const double dt) +{ + if (dt < 0) + { + RCLCPP_ERROR(LOGGER, "The period (%f) must be a non-negative number", dt); + return false; + } + return true; +} + } // namespace kinematics_interface_kdl #include "pluginlib/class_list_macros.hpp" diff --git a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp index 9176c3a..bb75bd0 100644 --- a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp @@ -108,6 +108,21 @@ TEST_F(TestKDLPlugin, KDL_plugin_function) { ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); } + + // compute the difference between two cartesian frames + Eigen::Matrix x_a, x_b; + x_a << 0, 1, 0, 0, 0, 0, 1; + x_b << 2, 3, 0, 0, 1, 0, 0; + double dt = 1.0; + delta_x = Eigen::Matrix::Zero(); + delta_x_est << 2, 2, 0, 0, 3.14, 0; + ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x)); + + // ensure that difference math is correct + for (size_t i = 0; i < static_cast(delta_x.size()); ++i) + { + ASSERT_NEAR(delta_x(i), delta_x_est(i), 0.02); + } } TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector) @@ -142,6 +157,21 @@ TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector) { ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); } + + // compute the difference between two cartesian frames + std::vector x_a(7), x_b(7); + x_a = {0, 1, 0, 0, 0, 0, 1}; + x_b = {2, 3, 0, 0, 1, 0, 0}; + double dt = 1.0; + delta_x = {0, 0, 0, 0, 0, 0}; + delta_x_est = {2, 2, 0, 0, 3.14, 0}; + ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x)); + + // ensure that difference math is correct + for (size_t i = 0; i < static_cast(delta_x.size()); ++i) + { + ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02); + } } TEST_F(TestKDLPlugin, incorrect_input_sizes) @@ -161,10 +191,14 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes) delta_x[2] = 1; Eigen::Matrix delta_theta = Eigen::Matrix::Zero(); Eigen::Matrix delta_x_est; + Eigen::Matrix x_a, x_b; // wrong size input vector Eigen::Matrix vec_5 = 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)); @@ -183,6 +217,9 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes) 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)); + + // 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)