diff --git a/src/kinematics.cpp b/src/kinematics.cpp
index 7080b3310ef1fcc88d2b2a7f8ce81da116ca0c06..5f8ef38d8f0d6b58d1e7c193c0775f5706098db3 100644
--- a/src/kinematics.cpp
+++ b/src/kinematics.cpp
@@ -25,7 +25,7 @@ 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 = Jb2i.inverse();
 
   // Jacobian from quadrotor body to camera
   Eigen::MatrixXd Jb2c = Eigen::MatrixXd::Zero(6,6);
@@ -182,11 +182,15 @@ Eigen::MatrixXd CKinematics::ArmJac(UAM::CArm& arm)
 
   Eigen::MatrixXd Ja = Eigen::MatrixXd::Zero(6,arm.nj);
 
-  for (int ii=0; ii<6; ++ii)
+  for (unsigned int ii=0; ii<6; ++ii)
   {
     for (unsigned int jj=0; jj<arm.nj; ++jj)
       Ja(ii,jj) = arm.jacobian.data(ii,jj);
   }
+
+  // TODO: Define the arm in order to avoid this.
+  Ja.col(arm.nj-1) = -Ja.col(arm.nj-1);
+
   return Ja;
 }
 
diff --git a/src/tasks/eepos.cpp b/src/tasks/eepos.cpp
index 42d373feddc49a649bb7a6f93e4152c915d854c4..9433c9e78087845f3073b7b2a74001ad643f3d79 100644
--- a/src/tasks/eepos.cpp
+++ b/src/tasks/eepos.cpp
@@ -119,7 +119,6 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
   // Task Jacobian _____________________________________________
 
   // Quadrotor
-
   Eigen::Matrix3d Rb_in_w = HT.baselink_in_w.block(0,0,3,3);
   Eigen::Matrix3d Re_in_w = HT.cam_in_w.block(0,0,3,3);
   Eigen::Matrix3d Re_in_b = HT.cam_in_baselink.block(0,0,3,3);
@@ -132,13 +131,6 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
   Eigen::Matrix3d wTb_inv = wTb.inverse();
   Eigen::Matrix3d wTe = UAM::CCommon_Fc::GetWronskian(euRe_in_w(0),euRe_in_w(1));
 
-
-  Eigen::MatrixXd JEEPOS_omw = Eigen::MatrixXd::Zero(6,6+arm.nj);
-  JEEPOS_omw.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3); 
-  JEEPOS_omw.block(3,0,3,3) = Eigen::MatrixXd::Zero(3,3);
-  JEEPOS_omw.block(0,3,3,3) = -Rb_in_w*(UAM::CCommon_Fc::Skew(pe_in_b))*wTb_inv;
-  JEEPOS_omw.block(3,3,3,3) = Re_in_w*Re_in_b.transpose()*wTb_inv;
-
   // TODO: Remove
   // Eigen::Matrix3d Aux_Frame_Matrix1 = Eigen::Vector3d(1,-1,1).asDiagonal();
   // Eigen::Matrix3d Aux_Frame_Matrix1 = Eigen::Matrix3d::Zero();
@@ -147,6 +139,12 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
   // Aux_Frame_Matrix1(2,0) = 1.0;
   //JEEPOS_omw.block(0,3,3,3) = -Rb_in_w*(UAM::CCommon_Fc::Skew(Aux_Frame_Matrix1*pe_in_b))*wTb_inv;
   
+  Eigen::MatrixXd JEEPOS_omw = Eigen::MatrixXd::Zero(6,6+arm.nj);
+  JEEPOS_omw.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3); 
+  JEEPOS_omw.block(3,0,3,3) = Eigen::MatrixXd::Zero(3,3);
+  JEEPOS_omw.block(0,3,3,3) = -Rb_in_w*(UAM::CCommon_Fc::Skew(pe_in_b))*wTb_inv;
+  JEEPOS_omw.block(3,3,3,3) = Re_in_w*Re_in_b.transpose()*wTb_inv;
+
   // Arm
   Eigen::MatrixXd ArmJac = UAM::CKinematics::ArmJac(arm);
 
@@ -156,18 +154,14 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
   RRb_in_w.block(3,3,3,3) = Rb_in_w*Rarmb_in_b;
   JEEPOS_omw.block(0,6,6,arm.nj) = RRb_in_w * ArmJac;
 
-  Eigen::MatrixXd Angvel_in_w2euldiff_in_w = Eigen::MatrixXd::Zero(6,6);
-  Angvel_in_w2euldiff_in_w.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3);
-  Angvel_in_w2euldiff_in_w.block(3,3,3,3) = wTe*Re_in_w.transpose();
+  Eigen::MatrixXd omega2euldiff_in_w = Eigen::MatrixXd::Zero(6,6);
+  omega2euldiff_in_w.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3);
+  omega2euldiff_in_w.block(3,3,3,3) = wTe*Re_in_w.transpose();
   
-  JEEPOS.block(0,0,6,6) = Angvel_in_w2euldiff_in_w*JEEPOS_omw.block(0,0,6,6); 
-  JEEPOS.block(0,6,6,arm.nj) = Angvel_in_w2euldiff_in_w*JEEPOS_omw.block(0,6,6,arm.nj); 
-
-  // TODO: WTF is this?
-  JEEPOS.col(5+arm.nj) = -JEEPOS.col(5+arm.nj);
+  JEEPOS.block(0,0,6,6) = omega2euldiff_in_w*JEEPOS_omw.block(0,0,6,6); 
+  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::CalcPinv(JEEPOS);
   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)