moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
pseudoinverse.hpp
Go to the documentation of this file.
1#pragma once
2
3#include <Eigen/Eigenvalues>
4
5#include <algorithm>
6#include <cmath>
7
9{
18inline Eigen::VectorXd solvePseudoinverse(const Eigen::Ref<const Eigen::MatrixXd>& jacobian,
19 const Eigen::Ref<const Eigen::VectorXd>& input, double threshold)
20{
21 const Eigen::MatrixXd normal = jacobian * jacobian.transpose();
22 const Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(normal);
23 const Eigen::VectorXd& eigenvalues = solver.eigenvalues();
24 const Eigen::MatrixXd& eigenvectors = solver.eigenvectors();
25 const double singular_max = std::sqrt(std::max(0.0, eigenvalues.maxCoeff()));
26 const double absolute_threshold = threshold * singular_max;
27 Eigen::VectorXd projected = eigenvectors.transpose() * input;
28 for (Eigen::Index index = 0; index < eigenvalues.size(); ++index)
29 {
30 const double singular = std::sqrt(std::max(0.0, eigenvalues(index)));
31 projected(index) = singular > absolute_threshold ? projected(index) / eigenvalues(index) : 0.0;
32 }
33 return jacobian.transpose() * (eigenvectors * projected);
34}
35} // namespace kdl_kinematics_plugin::internal
Eigen::VectorXd solvePseudoinverse(const Eigen::Ref< const Eigen::MatrixXd > &jacobian, const Eigen::Ref< const Eigen::VectorXd > &input, double threshold)
KeyboardReader input