diff --git a/src/common_fc.cpp b/src/common_fc.cpp
index 7633a2547384d95d4b5590cc38a0abdeab904ba1..470e8a4e59b4419678cbd30bb048977a8c0059ae 100644
--- a/src/common_fc.cpp
+++ b/src/common_fc.cpp
@@ -6,6 +6,15 @@ CCommon_Fc::CCommon_Fc(){}
 
 CCommon_Fc::~CCommon_Fc(){}
 
+Eigen::MatrixXd CCommon_Fc::CalcPinv(const Eigen::MatrixXd& a, double epsilon)
+{
+  Eigen::JacobiSVD<Eigen::MatrixXd> svdd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
+  double tolerance = epsilon * std::max(a.cols(), a.rows()) *svdd.singularValues().array().abs()(0);
+  Eigen::MatrixXd mpinv = svdd.matrixV() *  (svdd.singularValues().array().abs() > tolerance).select(svdd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svdd.matrixU().adjoint();
+  return mpinv;
+}
+
+
 Eigen::MatrixXd CCommon_Fc::WeightInvJac(const Eigen::MatrixXd& J, const Eigen::MatrixXd& invjac_delta_gain, const double& invjac_alpha_min, const double& target_dist)
 {
   // W: Weighting matrix (motion distribution) ______________________
diff --git a/src/common_fc.h b/src/common_fc.h
index 4ff7765627dd9fe8fdc2edadb27c8752743763b6..b0944c36d52da609540ad158df28950c9be98511 100644
--- a/src/common_fc.h
+++ b/src/common_fc.h
@@ -57,13 +57,7 @@ class CCommon_Fc
     *
     * Compute the matrix pseudo-inverse using SVD
     */
-    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();
-      return mpinv;
-    }
+    static Eigen::MatrixXd CalcPinv(const Eigen::MatrixXd& a, double epsilon = std::numeric_limits<double>::epsilon());
 
     /**
     * \brief KDL Frame to Homogeneous transform
diff --git a/src/common_obj.h b/src/common_obj.h
index 95130d6711867f65d9167c5f8c556018e31634d2..42b97298428a731b50962185bc5ff07ee3a2504d 100644
--- a/src/common_obj.h
+++ b/src/common_obj.h
@@ -39,7 +39,7 @@ class CArmJoint{
 class ArmCogData{
   public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    std::vector<Eigen::MatrixXd> link_cog; // Vector of link CoG relative to base 
+    std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> > link_cog; // Vector of link CoG relative to base 
     Eigen::MatrixXd arm_cog;               // Arm CoG coordinates w.r.t. arm base link;
 };
 
@@ -56,7 +56,7 @@ class CArm{
     boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver;          // chain solver.
     KDL::JntArray jnt_pos_kdl;                                              // Array of arm positions.
     KDL::Jacobian jacobian;                                                 // Jacobian.
-    std::vector<Eigen::MatrixXd> T_base_to_joint;                           // Homogenous Transforms from arm base to each link.
+    std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> > T_base_to_joint;                           // Homogenous Transforms from arm base to each link.
     CArm(){};
     ~CArm(){};
 };
diff --git a/src/kinematics.cpp b/src/kinematics.cpp
index 5f8ef38d8f0d6b58d1e7c193c0775f5706098db3..2965b76c8cab4d7dde425b7bc02fc084716c9024 100644
--- a/src/kinematics.cpp
+++ b/src/kinematics.cpp
@@ -24,8 +24,10 @@ void CKinematics::UAMKine(const UAM::CQuad& quad, UAM::CArm& arm, UAM::CHT& HT,
   // Quadrotor Jacobian body to inertial frames
   Eigen::MatrixXd Jb2i = QuadJac(quad.pos);
 
+std::cout << __LINE__ << " [" << quad.pos.transpose() << "]" << std::endl;
+
   // Inertial to body frames
-  Eigen::MatrixXd Ji2b = Jb2i.inverse();
+  Eigen::MatrixXd Ji2b = UAM::CCommon_Fc::CalcPinv(Jb2i);
 
   // Jacobian from quadrotor body to camera
   Eigen::MatrixXd Jb2c = Eigen::MatrixXd::Zero(6,6);
diff --git a/src/kinematics.h b/src/kinematics.h
index 442dffd74bc87758274ec7dcf9ff39825c650916..f4c1e31d2381392d35adacb30348b519f2cf91bc 100644
--- a/src/kinematics.h
+++ b/src/kinematics.h
@@ -17,7 +17,6 @@
 #include <kdl/chainiksolvervel_pinv.hpp>
 #include <kdl/chainiksolvervel_pinv_givens.hpp>
 #include <kdl/chainjnttojacsolver.hpp>
-#include <kdl/chainjnttojacsolver.hpp>
 
 // Own stuff
 #include "common_obj.h"