From 8a4c78cbb7b3986a50ed3ffc7b4e7124fe767578 Mon Sep 17 00:00:00 2001 From: asantamaria <somriu@gmail.com> Date: Mon, 22 Aug 2016 13:03:07 +0200 Subject: [PATCH] Commented PINV stuff to work in RR laptop --- src/common_fc.cpp | 7 ++++--- src/common_fc.h | 7 ++++--- src/kinematics.cpp | 3 ++- src/tasks/ir.cpp | 3 ++- src/tasks/jl.cpp | 6 ++++-- 5 files changed, 16 insertions(+), 10 deletions(-) diff --git a/src/common_fc.cpp b/src/common_fc.cpp index 3828dfd..295d30f 100644 --- a/src/common_fc.cpp +++ b/src/common_fc.cpp @@ -29,9 +29,10 @@ Eigen::MatrixXd CCommon_Fc::WeightInvJac(const Eigen::MatrixXd& J, const Eigen:: } // weighting matrix - Eigen::MatrixXd Winv = CalcPinv(W); - Eigen::MatrixXd temp = J*Winv*J.transpose(); - Eigen::MatrixXd J_inverse = Winv*J.transpose()*CalcPinv(temp); + // Eigen::MatrixXd Winv = CalcPinv(W); + // Eigen::MatrixXd temp = J*Winv*J.transpose(); + // Eigen::MatrixXd J_inverse = Winv*J.transpose()*CalcPinv(temp); + J_inverse = J.transpose(); return J_inverse; } diff --git a/src/common_fc.h b/src/common_fc.h index 2523d17..fdf1e5f 100644 --- a/src/common_fc.h +++ b/src/common_fc.h @@ -61,9 +61,10 @@ class CCommon_Fc */ template<typename _Matrix_Type_> static _Matrix_Type_ CalcPinv(const _Matrix_Type_ &a, double epsilon = std::numeric_limits<double>::epsilon()) { - Eigen::JacobiSVD< _Matrix_Type_ > svdd(a, Eigen::ComputeThinU | Eigen::ComputeThinV); - double tolerance = epsilon * std::max(a.cols(), a.rows()) *svdd.singularValues().array().abs()(0); - _Matrix_Type_ mpinv = svdd.matrixV() * (svdd.singularValues().array().abs() > tolerance).select(svdd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svdd.matrixU().adjoint(); + //Eigen::JacobiSVD< _Matrix_Type_ > svdd(a, Eigen::ComputeThinU | Eigen::ComputeThinV); + //double tolerance = epsilon * std::max(a.cols(), a.rows()) *svdd.singularValues().array().abs()(0); + //_Matrix_Type_ mpinv = svdd.matrixV() * (svdd.singularValues().array().abs() > tolerance).select(svdd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svdd.matrixU().adjoint(); + _Matrix_Type_ mpinv = a.transpose(); return mpinv; } diff --git a/src/kinematics.cpp b/src/kinematics.cpp index 7080b33..0ec0f0e 100644 --- a/src/kinematics.cpp +++ b/src/kinematics.cpp @@ -25,7 +25,8 @@ void CKinematics::UAMKine(const UAM::CQuad& quad, UAM::CArm& arm, UAM::CHT& HT, Eigen::MatrixXd Jb2i = QuadJac(quad.pos); // Inertial to body frames - Eigen::MatrixXd Ji2b = CCommon_Fc::CalcPinv(Jb2i); + // Eigen::MatrixXd Ji2b = CCommon_Fc::CalcPinv(Jb2i); + Eigen::MatrixXd Ji2b = Jb2i.inverse(); // Jacobian from quadrotor body to camera Eigen::MatrixXd Jb2c = Eigen::MatrixXd::Zero(6,6); diff --git a/src/tasks/ir.cpp b/src/tasks/ir.cpp index 2db32f6..2437b85 100644 --- a/src/tasks/ir.cpp +++ b/src/tasks/ir.cpp @@ -44,7 +44,8 @@ void CTaskIR::TaskErrorJac(const int& arm_dof, const double& inf_rad, const Eige } // } - Eigen::MatrixXd Hinv = UAM::CCommon_Fc::CalcPinv(H); + // Eigen::MatrixXd Hinv = UAM::CCommon_Fc::CalcPinv(H); + Eigen::MatrixXd Hinv = H.transpose(); // weighting matrix Eigen::MatrixXd temp = JIR*Hinv*JIR.transpose(); diff --git a/src/tasks/jl.cpp b/src/tasks/jl.cpp index 9b972d6..4e8f504 100644 --- a/src/tasks/jl.cpp +++ b/src/tasks/jl.cpp @@ -47,7 +47,8 @@ void CTaskJL::TaskErrorJac(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos_des JL.block(0,4,1,arm.nj) = pow_n*Jtemp.transpose(); // Task Jacobian Pseudo-inverse - JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL); + //JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL); + JL_pseudo = JL.transpose(); } void CTaskJL::TaskErrorJacFull(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos_des, Eigen::MatrixXd& jntlim_pos_error, Eigen::MatrixXd& sigmaL, Eigen::MatrixXd& JL, Eigen::MatrixXd& JL_pseudo) @@ -69,7 +70,8 @@ void CTaskJL::TaskErrorJacFull(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos } // Task Jacobian Pseudo-inverse - JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL); + //JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL); + JL_pseudo = JL.transpose(); } } // End of UAM namespace \ No newline at end of file -- GitLab