after merge

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) ______________________

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

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(){};
 };

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);

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"

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);