From 3fedc3e1d67f925c2b948eedd594b7b99b79be2b Mon Sep 17 00:00:00 2001
From: asantamaria <asantamaria@iri.upc.edu>
Date: Tue, 30 Aug 2016 19:04:15 +0200
Subject: [PATCH] minor changes

---
 src/tasks/eepos.cpp | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/tasks/eepos.cpp b/src/tasks/eepos.cpp
index 734d99b..7c0c4a1 100644
--- a/src/tasks/eepos.cpp
+++ b/src/tasks/eepos.cpp
@@ -111,7 +111,6 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
   // Task Error ________________________________________________
 
   // End-Effector Position Error 
-
   sigmaEEPOS.block(0,0,3,1) = eepos_des.block(0,0,3,1) - HT.cam_in_w.block(0,3,3,1);
 
   // End-Effector Orientation Error
@@ -169,8 +168,8 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
   JEEPOS.block(0,6,6,arm.nj) = omega2euldiff_in_w*JEEPOS_omw.block(0,6,6,arm.nj); 
 
   // task jacobian pseudo inverse
-  //JEEPOS_pseudo = UAM::CCommon_Fc::WeightInvJac(JEEPOS,vs_delta_gain,vs_alpha_min,sigmaEEPOS.block(0,0,3,1).norm());
-  JEEPOS_pseudo = JEEPOS.transpose();
+  JEEPOS_pseudo = UAM::CCommon_Fc::WeightInvJac(JEEPOS,vs_delta_gain,vs_alpha_min,sigmaEEPOS.block(0,0,3,1).norm());
+
   // Return reduced Jacobians (without roll and pitch columns)
   JEEPOS_reduced = Eigen::MatrixXd::Zero(6,4+arm.nj);
   JEEPOS_reduced.block(0,0,6,3) = JEEPOS.block(0,0,6,3);
-- 
GitLab