From 49c4e11afae1dc2c1e5296efa5d08656ffa3dd87 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 7 Oct 2022 17:56:26 +0200 Subject: [PATCH 1/3] Avoid extra copy when assigning std::array to KDL::JntArray --- franka_gazebo/src/model_kdl.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/franka_gazebo/src/model_kdl.cpp b/franka_gazebo/src/model_kdl.cpp index ca398d0d1..8c3857dd0 100644 --- a/franka_gazebo/src/model_kdl.cpp +++ b/franka_gazebo/src/model_kdl.cpp @@ -122,7 +122,7 @@ std::array ModelKDL::pose( KDL::ChainFkSolverPos_recursive solver(chain); - kq.data = Eigen::Matrix(q.data()); + kq.data = Eigen::Matrix::Map(q.data()); int error = solver.JntToCart(kq, kp, segment(frame)); if (error != KDL::SolverI::E_NOERROR) { @@ -146,7 +146,7 @@ std::array ModelKDL::bodyJacobian( const { KDL::JntArray kq; KDL::Jacobian J(7); // NOLINT(readability-identifier-naming) - kq.data = Eigen::Matrix(q.data()); + kq.data = Eigen::Matrix::Map(q.data()); // Augment the chain with the two virtual frames 'EE' and 'K' KDL::Chain chain = this->chain_; // copy @@ -189,7 +189,7 @@ std::array ModelKDL::zeroJacobian( const { KDL::JntArray kq; KDL::Jacobian J(7); // NOLINT(readability-identifier-naming) - kq.data = Eigen::Matrix(q.data()); + kq.data = Eigen::Matrix::Map(q.data()); // Augment the chain with the two virtual frames 'EE' and 'K' KDL::Chain chain = this->chain_; // copy @@ -222,7 +222,7 @@ std::array ModelKDL::mass( const { KDL::JntArray kq; KDL::JntSpaceInertiaMatrix M(7); // NOLINT(readability-identifier-naming) - kq.data = Eigen::Matrix(q.data()); + kq.data = Eigen::Matrix::Map(q.data()); KDL::Chain chain = this->chain_; // copy augmentFrame("load", F_x_Ctotal, m_total, I_total, chain); @@ -247,8 +247,8 @@ std::array ModelKDL::coriolis( const std::array& F_x_Ctotal) // NOLINT(readability-identifier-naming) const { KDL::JntArray kq, kdq, kc(7); - kq.data = Eigen::Matrix(q.data()); - kdq.data = Eigen::Matrix(dq.data()); + kq.data = Eigen::Matrix::Map(q.data()); + kdq.data = Eigen::Matrix::Map(dq.data()); KDL::Chain chain = this->chain_; // copy augmentFrame("load", F_x_Ctotal, m_total, I_total, chain); @@ -272,7 +272,7 @@ std::array ModelKDL::gravity( const std::array& gravity_earth) const { KDL::JntArray kq, kg(7); KDL::Vector grav(gravity_earth[0], gravity_earth[1], gravity_earth[2]); - kq.data = Eigen::Matrix(q.data()); + kq.data = Eigen::Matrix::Map(q.data()); KDL::Chain chain = this->chain_; // copy augmentFrame("load", F_x_Ctotal, m_total, {1, 0, 0, 0, 1, 0, 0, 0, 1}, chain); From f6c2b3582497755024c9a3940ca33a9d4074ceea Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 7 Oct 2022 17:49:43 +0200 Subject: [PATCH 2/3] Avoid computation of full SVD ... when only singular values are needed. Actually, it might suffice to compute the determinant! --- franka_gazebo/src/model_kdl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/franka_gazebo/src/model_kdl.cpp b/franka_gazebo/src/model_kdl.cpp index 8c3857dd0..7f5cac6a6 100644 --- a/franka_gazebo/src/model_kdl.cpp +++ b/franka_gazebo/src/model_kdl.cpp @@ -37,7 +37,7 @@ bool ModelKDL::isCloseToSingularity(const KDL::Jacobian& jacobian) const { return false; } Eigen::Matrix mat = jacobian.data * jacobian.data.transpose(); - Eigen::JacobiSVD svd(mat, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::JacobiSVD> svd(mat); double critical = svd.singularValues().tail(1)(0); return critical < this->singularity_threshold_; } From 092ae9498b1295748a749dd8e68fb43df2e08a84 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 7 Oct 2022 17:46:21 +0200 Subject: [PATCH 3/3] 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..7805c302c 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