Calculate Jacobian Inverse (#92)

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_;
@@ -75,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;
 }
@@ -182,6 +179,34 @@ bool KinematicsInterfaceKDL::calculate_jacobian(
   return true;
 }
 
+bool KinematicsInterfaceKDL::calculate_jacobian_inverse(
+  const Eigen::Matrix<double, Eigen::Dynamic, 1> & 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();
+
+  jacobian_inverse = *jacobian_inverse_;
+
+  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"

kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp

@@ -108,6 +108,26 @@ TEST_F(TestKDLPlugin, KDL_plugin_function)
   {
     ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
   }
+
+  // calculate jacobian
+  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian = Eigen::Matrix<double, 6, 2>::Zero();
+  ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian));
+
+  // calculate jacobian inverse
+  Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian_inverse =
+    jacobian.completeOrthogonalDecomposition().pseudoInverse();
+  Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian_inverse_est =
+    Eigen::Matrix<double, 2, 6>::Zero();
+  ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est));
+
+  // ensure jacobian inverse math is correct
+  for (size_t i = 0; i < static_cast<size_t>(jacobian_inverse.rows()); ++i)
+  {
+    for (size_t j = 0; j < static_cast<size_t>(jacobian_inverse.cols()); ++j)
+    {
+      ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02);
+    }
+  }
 }
 
 TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector)
@@ -142,6 +162,26 @@ TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector)
   {
     ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
   }
+
+  // calculate jacobian
+  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian = Eigen::Matrix<double, 6, 2>::Zero();
+  ASSERT_TRUE(ik_->calculate_jacobian(pos, end_effector_, jacobian));
+
+  // calculate jacobian inverse
+  Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian_inverse =
+    jacobian.completeOrthogonalDecomposition().pseudoInverse();
+  Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian_inverse_est =
+    Eigen::Matrix<double, 2, 6>::Zero();
+  ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est));
+
+  // ensure jacobian inverse math is correct
+  for (size_t i = 0; i < static_cast<size_t>(jacobian_inverse.rows()); ++i)
+  {
+    for (size_t j = 0; j < static_cast<size_t>(jacobian_inverse.cols()); ++j)
+    {
+      ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02);
+    }
+  }
 }
 
 TEST_F(TestKDLPlugin, incorrect_input_sizes)
@@ -161,10 +201,14 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes)
   delta_x[2] = 1;
   Eigen::Matrix<double, Eigen::Dynamic, 1> delta_theta = Eigen::Matrix<double, 2, 1>::Zero();
   Eigen::Matrix<double, 6, 1> delta_x_est;
+  Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian = Eigen::Matrix<double, 2, 6>::Zero();
 
   // wrong size input vector
   Eigen::Matrix<double, Eigen::Dynamic, 1> vec_5 = Eigen::Matrix<double, 5, 1>::Zero();
 
+  // wrong size input jacobian
+  Eigen::Matrix<double, Eigen::Dynamic, 6> mat_5_6 = Eigen::Matrix<double, 5, 6>::Zero();
+
   // calculate transform
   ASSERT_FALSE(ik_->calculate_link_transform(vec_5, end_effector_, end_effector_transform));
   ASSERT_FALSE(ik_->calculate_link_transform(pos, "link_not_in_model", end_effector_transform));
@@ -183,6 +227,11 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes)
     ik_->convert_joint_deltas_to_cartesian_deltas(pos, vec_5, end_effector_, delta_x_est));
   ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas(
     pos, delta_theta, "link_not_in_model", delta_x_est));
+
+  // calculate jacobian inverse
+  ASSERT_FALSE(ik_->calculate_jacobian_inverse(vec_5, end_effector_, jacobian));
+  ASSERT_FALSE(ik_->calculate_jacobian_inverse(pos, end_effector_, mat_5_6));
+  ASSERT_FALSE(ik_->calculate_jacobian_inverse(pos, "link_not_in_model", jacobian));
 }
 
 TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description)