minor changes to add obstacle avoidance simulation

src/quadarm_task_priority_ctrl.cpp

@@ -16,7 +16,7 @@ CQuadarm_Task_Priority_Ctrl::~CQuadarm_Task_Priority_Ctrl()
 }
 
 // Main public function
-void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_height,
+void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const MatrixXd& quad_dist_obs,
                                                              const goal_obj& goal,
                                                              ht& HT,
                                                              arm& arm,
@@ -27,7 +27,7 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_
 {
 
   // Current quadrotor height
-  this->quad_height_ = quad_height;
+  this->quad_dist_obs_ = quad_dist_obs;
 
   // Goal related objects
   this->cam_vel_ = goal.cam_vel;
@@ -330,8 +330,7 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
 void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo,MatrixXd& sigmaIR)
 {
   // Distance to the obstacle (currently detecting only the ground)
-  MatrixXd d_obs = MatrixXd::Zero(3,1);
-  d_obs(2,0) = this->quad_height_;
+  MatrixXd d_obs = this->quad_dist_obs_;
 
   // Inflation radius
   MatrixXd inf_radius = MatrixXd::Zero(1,1);
@@ -339,7 +338,8 @@ void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo
 
   // Task vector 
   Eigen::MatrixXd euc_d_obs = (d_obs.transpose()*d_obs).array().sqrt();
-  sigmaIR = inf_radius-euc_d_obs;
+  // sigmaIR = inf_radius-euc_d_obs;
+  sigmaIR = euc_d_obs-inf_radius;
 
   // Task Jacobian
   JIR = MatrixXd::Zero(1,4+this->arm_.nj);
@@ -349,12 +349,19 @@ void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo
   // Bloquing matrix
   MatrixXd H = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
 
-  for (unsigned int ii = 0; ii < 3; ++ii)
-    if (d_obs(ii,0) < inf_radius(0,0) && d_obs(ii,0) > 0.0)
+  // Euclidean distance to the obstacle
+  // if (sqrt(pow(d_obs(0,0),2)+pow(d_obs(1,0),2)+pow(d_obs(2,0),2))<inf_radius(0,0)) 
+  // {
+    // DOF to block
+    for (unsigned int ii = 0; ii < 3; ++ii)
     {
-      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;
+      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_radius(0,0) << " Obstacle distance: " << d_obs(ii,0) << endl;
+        H(ii,ii) = 1.0;
+      }
     }
+  // }
 
   MatrixXd Hinv = calcPinv(H);
 

src/quadarm_task_priority_ctrl.h

@@ -140,7 +140,7 @@ class CQuadarm_Task_Priority_Ctrl
 
     double target_dist_; // Euclidean distance to target.
 
-    double quad_height_; // Quadrotor ground distance.
+    MatrixXd quad_dist_obs_; // Quadrotor distance to obstacle.
 
     string datetime_; // Initial date and time.
 
@@ -281,7 +281,7 @@ class CQuadarm_Task_Priority_Ctrl
     * Main public function call of Quadarm Task Priority Control.
     *
     */     
-    void quadarm_task_priority_ctrl(const double& quad_height,
+    void quadarm_task_priority_ctrl(const MatrixXd& quad_dist_obs,
                                     const goal_obj& goal,
                                     ht& HT,
                                     arm& arm,