From c186abd86039055d4b7e6873fd3022d5d3a04b7f Mon Sep 17 00:00:00 2001
From: asantamaria <somriu@gmail.com>
Date: Wed, 24 Aug 2016 13:32:21 +0200
Subject: [PATCH] merged dev branch and changed again to CalcPinv for master
---
src/common_fc.cpp | 9 ++++-----
src/common_fc.h | 7 +++----
src/tasks/cog.cpp | 6 ++----
src/tasks/ir.cpp | 5 ++---
src/tasks/jl.cpp | 6 ++----
5 files changed, 13 insertions(+), 20 deletions(-)
diff --git a/src/common_fc.cpp b/src/common_fc.cpp
index 295d30f..7633a25 100644
--- a/src/common_fc.cpp
+++ b/src/common_fc.cpp
@@ -29,11 +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);
- J_inverse = J.transpose();
-
+ Eigen::MatrixXd Winv = CalcPinv(W);
+ Eigen::MatrixXd temp = J*Winv*J.transpose();
+ Eigen::MatrixXd J_inverse = Winv*J.transpose()*CalcPinv(temp);
+
return J_inverse;
}
diff --git a/src/common_fc.h b/src/common_fc.h
index fdf1e5f..2523d17 100644
--- a/src/common_fc.h
+++ b/src/common_fc.h
@@ -61,10 +61,9 @@ 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();
- _Matrix_Type_ mpinv = a.transpose();
+ 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();
return mpinv;
}
diff --git a/src/tasks/cog.cpp b/src/tasks/cog.cpp
index 9283a37..799497b 100644
--- a/src/tasks/cog.cpp
+++ b/src/tasks/cog.cpp
@@ -169,8 +169,7 @@ void CTaskCOG::TaskErrorJacFull(UAM::CArm& arm, const UAM::CHT& HT, Eigen::Matri
JG.block(0,4,2,arm.nj)=Jacob_temp.block(0,0,2,arm.nj);
- //JG_pseudo = UAM::CCommon_Fc::CalcPinv(JG);
- JG_pseudo = JG.transpose();
+ JG_pseudo = UAM::CCommon_Fc::CalcPinv(JG);
}
void CTaskCOG::TaskErrorJac(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd& cog_PGxy, Eigen::MatrixXd& cog_arm, Eigen::MatrixXd& sigmaG, Eigen::MatrixXd& JG, Eigen::MatrixXd& JG_pseudo, UAM::ArmCogData& arm_cog_data)
@@ -192,8 +191,7 @@ void CTaskCOG::TaskErrorJac(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd&
JG.block(0,4,1,arm.nj)=2*cog_PGxy.transpose()*Jacob_temp.block(0,0,2,arm.nj);
- //JG_pseudo = UAM::CCommon_Fc::CalcPinv(JG);
- JG_pseudo = JG.transpose();
+ JG_pseudo = UAM::CCommon_Fc::CalcPinv(JG);
}
} // End of UAM namespace
\ No newline at end of file
diff --git a/src/tasks/ir.cpp b/src/tasks/ir.cpp
index 2437b85..584d4c9 100644
--- a/src/tasks/ir.cpp
+++ b/src/tasks/ir.cpp
@@ -44,9 +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 = H.transpose();
-
+ Eigen::MatrixXd Hinv = UAM::CCommon_Fc::CalcPinv(H);
+
// weighting matrix
Eigen::MatrixXd temp = JIR*Hinv*JIR.transpose();
JIR_pseudo = Hinv*JIR.transpose()*temp.transpose();
diff --git a/src/tasks/jl.cpp b/src/tasks/jl.cpp
index 4e8f504..9b972d6 100644
--- a/src/tasks/jl.cpp
+++ b/src/tasks/jl.cpp
@@ -47,8 +47,7 @@ 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 = JL.transpose();
+ JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
}
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)
@@ -70,8 +69,7 @@ void CTaskJL::TaskErrorJacFull(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos
}
// Task Jacobian Pseudo-inverse
- //JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
- JL_pseudo = JL.transpose();
+ JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
}
} // End of UAM namespace
\ No newline at end of file
--
GitLab