Before isolating visual servo into a single function

src/quadarm_task_priority_ctrl.cpp

@@ -200,56 +200,50 @@ MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian()
 // Control
 void CQuadarm_Task_Priority_Ctrl::qa_control(){
   
+  // Control law ________________________________________________________________
+
+  // TASK: Security task (Inflation radius) _____________________
+
+  // task Jacobian and sigma
+  MatrixXd JIR,JIR_pseudo,sigmaIR;
+  task_infrad(JIR,JIR_pseudo,sigmaIR);
+  
+
+  // 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 //////////////////
+  // 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);
-
+  // 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;
 
-
-  // Control law ________________________________________________________________
-
-  // Secutiry task (Inflation radius)
-  MatrixXd JIR,JIR_pseudo,sigmaIR;
-  task_infrad(JIR,JIR_pseudo,sigmaIR);
-  
-
-  // Primary control task (Visual Servo) ________________________
-  
   this->ctrl_params_.vs_vel = Jqa1_wpseudo * sigmaVS;
 
-
-  // Jqa1
-  // Jqa1_wpseudo
-  // sigmaVS
-
-  // Secondary control task (CoG alignement) ____________________
+  // TASK: CoG alignement ______________________________________
 
   // Null space projector
-  //MatrixXd Nqa1 = (eye-(Jqa1_pseudo*Jqa1));
   MatrixXd Nqa1 = (eye-(Jqa1_wpseudo*Jqa1));
   
-  // Secondary task Jacobian and sigma
+  // task Jacobian and sigma
   MatrixXd JG,JG_pseudo,sigmaG;
   task_cog(JG,JG_pseudo,sigmaG);
   
   // Gain
   double lambdaG = this->ctrl_params_.cog_gain;
   
-  // Secondary task velocity
+  // task velocity
   this->ctrl_params_.cog_vel = Nqa1 * JG_pseudo * lambdaG * sigmaG;
 
   // Third control task (Joint limits) ________________________
@@ -260,21 +254,19 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
   Jqa1_G.block(Jqa1.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);
+  
   // Third task after secondary aligning CoG
   Nqa1_G = (eye-(Jqa1_G_pseudo*Jqa1_G));
+  //Nqa1_G = Nqa1; // Third task as secondary
 
-  // Third task as secondary
-  //Nqa1_G = Nqa1;
-
-  // Sec task Jacobian and sigma
+  // task Jacobian and sigma
   MatrixXd JL,JL_pseudo,sigmaL;
   task_jl(JL,JL_pseudo,sigmaL);
   
-
   // Gain
   double lambdaL = this->ctrl_params_.jntlim_gain;
 
-  // Third task velocity
+  // task velocity
   this->ctrl_params_.jntlim_vel = Nqa1_G * JL_pseudo * lambdaL * sigmaL;
 
   //******************************************************************