diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index d6a820703f73a0e49355a0874ca0d97cf32c9d9a..1432e04663140083d22ffa5bbb9324d88cca3f40 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -291,10 +291,7 @@ 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; 
-
-//TODO
-Vtotal = JVS_wpseudo * sigmaVS;
+  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_.lambda_robot.array()*Vtotal.array();
 
@@ -366,8 +363,6 @@ void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,Mat
   JVS_pseudo = calcPinv(Jqa1);
   //JVS_pseudo = weight_jacvs_inverse(Jqa1);
 
-//TODO
-
   // task velocity vector
   sigmaVS = this->cam_vel_-Jqa2*this->ctrl_params_.v_rollpitch;
 }