Calculate Jacobian Inverse

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

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

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"