diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index f9fa15659518fb1b85d06f1924cd7da2df9c7ef5..47ef9d9c72fd13b94114584ce45cd83eeccbc54e 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -210,8 +210,11 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
   MatrixXd JIR,JIR_pseudo,sigmaIR;
   task_infrad(JIR,JIR_pseudo,sigmaIR);
   
+  // Gain
+  double lambdaIR = this->ctrl_params_.ir_gain;
+
   // task velocity
-  this->ctrl_params_.ir_vel = JIR_pseudo * sigmaIR;
+  this->ctrl_params_.ir_vel = JIR_pseudo * lambdaIR * sigmaIR;
 
   // TASK 1: Visual Servo _________________________________________
   
@@ -323,7 +326,8 @@ void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo
   inf_radius(0,0) = this->ctrl_params_.inf_radius;
 
   // Task vector 
-  sigmaIR = d_obs.transpose()*d_obs-inf_radius;
+  Eigen::MatrixXd euc_d_obs = (d_obs.transpose()*d_obs).array().sqrt();
+  sigmaIR = inf_radius-euc_d_obs;
 
   // Task Jacobian
   JIR = MatrixXd::Zero(1,4+this->arm_.nj);
diff --git a/src/quadarm_task_priority_ctrl.h b/src/quadarm_task_priority_ctrl.h
index 543a7fed3ed0d4b97cf778b5abe98af5ea8fa5dc..dcf60fa426126e8ad59417b07cb710e1648aafd0 100644
--- a/src/quadarm_task_priority_ctrl.h
+++ b/src/quadarm_task_priority_ctrl.h
@@ -97,6 +97,7 @@ typedef struct{
     MatrixXd vs_vel;  // Primary task velocity.      
     double vs_alpha_min; // Alpha value for gain matrix pseudo inverse.
     MatrixXd vs_delta_gain; // Delta values (min and max) for gain matrix pseudo inverse W.
+    double ir_gain; // Gain of Inflation radius task.
     double inf_radius; // Security distance to an obstacle (Inflation radius) to prevent Collisions.
     double cog_gain; // Gain of CoG alignment secondary task.
     MatrixXd cog_vel; // Secondary task velocity.