From dc5fff77508cb4c371596ed9d76e86ee68f247e6 Mon Sep 17 00:00:00 2001
From: asantamaria <asantamaria@iri.upc.edu>
Date: Mon, 29 Aug 2016 16:49:10 +0200
Subject: [PATCH] debugging sgfault
---
src/common_fc.cpp | 9 +++++++++
src/common_fc.h | 8 +-------
src/common_obj.h | 4 ++--
src/kinematics.cpp | 4 +++-
src/kinematics.h | 1 -
5 files changed, 15 insertions(+), 11 deletions(-)
diff --git a/src/common_fc.cpp b/src/common_fc.cpp
index 7633a25..470e8a4 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 4ff7765..b0944c3 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 95130d6..42b9729 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 5f8ef38..2965b76 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 442dffd..f4c1e31 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"
--
GitLab