From 5ac1077a91cde99b1d2bd6e8b3dbfb9267c30ee5 Mon Sep 17 00:00:00 2001
From: francescodonofrio <116165932+francescodonofrio@users.noreply.github.com>
Date: Fri, 15 Nov 2024 18:14:53 +0100
Subject: [PATCH] Refactor convert_cartesian_deltas_to_joint_deltas to use
 compute_jacobian_inverse method

---
 .../kinematics_interface_kdl.hpp              |  1 +
 .../src/kinematics_interface_kdl.cpp          | 22 +++++++++----------
 2 files changed, 12 insertions(+), 11 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 83c32cd..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
@@ -80,6 +80,7 @@ class KinematicsInterfaceKDL : public kinematics_interface::KinematicsInterface
   KDL::JntArray q_;
   KDL::Frame frame_;
   std::shared_ptr<KDL::Jacobian> jacobian_;
+  std::shared_ptr<Eigen::Matrix<double, Eigen::Dynamic, 6>> jacobian_inverse_;
   std::shared_ptr<KDL::ChainJntToJacSolver> jac_solver_;
   std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface_;
   std::unordered_map<std::string, int> link_name_map_;
diff --git a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp
index d3f5690..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<KDL::ChainFkSolverPos_recursive>(chain_);
   jac_solver_ = std::make_shared<KDL::ChainJntToJacSolver>(chain_);
   jacobian_ = std::make_shared<KDL::Jacobian>(num_joints_);
+  jacobian_inverse_ = std::make_shared<Eigen::Matrix<double, Eigen::Dynamic, 6>>(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<double, 6, Eigen::Dynamic> J = jacobian_->data;
-  // damped inverse
-  Eigen::Matrix<double, Eigen::Dynamic, 6> J_inverse =
-    (J.transpose() * J + alpha * I).inverse() * J.transpose();
-  delta_theta = J_inverse * delta_x;
+  delta_theta = *jacobian_inverse_ * delta_x;
 
   return true;
 }
@@ -202,7 +199,10 @@ bool KinematicsInterfaceKDL::calculate_jacobian_inverse(
   Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian = jacobian_->data;
 
   // damped inverse
-  jacobian_inverse = (jacobian.transpose() * jacobian + alpha * I).inverse() * jacobian.transpose();
+  *jacobian_inverse_ =
+    (jacobian.transpose() * jacobian + alpha * I).inverse() * jacobian.transpose();
+
+  jacobian_inverse = *jacobian_inverse_;
 
   return true;
 }