added security task

src/quadarm_task_priority_ctrl.cpp

@@ -309,58 +309,31 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
   this->vel_.quadarm.block(0,0,3,1) = Vtotal.block(0,0,3,1);
   this->vel_.quadarm.block(5,0,1+this->arm_.nj,1) = Vtotal.block(3,0,1+this->arm_.nj,1);
 }
-void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,MatrixXd& sigmaVS)
-{
-  // Underactuated Quadrotor 
-  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);
-  Jqa1.block(0,3,6,1+this->arm_.nj) = this->Jqa_.block(0,5,6,1+this->arm_.nj);
-  Jqa2 = this->Jqa_.block(0,3,6,2);
-  MatrixXd eye = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);
-
-  // task jacobian
-  JVS = Jqa1;
-
-  // task jacobian pseudo inverse 
-  //JVS_pseudo = calcPinv(Jqa1);
-  JVS_pseudo = weight_jacvs_inverse(Jqa1);
-
-  // task velocity vector
-  sigmaVS = this->vel_.cam-Jqa2*this->ctrl_params_.v_rollpitch;
-}
 void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo,MatrixXd& sigmaIR)
 {
-  // Get diagonal gain matrix
-  MatrixXd Kp=MatrixXd::Zero(3,3);
-  for (unsigned int ii = 0; ii < 3; ++ii)
-    Kp(ii,ii) = 1;
-
-  // Current distance to obstacle (initialize far away)
-  MatrixXd obs_dist = 100*MatrixXd::Ones(3,1);
-  obs_dist(2,0) = this->quad_height_; // ground distance
+  // Distance to the obstacle (currently detecting only the ground)
+  MatrixXd d_obs = MatrixXd::Zero(3,1);
+  d_obs(2,0) = this->quad_height_;
 
   // Inflation radius
-  MatrixXd inf_rad = this->ctrl_params_.inf_radius*MatrixXd::Ones(3,1);
+  MatrixXd inf_radius = MatrixXd::Zero(1,1);
+  inf_radius(0,0) = this->ctrl_params_.inf_radius;
 
-  // Current error 
-  MatrixXd e_infrad = obs_dist - inf_rad;
-  sigmaIR = e_infrad.transpose()*Kp*e_infrad;
+  // Task vector 
+  sigmaIR = d_obs.transpose()*d_obs-inf_radius;
 
   // Task Jacobian
   JIR = MatrixXd::Zero(1,4+this->arm_.nj);
-  MatrixXd Jtemp = Kp*e_infrad;
-  JIR.block(0,0,1,3) = -2*Jtemp.transpose();
-
+  MatrixXd Jtemp = d_obs;
+  JIR.block(0,0,1,3) = 2*Jtemp.transpose();
 
   // Bloquing matrix
   MatrixXd H = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
 
   for (unsigned int ii = 0; ii < 3; ++ii)
-    if (e_infrad(ii,0) < 0)
+    if (d_obs(ii,0) < inf_radius(0,0) && d_obs(ii,0) > 0.0)
     {
-      cout << "WARNING!! Inflation Radius violation. DOF: " << ii << " Inf. radius: " << inf_rad(ii,0) << " Obstacle distance: " << obs_dist(ii,0) << endl;
+      cout << "WARNING!! Inflation Radius violation. DOF: " << ii << " Inf. radius: " << inf_radius(0,0) << " Obstacle distance: " << d_obs(ii,0) << endl;
       H(ii,ii) = 1.0;
     }
 
@@ -369,6 +342,35 @@ void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo
   // weighting matrix
   MatrixXd temp = JIR*Hinv*JIR.transpose();
   JIR_pseudo = Hinv*JIR.transpose()*calcPinv(temp);
+
+  // cout << "xxxxxx Task velocities xxxxxx" << endl;
+  // cout << JIR_pseudo*sigmaIR << endl;
+  // cout << "______ Task Jacobian _____" << endl;
+  // cout << JIR << endl;
+  // cout << "_______ Pseudo Inverse of Task Jacobian ____" << endl;
+  // cout << JIR_pseudo << endl;
+  // cout << "+++++++++" << endl;
+}
+void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,MatrixXd& sigmaVS)
+{
+  // Underactuated Quadrotor 
+  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);
+  Jqa1.block(0,3,6,1+this->arm_.nj) = this->Jqa_.block(0,5,6,1+this->arm_.nj);
+  Jqa2 = this->Jqa_.block(0,3,6,2);
+  MatrixXd eye = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);
+
+  // task jacobian
+  JVS = Jqa1;
+
+  // task jacobian pseudo inverse 
+  //JVS_pseudo = calcPinv(Jqa1);
+  JVS_pseudo = weight_jacvs_inverse(Jqa1);
+
+  // task velocity vector
+  sigmaVS = this->vel_.cam-Jqa2*this->ctrl_params_.v_rollpitch;
 }
 void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,MatrixXd& sigmaG)
 {