added ir_gain. to be tested with real robot

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);

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.