From f1e54352f4243c7d5d42b0f940bc9e3e196a0718 Mon Sep 17 00:00:00 2001 From: francesco-donofrio Date: Sat, 21 Dec 2024 20:30:28 +0100 Subject: [PATCH] Calculate Jacobian Inverse (#92) --- .../kinematics_interface.hpp | 15 +++++ .../src/kinematics_interface.cpp | 9 +++ .../kinematics_interface_kdl.hpp | 6 ++ .../src/kinematics_interface_kdl.cpp | 59 +++++++++++++++---- .../test/test_kinematics_interface_kdl.cpp | 49 +++++++++++++++ 5 files changed, 128 insertions(+), 10 deletions(-) diff --git a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp index 9e4f10c..d7c301a 100644 --- a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp +++ b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp @@ -95,6 +95,17 @@ class KinematicsInterface const Eigen::VectorXd & joint_pos, const std::string & link_name, Eigen::Matrix & jacobian) = 0; + /** + * \brief Calculates the jacobian inverse for a specified link using provided joint positions. + * \param[in] joint_pos joint positions of the robot in radians + * \param[in] link_name the name of the link to find the transform for + * \param[out] jacobian_inverse Jacobian inverse matrix of the specified link in row major format. + * \return true if successful + */ + virtual bool calculate_jacobian_inverse( + const Eigen::VectorXd & joint_pos, const std::string & link_name, + Eigen::Matrix & jacobian_inverse) = 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 +121,10 @@ class KinematicsInterface bool calculate_jacobian( const std::vector & joint_pos_vec, const std::string & link_name, Eigen::Matrix & jacobian); + + bool calculate_jacobian_inverse( + const std::vector & joint_pos_vec, const std::string & link_name, + Eigen::Matrix & jacobian_inverse); }; } // namespace kinematics_interface diff --git a/kinematics_interface/src/kinematics_interface.cpp b/kinematics_interface/src/kinematics_interface.cpp index a3b94b5..917a612 100644 --- a/kinematics_interface/src/kinematics_interface.cpp +++ b/kinematics_interface/src/kinematics_interface.cpp @@ -80,4 +80,13 @@ bool KinematicsInterface::calculate_jacobian( return calculate_jacobian(joint_pos, link_name, jacobian); } +bool KinematicsInterface::calculate_jacobian_inverse( + const std::vector & joint_pos_vec, const std::string & link_name, + Eigen::Matrix & jacobian_inverse) +{ + auto joint_pos = Eigen::Map(joint_pos_vec.data(), joint_pos_vec.size()); + + return calculate_jacobian_inverse(joint_pos, link_name, jacobian_inverse); +} + } // namespace kinematics_interface 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..6a4852f 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_jacobian_inverse( + const Eigen::VectorXd & joint_pos, const std::string & link_name, + Eigen::Matrix & jacobian_inverse) 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_jacobian_inverse(const Eigen::Matrix & jacobian); bool initialized = false; std::string root_name_; @@ -75,6 +80,7 @@ class KinematicsInterfaceKDL : public kinematics_interface::KinematicsInterface KDL::JntArray q_; KDL::Frame frame_; std::shared_ptr jacobian_; + std::shared_ptr> jacobian_inverse_; std::shared_ptr jac_solver_; std::shared_ptr parameters_interface_; std::unordered_map link_name_map_; diff --git a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp index 8c86d00..64c84f6 100644 --- a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp @@ -105,6 +105,7 @@ bool KinematicsInterfaceKDL::initialize( fk_pos_solver_ = std::make_shared(chain_); jac_solver_ = std::make_shared(chain_); jacobian_ = std::make_shared(num_joints_); + jacobian_inverse_ = std::make_shared>(num_joints_, 6); return true; } @@ -145,17 +146,13 @@ bool KinematicsInterfaceKDL::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; } @@ -182,6 +179,34 @@ bool KinematicsInterfaceKDL::calculate_jacobian( return true; } +bool KinematicsInterfaceKDL::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 KinematicsInterfaceKDL::calculate_link_transform( const Eigen::Matrix & joint_pos, const std::string & link_name, Eigen::Isometry3d & transform) @@ -269,6 +294,20 @@ bool KinematicsInterfaceKDL::verify_jacobian( return true; } +bool KinematicsInterfaceKDL::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; +} + } // 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..442472c 100644 --- a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp @@ -108,6 +108,26 @@ TEST_F(TestKDLPlugin, KDL_plugin_function) { ASSERT_NEAR(delta_x[i], delta_x_est[i], 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 + 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); + } + } } TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector) @@ -142,6 +162,26 @@ TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector) { ASSERT_NEAR(delta_x[i], delta_x_est[i], 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 + 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); + } + } } TEST_F(TestKDLPlugin, incorrect_input_sizes) @@ -161,10 +201,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 jacobian = Eigen::Matrix::Zero(); // wrong size input vector Eigen::Matrix vec_5 = Eigen::Matrix::Zero(); + // wrong size input jacobian + Eigen::Matrix mat_5_6 = Eigen::Matrix::Zero(); + // 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 +227,11 @@ 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)); + + // 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)); } TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description)