From 7ddaeb2154c4b1b3195955f87669e206e9403ea3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 7 Oct 2022 17:46:21 +0200 Subject: [PATCH] More efficient computation of PseudoInverse Considering S as a rectangular matrix when only the diagonal elements are relevant is vasting computing resources in matrix multiplication. --- .../franka_example_controllers/pseudo_inversion.h | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.h b/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.h index c82092748..9d4fe9c03 100644 --- a/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.h +++ b/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.h @@ -16,15 +16,13 @@ namespace franka_example_controllers { inline void pseudoInverse(const Eigen::MatrixXd& M_, Eigen::MatrixXd& M_pinv_, bool damped = true) { double lambda_ = damped ? 0.2 : 0.0; - Eigen::JacobiSVD svd(M_, Eigen::ComputeFullU | Eigen::ComputeFullV); - Eigen::JacobiSVD::SingularValuesType sing_vals_ = svd.singularValues(); - Eigen::MatrixXd S_ = M_; // copying the dimensions of M_, its content is not needed. - S_.setZero(); + Eigen::JacobiSVD svd(M_, Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::VectorXd sing_vals_ = svd.singularValues(); + 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