added eigen values of jacobians

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

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
     *