VS working with IBVS. no secondary tasks. No Weighted Pseudoinverse. No saturations

src/quadarm_task_priority_ctrl.cpp

@@ -291,7 +291,10 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
   // Task 0 + 1 + 3
   //Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.jntlim_vel; 
   // All tasks
-  Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel + this->ctrl_params_.jntlim_vel; 
+  //Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel + this->ctrl_params_.jntlim_vel; 
+
+//TODO
+Vtotal = JVS_wpseudo * sigmaVS;
 
   Vtotal = this->ctrl_params_.lambda_robot.array()*Vtotal.array();
 
@@ -360,10 +363,10 @@ void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,Mat
   JVS = Jqa1;
 
   // task jacobian pseudo inverse 
-  //JVS_pseudo = calcPinv(Jqa1);
-  JVS_pseudo = weight_jacvs_inverse(Jqa1);
+  JVS_pseudo = calcPinv(Jqa1);
+  //JVS_pseudo = weight_jacvs_inverse(Jqa1);
 
-  //std::cout << "No weighted pseudo inverse: @line:" << __LINE__ << std::endl;
+//TODO
 
   // task velocity vector
   sigmaVS = this->cam_vel_-Jqa2*this->ctrl_params_.v_rollpitch;