Add the methods for the computation of the jacobian inverse

Author: francescodonofrio

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<double, 6, Eigen::Dynamic> & 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<double, Eigen::Dynamic, 6> & jacobian_inverse) = 0;
+
   bool convert_cartesian_deltas_to_joint_deltas(
     std::vector<double> & joint_pos_vec, const std::vector<double> & delta_x_vec,
     const std::string & link_name, std::vector<double> & delta_theta_vec);
@@ -110,6 +121,10 @@ class KinematicsInterface
   bool calculate_jacobian(
     const std::vector<double> & joint_pos_vec, const std::string & link_name,
     Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian);
+
+  bool calculate_jacobian_inverse(
+    const std::vector<double> & joint_pos_vec, const std::string & link_name,
+    Eigen::Matrix<double, Eigen::Dynamic, 6> & jacobian_inverse);
 };
 
 }  // namespace kinematics_interface

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<double> & joint_pos_vec, const std::string & link_name,
+  Eigen::Matrix<double, Eigen::Dynamic, 6> & jacobian_inverse)
+{
+  auto joint_pos = Eigen::Map<const Eigen::VectorXd>(joint_pos_vec.data(), joint_pos_vec.size());
+
+  return calculate_jacobian_inverse(joint_pos, link_name, jacobian_inverse);
+}
+
 }  // namespace kinematics_interface