From 753504af05e1c140e87dbd6577aa66447044e095 Mon Sep 17 00:00:00 2001
From: francescodonofrio <116165932+francescodonofrio@users.noreply.github.com>
Date: Thu, 14 Nov 2024 17:51:25 +0100
Subject: [PATCH] Add the KDL implementation for the computation of the
 jacobian inverse

---
 .../kinematics_interface_kdl.hpp              |  5 +++
 .../src/kinematics_interface_kdl.cpp          | 39 +++++++++++++++++++
 2 files changed, 44 insertions(+)

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..83c32cd 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<double, 6, Eigen::Dynamic> & jacobian) override;
 
+  bool calculate_jacobian_inverse(
+    const Eigen::VectorXd & joint_pos, const std::string & link_name,
+    Eigen::Matrix<double, Eigen::Dynamic, 6> & 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<double, 6, Eigen::Dynamic> & jacobian);
+  bool verify_jacobian_inverse(const Eigen::Matrix<double, Eigen::Dynamic, 6> & jacobian);
 
   bool initialized = false;
   std::string root_name_;
diff --git a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp
index 8c86d00..12b12fb 100644
--- a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp
+++ b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp
@@ -182,6 +182,31 @@ bool KinematicsInterfaceKDL::calculate_jacobian(
   return true;
 }
 
+bool KinematicsInterfaceKDL::calculate_jacobian_inverse(
+  const Eigen::VectorXd & joint_pos, const std::string & link_name,
+  Eigen::Matrix<double, Eigen::Dynamic, 6> & 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<double, 6, Eigen::Dynamic> jacobian = jacobian_->data;
+
+  // damped inverse
+  jacobian_inverse = (jacobian.transpose() * jacobian + alpha * I).inverse() * jacobian.transpose();
+
+  return true;
+}
+
 bool KinematicsInterfaceKDL::calculate_link_transform(
   const Eigen::Matrix<double, Eigen::Dynamic, 1> & 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<double, Eigen::Dynamic, 6> & 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"