From 3b62c6acdc86e16c2f86a8eeee1f5593ef2c40af Mon Sep 17 00:00:00 2001
From: asantamaria <somriu@gmail.com>
Date: Mon, 9 Nov 2015 20:42:04 +0100
Subject: [PATCH] added eigen values of jacobians
---
src/quadarm_task_priority_ctrl.cpp | 12 ++++++++++++
src/quadarm_task_priority_ctrl.h | 21 +++++++++++++++------
2 files changed, 27 insertions(+), 6 deletions(-)
diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 47ef9d9..92a3370 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -314,6 +314,18 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
this->uam_.vel = MatrixXd::Zero(6+this->arm_.nj,1);
this->uam_.vel.block(0,0,3,1) = Vtotal.block(0,0,3,1);
this->uam_.vel.block(5,0,1+this->arm_.nj,1) = Vtotal.block(3,0,1+this->arm_.nj,1);
+
+ // Eigenvalues check: Singular values (the positive square roots of the nonzero eigenvalues of the corresponding matrix J^T*J)
+ MatrixXd Jvssq = JVS.transpose()*JVS;
+ MatrixXd Jgsq = JG.transpose()*JG;
+ MatrixXd Jlsq = JL.transpose()*JL;
+ this->eig_values_.vs = Jvssq.eigenvalues();
+ this->eig_values_.cog = Jgsq.eigenvalues();
+ this->eig_values_.jl = Jgsq.eigenvalues();
+ // std::cout << "***********" << std::endl << std::endl;
+ // std::cout << "VS eigenvalues:" << std::endl << Jvssq.eigenvalues() << std::endl;
+ // std::cout << "CoG eigenvalues:" << std::endl << Jgsq.eigenvalues()*1e6 << std::endl;
+ // std::cout << "JL eigenvalues:" << std::endl << Jlsq.eigenvalues() << std::endl;
}
void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo,MatrixXd& sigmaIR)
{
diff --git a/src/quadarm_task_priority_ctrl.h b/src/quadarm_task_priority_ctrl.h
index dcf60fa..bee41f0 100644
--- a/src/quadarm_task_priority_ctrl.h
+++ b/src/quadarm_task_priority_ctrl.h
@@ -116,27 +116,33 @@ typedef struct{
MatrixXd arm_cog; // Arm CoG coordinates w.r.t. arm base link;
}cog_data;
+typedef struct{
+ VectorXcd vs;
+ VectorXcd cog;
+ VectorXcd jl;
+}eig_values;
+
class CQuadarm_Task_Priority_Ctrl
{
private:
- uam uam_; // Unmanned Aerial Manipulator
+ uam uam_; // Unmanned Aerial Manipulator.
- arm arm_; // Arm
+ arm arm_; // Arm.
- quadrotor quad_; // Quadrotor
+ quadrotor quad_; // Quadrotor.
ctrl_params ctrl_params_; // Control Law parameters.
- ht T_; // Homogeneous Transforms
+ ht T_; // Homogeneous Transforms.
- MatrixXd cam_vel_; // Camera velocities
+ MatrixXd cam_vel_; // Camera velocities.
double target_dist_; // Euclidean distance to target.
double quad_height_; // Quadrotor ground distance.
- string datetime_; // Initial date and time
+ string datetime_; // Initial date and time.
/**
* \brief Get date and time
@@ -250,6 +256,9 @@ class CQuadarm_Task_Priority_Ctrl
// they must be private and set get functions created.
cog_data arm_cog_data_; // Arm CoG data to debug in higher levels.
+ eig_values eig_values_; // Task jacobian eigenvalues.
+
+
/**
* \brief Constructor
*
--
GitLab