diff --git a/src/common_fc.cpp b/src/common_fc.cpp
index 7633a2547384d95d4b5590cc38a0abdeab904ba1..d9cc2d1f677533a66d3e27524228995b3f17e598 100644
--- a/src/common_fc.cpp
+++ b/src/common_fc.cpp
@@ -6,6 +6,59 @@ 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;
+
+// see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
+
+const Eigen::MatrixXd* m;
+Eigen::MatrixXd t;
+Eigen::MatrixXd m_pinv;
+
+// transpose so SVD decomp can work...
+if ( a.rows()<a.cols() )
+{
+ t = a.transpose();
+ m = &t;
+}
+else
+ m = &a;
+
+// SVD
+//JacobiSVD<Eigen::MatrixXd> svd = m->jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
+Eigen::JacobiSVD<Eigen::MatrixXd> svd = m->jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
+Eigen::MatrixXd vSingular = svd.singularValues();
+// Build a diagonal matrix with the Inverted Singular values
+// The pseudo inverted singular matrix is easy to compute :
+// is formed by replacing every nonzero entry by its reciprocal (inversing).
+Eigen::MatrixXd vPseudoInvertedSingular(svd.matrixV().cols(),1);
+for (int iRow =0; iRow<vSingular.rows(); iRow++)
+{
+ if ( fabs(vSingular(iRow))<=epsilon )
+ vPseudoInvertedSingular(iRow,0)=0.;
+ else
+ vPseudoInvertedSingular(iRow,0)=1./vSingular(iRow);
+}
+// A little optimization here
+Eigen::MatrixXd mAdjointU = svd.matrixU().adjoint().block(0,0,vSingular.rows(),svd.matrixU().adjoint().cols());
+// Pseudo-Inversion : V * S * U'
+m_pinv = (svd.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU ;
+
+// transpose back if necessary
+if ( a.rows()<a.cols() )
+ return m_pinv.transpose();*/
+
+Eigen::MatrixXd m_pinv;
+m_pinv = a.transpose();
+
+return m_pinv;
+}
+
+
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 3c4b37f13f0b6132f3155b5814d758e3bdb66f0d..b0944c36d52da609540ad158df28950c9be98511 100644
--- a/src/common_fc.h
+++ b/src/common_fc.h
@@ -14,8 +14,6 @@
// Eigen
#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Eigenvalues>
-#include <eigen3/Eigen/SVD>
// KDL
#include <kdl/frames.hpp>
@@ -59,14 +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();
- _Matrix_Type_ mpinv = a.transpose();
- 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 e8849c02cb2a6f9032a31ecef2b6b87ceb1a5103..f4c1e31d2381392d35adacb30348b519f2cf91bc 100644
--- a/src/kinematics.h
+++ b/src/kinematics.h
@@ -3,8 +3,6 @@
// Eigen
#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Eigenvalues>
-#include <eigen3/Eigen/SVD>
// KDL stuff
#include <kdl/frames.hpp>
@@ -19,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"
diff --git a/src/tasks/eepos.cpp b/src/tasks/eepos.cpp
index eb56644d3c82a30791f3561541abdd5d929895c5..734d99bd874a39d31486ed4cc832b58b5ae2e75f 100644
--- a/src/tasks/eepos.cpp
+++ b/src/tasks/eepos.cpp
@@ -153,8 +153,12 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
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);
+ // TODO: resolve why these blocks are different
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;
+ // 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);
+
JEEPOS_omw.block(0,6,6,arm.nj) = RRb_in_w * ArmJac;
Eigen::MatrixXd omega2euldiff_in_w = Eigen::MatrixXd::Zero(6,6);