diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp index b0d45a9e501030136abce0043cf250f1907284f2..06f1415ae1b412b9efb5acacb25ddfd546474fcf 100644 --- a/src/quadarm_task_priority_ctrl.cpp +++ b/src/quadarm_task_priority_ctrl.cpp @@ -326,6 +326,13 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){ // std::cout << "VS eigenvalues:" << std::endl << this->eig_values_.vs << std::endl; // std::cout << "CoG eigenvalues:" << std::endl << this->eig_values_.cog << std::endl; // std::cout << "JL eigenvalues:" << std::endl << this->eig_values_.jl << std::endl; + + // Storing Jacobians to use them externally + this->jacobians_.ir = JIR; + this->jacobians_.vs = JVS; + this->jacobians_.cog = JG; + this->jacobians_.jl = JL; + } 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 fce566c39f417b8b9f6ffb674a42a664188da77d..75766a4d1fb39d7cd4b02c69d342eaf24fbd2a39 100644 --- a/src/quadarm_task_priority_ctrl.h +++ b/src/quadarm_task_priority_ctrl.h @@ -122,6 +122,13 @@ typedef struct{ VectorXd jl; }eig_values; +typedef struct{ + MatrixXd ir; + MatrixXd vs; + MatrixXd cog; + MatrixXd jl; +}jacobians; + class CQuadarm_Task_Priority_Ctrl { private: @@ -258,6 +265,7 @@ class CQuadarm_Task_Priority_Ctrl eig_values eig_values_; // Task jacobian eigenvalues. + jacobians jacobians_; // Task Jacobians. /** * \brief Constructor