rearranging vs task to a single function

src/quadarm_task_priority_ctrl.cpp

@@ -201,7 +201,7 @@ MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian()
 void CQuadarm_Task_Priority_Ctrl::qa_control(){
   
   // Underactuated Quadrotor 
-  MatrixXd J1(8,4),J2(8,2),V1(4,1),Jqa1(6,4+this->arm_.nj),Jqa2(6,2);
+  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);
@@ -217,6 +217,9 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
   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)
@@ -225,7 +228,7 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
   
 
   // Primary control task (Visual Servo) ________________________
-  MatrixXd sigmaVS = this->vel_.cam-Jqa2*this->ctrl_params_.v_rollpitch;
+  
   this->ctrl_params_.vs_vel = Jqa1_wpseudo * sigmaVS;