Corrected matrix JACEEPOS, from robot arm to translation

src/tasks/eepos.cpp

@@ -111,8 +111,9 @@ 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
   sigmaEEPOS.block(3,0,3,1) = eepos_des.block(3,0,3,1) -  RToEulerContinuous(HT.cam_in_w.block(0,0,3,3),eepos_des.block(3,0,3,1));
 
@@ -151,8 +152,9 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
   Eigen::MatrixXd RRb_in_w = Eigen::MatrixXd::Zero(6,6);
   Eigen::Matrix3d Rarmb_in_b = HT.armbase_in_baselink.block(0,0,3,3);
   Eigen::Matrix3d Rl1_in_armb = HT.link1_in_armbase.block(0,0,3,3);
-  RRb_in_w.block(0,0,3,3) = Rb_in_w * Rarmb_in_b * Rl1_in_armb;
-  RRb_in_w.block(3,3,3,3) = RRb_in_w.block(0,0,3,3);
+
+  RRb_in_w.block(0,0,3,3) =  Rb_in_w;
+  RRb_in_w.block(3,3,3,3) =  Rb_in_w * Rarmb_in_b * Rl1_in_armb;
   JEEPOS_omw.block(0,6,6,arm.nj) = RRb_in_w * ArmJac;
 
   Eigen::MatrixXd omega2euldiff_in_w = Eigen::MatrixXd::Zero(6,6);
@@ -163,8 +165,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 = UAM::CCommon_Fc::WeightInvJac(JEEPOS,vs_delta_gain,vs_alpha_min,sigmaEEPOS.block(0,0,3,1).norm());
+  JEEPOS_pseudo = JEEPOS.transpose();
   // 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);