diff --git a/FindKDL.cmake b/FindKDL.cmake index 4ebbb6735d84a9dfe778e864d20608db062c58b8..44e132d4af529c02e2416b2a7628b6f4028e216b 100644 --- a/FindKDL.cmake +++ b/FindKDL.cmake @@ -1,15 +1,15 @@ # Locate KDL install directory +MESSAGE(STATUS "Detecting KDL: using orocos_kdl-config.cmake to locate files.") + FIND_PATH(KDL_DIR orocos_kdl-config.cmake /usr/local/share/orocos_kdl) INCLUDE (${KDL_DIR}/orocos_kdl-config.cmake) -MESSAGE(STATUS "Detecting KDL: using orocos_kdl-config.cmake to locate files.") - IF (orocos_kdl_INCLUDE_DIRS AND orocos_kdl_LIBRARIES) SET(KDL_FOUND TRUE) MESSAGE(STATUS " Libraries: ${orocos_kdl_LIBRARIES}") MESSAGE(STATUS " Defines: ${orocos_kdl_INCLUDE_DIRS}") ELSE (orocos_kdl_INCLUDE_DIRS AND orocos_kdl_LIBRARIES) SET(KDL_FOUND FALSE) -ENDIF (orocos_kdl_INCLUDE_DIRS AND orocos_kdl_LIBRARIES) +ENDIF (orocos_kdl_INCLUDE_DIRS AND orocos_kdl_LIBRARIES) \ No newline at end of file diff --git a/src/common_fc.cpp b/src/common_fc.cpp index d9cc2d1f677533a66d3e27524228995b3f17e598..470e8a4e59b4419678cbd30bb048977a8c0059ae 100644 --- a/src/common_fc.cpp +++ b/src/common_fc.cpp @@ -8,54 +8,10 @@ CCommon_Fc::~CCommon_Fc(){} Eigen::MatrixXd CCommon_Fc::CalcPinv(const Eigen::MatrixXd& a, double epsilon) { -/* // Eigen::JacobiSVD<Eigen::MatrixXd> svdd(a, Eigen::ComputeThinU | Eigen::ComputeThinV); - // double tolerance = epsilon * std::max(a.cols(), a.rows()) *svdd.singularValues().array().abs()(0); - // Eigen::MatrixXd mpinv = svdd.matrixV() * (svdd.singularValues().array().abs() > tolerance).select(svdd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svdd.matrixU().adjoint(); - // return mpinv; - -// see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method - -const Eigen::MatrixXd* m; -Eigen::MatrixXd t; -Eigen::MatrixXd m_pinv; - -// transpose so SVD decomp can work... -if ( a.rows()<a.cols() ) -{ - t = a.transpose(); - m = &t; -} -else - m = &a; - -// SVD -//JacobiSVD<Eigen::MatrixXd> svd = m->jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV); -Eigen::JacobiSVD<Eigen::MatrixXd> svd = m->jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV); -Eigen::MatrixXd vSingular = svd.singularValues(); -// Build a diagonal matrix with the Inverted Singular values -// The pseudo inverted singular matrix is easy to compute : -// is formed by replacing every nonzero entry by its reciprocal (inversing). -Eigen::MatrixXd vPseudoInvertedSingular(svd.matrixV().cols(),1); -for (int iRow =0; iRow<vSingular.rows(); iRow++) -{ - if ( fabs(vSingular(iRow))<=epsilon ) - vPseudoInvertedSingular(iRow,0)=0.; - else - vPseudoInvertedSingular(iRow,0)=1./vSingular(iRow); -} -// A little optimization here -Eigen::MatrixXd mAdjointU = svd.matrixU().adjoint().block(0,0,vSingular.rows(),svd.matrixU().adjoint().cols()); -// Pseudo-Inversion : V * S * U' -m_pinv = (svd.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU ; - -// transpose back if necessary -if ( a.rows()<a.cols() ) - return m_pinv.transpose();*/ - -Eigen::MatrixXd m_pinv; -m_pinv = a.transpose(); - -return m_pinv; + Eigen::JacobiSVD<Eigen::MatrixXd> svdd(a, Eigen::ComputeThinU | Eigen::ComputeThinV); + double tolerance = epsilon * std::max(a.cols(), a.rows()) *svdd.singularValues().array().abs()(0); + Eigen::MatrixXd mpinv = svdd.matrixV() * (svdd.singularValues().array().abs() > tolerance).select(svdd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svdd.matrixU().adjoint(); + return mpinv; } diff --git a/src/kinematics.cpp b/src/kinematics.cpp index 2965b76c8cab4d7dde425b7bc02fc084716c9024..5e7cf9d4d4ce4333e98103d215c338082c1025ee 100644 --- a/src/kinematics.cpp +++ b/src/kinematics.cpp @@ -24,8 +24,6 @@ void CKinematics::UAMKine(const UAM::CQuad& quad, UAM::CArm& arm, UAM::CHT& HT, // Quadrotor Jacobian body to inertial frames Eigen::MatrixXd Jb2i = QuadJac(quad.pos); -std::cout << __LINE__ << " [" << quad.pos.transpose() << "]" << std::endl; - // Inertial to body frames Eigen::MatrixXd Ji2b = UAM::CCommon_Fc::CalcPinv(Jb2i);