fix pinv bug

Author: evan-palmer

ik_solvers/src/pseudoinverse.cpp

@@ -12,17 +12,10 @@ auto least_squares(const Eigen::MatrixXd & jac) -> Eigen::MatrixXd
   return jac.completeOrthogonalDecomposition().pseudoInverse();
 }
 
-auto damped_least_squares(const Eigen::MatrixXd & jac, const Eigen::VectorXd & damping) -> Eigen::MatrixXd
-{
-  if (damping.size() != jac.rows()) {
-    throw std::invalid_argument("Damping vector must have the same size as the number of rows in the Jacobian.");
-  }
-  return jac.transpose() * ((jac * jac.transpose()) + damping.asDiagonal().toDenseMatrix()).inverse();
-}
-
 auto damped_least_squares(const Eigen::MatrixXd & jac, double damping) -> Eigen::MatrixXd
 {
-  return damped_least_squares(jac, damping * Eigen::MatrixXd::Identity(jac.rows(), jac.rows()));
+  const Eigen::MatrixXd eye = Eigen::MatrixXd::Identity(jac.rows(), jac.rows());
+  return jac.transpose() * ((jac * jac.transpose()) + (damping * eye)).inverse();
 }
 
 }  // namespace ik_solvers::pinv

ik_solvers/src/pseudoinverse.hpp

@@ -28,11 +28,6 @@ namespace ik_solvers::pinv
 /// Calculate the pseudoinverse of a Jacobian matrix using the Moore-Penrose method.
 auto least_squares(const Eigen::MatrixXd & jac) -> Eigen::MatrixXd;
 
-/// Calculate the pseudoinverse of a Jacobian matrix using the damped least squares method.
-///
-/// This version allows for more granular selection of the damping values to tune the solution velocities.
-auto damped_least_squares(const Eigen::MatrixXd & jac, const Eigen::VectorXd & damping) -> Eigen::MatrixXd;
-
 /// Calculate the pseudoinverse of a Jacobian matrix using the damped least squares method.
 auto damped_least_squares(const Eigen::MatrixXd & jac, double damping) -> Eigen::MatrixXd;