More efficient computation of PseudoInverse

Keeping S as a rectangular matrix when only the diagonal elements are relevant
is vasting computing resources in matrix multiplication.

franka_example_controllers/include/franka_example_controllers/pseudo_inversion.h

@@ -17,14 +17,12 @@ inline void pseudoInverse(const Eigen::MatrixXd& M_, Eigen::MatrixXd& M_pinv_, b
   double lambda_ = damped ? 0.2 : 0.0;
 
   Eigen::JacobiSVD<Eigen::MatrixXd> svd(M_, Eigen::ComputeFullU | Eigen::ComputeFullV);
-  Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType sing_vals_ = svd.singularValues();
-  Eigen::MatrixXd S_ = M_;  // copying the dimensions of M_, its content is not needed.
-  S_.setZero();
+  Eigen::VectorXd sing_vals_ = svd.singularValues().array().abs();
+  for (unsigned int i = 0; i < sing_vals_.size(); ++i)
+    sing_vals_[i] = sing_vals_[i] / (sing_vals_[i] * sing_vals_[i] + lambda_ * lambda_);
 
-  for (int i = 0; i < sing_vals_.size(); i++)
-    S_(i, i) = (sing_vals_(i)) / (sing_vals_(i) * sing_vals_(i) + lambda_ * lambda_);
-
-  M_pinv_ = Eigen::MatrixXd(svd.matrixV() * S_.transpose() * svd.matrixU().transpose());
+  M_pinv_.noalias() =
+      Eigen::MatrixXd((svd.matrixV() * sing_vals_.asDiagonal()) * svd.matrixU().transpose());
 }
 
 }  // namespace franka_example_controllers