diff --git a/src/common_fc.cpp b/src/common_fc.cpp
index 3828dfdfdf44b0d6430c62c1dcf827dabce7ce19..295d30f4cb6d0ea4a0b1b83cf61fc9417eed376c 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 2523d17912b14ee6b5015a874fcf7353bae39116..fdf1e5fd157d6f6fb61cab22cc8ce851f6b545d7 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 7080b3310ef1fcc88d2b2a7f8ce81da116ca0c06..0ec0f0ed13d2132f4446ee998cb34a8149b460f7 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 2db32f6cc2a57fbbbef8d8a81d3ee86d78b1e9be..2437b852168d9de7fb26c01036807012110bc83a 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 9b972d6def93f450f3a2d67c76de204e0cb314ef..4e8f5047d7b291292bf64555b5d1d48d223959a3 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