18 #include <Eigen/Dense> 26 template <
typename _Matrix_Type_>
28 Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
29 _Matrix_Type_ m_sigma = svd.singularValues();
30 double pinvtoler = 1.e-6;
31 _Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(), pinvmat.rows());
32 for (
long i = 0; i < m_sigma.rows(); ++i) {
33 if (m_sigma(i) > pinvtoler) {
34 m_sigma_inv(i, i) = 1.0 / m_sigma(i);
37 pinvmat = (svd.matrixV() * m_sigma_inv * svd.matrixU().transpose());
40 template <
typename Matrix3,
typename Po
int>
42 Matrix3 res = Matrix3::Zero(3, 3);
52 static const double MARGIN(0.001);
Definition: bernstein.h:20
void PseudoInverse(_Matrix_Type_ &pinvmat)
An inverse kinematics architecture enforcing an arbitrary number of strict priority levels (Reference...
Definition: MathDefs.h:27
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
Matrix3 skew(const Point &x)
Definition: MathDefs.h:41