cleaned code and VS moved to a single function. Starting with Inflation radius task

src/quadarm_task_priority_ctrl.cpp

@@ -211,30 +211,18 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
 
   // TASK: Visual Servo _________________________________________
   
-  // Underactuated Quadrotor 
-  MatrixXd J1(8,4),J2(8,2),Jqa1(6,4+this->arm_.nj),Jqa2(6,2);
-
-  // 9 DOF Extracting wx and wy from quadrotor 
-  Jqa1.block(0,0,6,3) = this->Jqa_.block(0,0,6,3);
-  Jqa1.block(0,3,6,1+this->arm_.nj) = this->Jqa_.block(0,5,6,1+this->arm_.nj);
-  Jqa2 = this->Jqa_.block(0,3,6,2);
-  MatrixXd eye(4+this->arm_.nj,4+this->arm_.nj);
-  eye = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);
-
-  // compute pseudo inverse 
-  //MatrixXd Jqa1_pseudo(4+this->arm_.nj,6);
-  //Jqa1_pseudo = calcPinv(Jqa1);
-  MatrixXd Jqa1_wpseudo(4+this->arm_.nj,6);
-  Jqa1_wpseudo = weight_jacvs_inverse(Jqa1);
-
-  MatrixXd sigmaVS = this->vel_.cam-Jqa2*this->ctrl_params_.v_rollpitch;
+  // task Jacobian and sigma
+  MatrixXd JVS,JVS_wpseudo,sigmaVS;
+  task_vs(JVS,JVS_wpseudo,sigmaVS);
 
-  this->ctrl_params_.vs_vel = Jqa1_wpseudo * sigmaVS;
+  // task velocity
+  this->ctrl_params_.vs_vel = JVS_wpseudo * sigmaVS;
 
   // TASK: CoG alignement ______________________________________
 
   // Null space projector
-  MatrixXd Nqa1 = (eye-(Jqa1_wpseudo*Jqa1));
+  MatrixXd eye = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);
+  MatrixXd Nqa1 = (eye-(JVS_wpseudo*JVS));
   
   // task Jacobian and sigma
   MatrixXd JG,JG_pseudo,sigmaG;
@@ -249,14 +237,14 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
   // Third control task (Joint limits) ________________________
   
   // Augmented null space projector from Augmented Jacobian
-  MatrixXd Jqa1_G = MatrixXd::Zero(Jqa1.rows()+JG.rows(),Jqa1.cols());
-  Jqa1_G.block(0,0,Jqa1.rows(),Jqa1.cols()) = Jqa1;
-  Jqa1_G.block(Jqa1.rows()-1,0,JG.rows(),JG.cols())=JG; 
+  MatrixXd JVS_G = MatrixXd::Zero(JVS.rows()+JG.rows(),JVS.cols());
+  JVS_G.block(0,0,JVS.rows(),JVS.cols()) = JVS;
+  JVS_G.block(JVS.rows()-1,0,JG.rows(),JG.cols())=JG; 
   MatrixXd Nqa1_G = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
-  MatrixXd Jqa1_G_pseudo = calcPinv(Jqa1_G);
+  MatrixXd JVS_G_pseudo = calcPinv(JVS_G);
   
   // Third task after secondary aligning CoG
-  Nqa1_G = (eye-(Jqa1_G_pseudo*Jqa1_G));
+  Nqa1_G = (eye-(JVS_G_pseudo*JVS_G));
   //Nqa1_G = Nqa1; // Third task as secondary
 
   // task Jacobian and sigma
@@ -298,9 +286,26 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
   this->vel_.quadarm.block(0,0,3,1) = Vtotal.block(0,0,3,1);
   this->vel_.quadarm.block(5,0,1+this->arm_.nj,1) = Vtotal.block(3,0,1+this->arm_.nj,1);
 }
-void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& Jqa1,MatrixXd& Jqa1_wpseudo,MatrixXd& sigmaVS)
+void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,MatrixXd& sigmaVS)
 {
+  // Underactuated Quadrotor 
+  MatrixXd J1(8,4),J2(8,2),Jqa1(6,4+this->arm_.nj),Jqa2(6,2);
+
+  // 9 DOF Extracting wx and wy from quadrotor 
+  Jqa1.block(0,0,6,3) = this->Jqa_.block(0,0,6,3);
+  Jqa1.block(0,3,6,1+this->arm_.nj) = this->Jqa_.block(0,5,6,1+this->arm_.nj);
+  Jqa2 = this->Jqa_.block(0,3,6,2);
+  MatrixXd eye = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);
+
+  // task jacobian
+  JVS = Jqa1;
+
+  // task jacobian pseudo inverse 
+  //JVS_pseudo = calcPinv(Jqa1);
+  JVS_pseudo = weight_jacvs_inverse(Jqa1);
 
+  // task velocity vector
+  sigmaVS = this->vel_.cam-Jqa2*this->ctrl_params_.v_rollpitch;
 }
 void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo,MatrixXd& sigmaIR)
 {

src/quadarm_task_priority_ctrl.h

@@ -177,7 +177,7 @@ class CQuadarm_Task_Priority_Ctrl
     * Compute vector and jacobians of the task corresponding to the visual servoing. 
     *
     */ 
-    void task_vs(MatrixXd& Jqa1,MatrixXd& Jqa1_wpseudo,MatrixXd& sigmaVS);
+    void task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,MatrixXd& sigmaVS);
 
     /**
     * \brief Task: Security inflation radius