From 349c3c9803d10f6ea0d17613436a7233b1fe61b9 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 Keeping 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 | 12 +++++------- 1 file changed, 5 insertions(+), 7 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..53a20fe18 100644 --- a/franka_example_controllers/include/franka_example_controllers/pseudo_inversion.h +++ b/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 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::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