Skip to content

Some improvements to efficiency #290

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
rhaschke wants to merge 3 commits into
base: develop
Choose a base branch
from
Open

Some improvements to efficiency #290

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
49c4e11
Avoid extra copy when assigning std::array to KDL::JntArray
rhaschke Oct 7, 2022
f6c2b35
Avoid computation of full SVD
rhaschke Oct 7, 2022
092ae94
More efficient computation of PseudoInverse
rhaschke Oct 7, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 6 additions & 8 deletions franka_example_controllers/include/franka_example_controllers/pseudo_inversion.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<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::JacobiSVD<Eigen::MatrixXd> 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
16 changes: 8 additions & 8 deletions franka_gazebo/src/model_kdl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ bool ModelKDL::isCloseToSingularity(const KDL::Jacobian& jacobian) const {
return false;
}
Eigen::Matrix<double, 6, 6> mat = jacobian.data * jacobian.data.transpose();
Eigen::JacobiSVD<Eigen::MatrixXd> svd(mat, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6>> svd(mat);
double critical = svd.singularValues().tail(1)(0);
return critical < this->singularity_threshold_;
}
Expand Down Expand Up @@ -122,7 +122,7 @@ std::array<double, 16> ModelKDL::pose(

KDL::ChainFkSolverPos_recursive solver(chain);

kq.data = Eigen::Matrix<double, 7, 1>(q.data());
kq.data = Eigen::Matrix<double, 7, 1>::Map(q.data());

int error = solver.JntToCart(kq, kp, segment(frame));
if (error != KDL::SolverI::E_NOERROR) {
Expand All @@ -146,7 +146,7 @@ std::array<double, 42> ModelKDL::bodyJacobian(
const {
KDL::JntArray kq;
KDL::Jacobian J(7); // NOLINT(readability-identifier-naming)
kq.data = Eigen::Matrix<double, 7, 1>(q.data());
kq.data = Eigen::Matrix<double, 7, 1>::Map(q.data());

// Augment the chain with the two virtual frames 'EE' and 'K'
KDL::Chain chain = this->chain_; // copy
Expand Down Expand Up @@ -189,7 +189,7 @@ std::array<double, 42> ModelKDL::zeroJacobian(
const {
KDL::JntArray kq;
KDL::Jacobian J(7); // NOLINT(readability-identifier-naming)
kq.data = Eigen::Matrix<double, 7, 1>(q.data());
kq.data = Eigen::Matrix<double, 7, 1>::Map(q.data());

// Augment the chain with the two virtual frames 'EE' and 'K'
KDL::Chain chain = this->chain_; // copy
Expand Down Expand Up @@ -222,7 +222,7 @@ std::array<double, 49> ModelKDL::mass(
const {
KDL::JntArray kq;
KDL::JntSpaceInertiaMatrix M(7); // NOLINT(readability-identifier-naming)
kq.data = Eigen::Matrix<double, 7, 1>(q.data());
kq.data = Eigen::Matrix<double, 7, 1>::Map(q.data());

KDL::Chain chain = this->chain_; // copy
augmentFrame("load", F_x_Ctotal, m_total, I_total, chain);
Expand All @@ -247,8 +247,8 @@ std::array<double, 7> ModelKDL::coriolis(
const std::array<double, 3>& F_x_Ctotal) // NOLINT(readability-identifier-naming)
const {
KDL::JntArray kq, kdq, kc(7);
kq.data = Eigen::Matrix<double, 7, 1>(q.data());
kdq.data = Eigen::Matrix<double, 7, 1>(dq.data());
kq.data = Eigen::Matrix<double, 7, 1>::Map(q.data());
kdq.data = Eigen::Matrix<double, 7, 1>::Map(dq.data());

KDL::Chain chain = this->chain_; // copy
augmentFrame("load", F_x_Ctotal, m_total, I_total, chain);
Expand All @@ -272,7 +272,7 @@ std::array<double, 7> ModelKDL::gravity(
const std::array<double, 3>& gravity_earth) const {
KDL::JntArray kq, kg(7);
KDL::Vector grav(gravity_earth[0], gravity_earth[1], gravity_earth[2]);
kq.data = Eigen::Matrix<double, 7, 1>(q.data());
kq.data = Eigen::Matrix<double, 7, 1>::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);
Expand Down