Refactor convert_cartesian_deltas_to_joint_deltas to use compute_jaco…

…bian_inverse method

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_;

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;
 }