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)