debugging sgfault

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

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

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

@@ -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"