fixed some public variables and generic obstacle distance

src/quadarm_task_priority_ctrl.cpp

@@ -16,22 +16,23 @@ CQuadarm_Task_Priority_Ctrl::~CQuadarm_Task_Priority_Ctrl()
 }
 
 // Main public function
-void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_height,
-                                                             const goal_obj& goal,
+void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const goal_obj& goal,
+                                                             const MatrixXd& obstacle_dist,
                                                              ht& HT,
                                                              arm& arm,
                                                              const quadrotor& quad,
                                                              ctrl_params& ctrl_params,
+                                                             cog_data& arm_cog_data,
 																														 MatrixXd& robot_pos,
                                                              MatrixXd& robot_vel)
 {
-
-  // Current quadrotor height
-  this->quad_height_ = quad_height;
-
   // Goal related objects
   this->cam_vel_ = goal.cam_vel;
   this->target_dist_ = goal.target_dist;
+  
+  // Distance to a detected obstacle (0 = no obstacle)
+  this->obstacle_dist_ = obstacle_dist;
+
   // Homogeneous transformations
   this->T_ = HT;
   // Control parameters
@@ -57,9 +58,11 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_
   HT = this->T_; // DEBUG. TODO: make it constant and do not use this trans. outside.
   arm.T_base_to_joint = this->arm_.T_base_to_joint; // DEBUG. TODO: make it constant and do not use this trans. outside.
   ctrl_params = this->ctrl_params_;
+  arm_cog_data = this->arm_cog_data_;
   robot_pos.block(0,0,6,1) = this->quad_.pos;
   robot_pos.block(6,0,this->arm_.nj,1) = this->arm_.jnt_pos;
   robot_vel = this->uam_.vel;
+
 }
 
 // Kinematics
@@ -314,32 +317,32 @@ 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_;
-
   // Inflation radius
   MatrixXd inf_radius = MatrixXd::Zero(1,1);
   inf_radius(0,0) = this->ctrl_params_.inf_radius;
 
   // Task vector 
-  sigmaIR = d_obs.transpose()*d_obs-inf_radius;
+  MatrixXd obs_euc_dist(1,1);
+  obs_euc_dist(0,0) = std::sqrt(std::pow(this->obstacle_dist_(0,0),2)+std::pow(this->obstacle_dist_(1,0),2)+std::pow(this->obstacle_dist_(2,0),2));
+  sigmaIR = inf_radius-obs_euc_dist;
 
   // Task Jacobian
   JIR = MatrixXd::Zero(1,4+this->arm_.nj);
-  MatrixXd Jtemp = d_obs;
+  MatrixXd Jtemp = this->obstacle_dist_;
   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 (d_obs(ii,0) < inf_radius(0,0) && d_obs(ii,0) > 0.0)
+  {
+    if (this->obstacle_dist_(ii,0) < inf_radius(0,0) && this->obstacle_dist_(ii,0) > 0.0)
     {
-      cout << "WARNING!! Inflation Radius violation. DOF: " << ii << " Inf. radius: " << inf_radius(0,0) << " Obstacle distance: " << d_obs(ii,0) << endl;
+      cout << "WARNING!! Inflation Radius violation. DOF: " << ii << " Inf. radius: " << inf_radius(0,0) << " Obstacle distance: " << this->obstacle_dist_(ii,0) << endl;
       H(ii,ii) = 1.0;
     }
-
+  } 
+  
   MatrixXd Hinv = calcPinv(H);
 
   // weighting matrix
@@ -348,9 +351,11 @@ void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo
 
   // cout << "xxxxxx Task velocities xxxxxx" << endl;
   // cout << JIR_pseudo*sigmaIR << endl;
+  // cout << "______ Sigma (IR) _____" << endl;
+  // cout << sigmaIR.transpose() << endl;  
   // cout << "______ Task Jacobian _____" << endl;
   // cout << JIR << endl;
-  // cout << "_______ Pseudo Inverse of Task Jacobian ____" << endl;
+  // cout << "_______ Pseudo Inverse of Task Jacobian (IR) ____" << endl;
   // cout << JIR_pseudo << endl;
   // cout << "+++++++++" << endl;
 }

src/quadarm_task_priority_ctrl.h

@@ -35,12 +35,6 @@ using namespace Eigen;
 using namespace KDL;
 using namespace std;
 
-// Goal related objects
-typedef struct{
-	MatrixXd cam_vel; // Desired camera velocity (end-effector)
-    double target_dist; // Target distance
-}goal_obj;
-
 // Homogeneous Transforms
 typedef struct{
     Matrix4d tag_in_cam; // Tag in camera frames.
@@ -51,6 +45,22 @@ typedef struct{
     Matrix4d armbase_in_baselink; // Camera in base_link frames.
 }ht;
 
+// VS Goal related objects
+typedef struct{
+    MatrixXd cam_vel; // Desired camera velocity (end-effector)
+    double target_dist; // Target distance
+}goal_obj;
+
+// CoG related objects
+typedef struct{
+   vector<MatrixXd> link_cog; // Vector of link CoG relative to base 
+   MatrixXd arm_cog; // Arm CoG coordinates w.r.t. arm base link;
+}cog_data;
+
+typedef struct{
+   bool vs_weight_psinv; // If using weighted pseudo inverse of the visual servo Jacobian
+}config;
+
 // Arm joint definition
 typedef struct{
     string link_parent; // Link parent name.
@@ -108,10 +118,6 @@ typedef struct{
     MatrixXd v_rollpitch; // Quadrotor roll and pitch angular velocities.
 }ctrl_params;
 
-typedef struct{
-   vector<MatrixXd> link_cog; // Vector of link CoG relative to base 
-   MatrixXd arm_cog; // Arm CoG coordinates w.r.t. arm base link;
-}cog_data;
 
 class CQuadarm_Task_Priority_Ctrl
 {
@@ -128,11 +134,13 @@ class CQuadarm_Task_Priority_Ctrl
 
     ht T_; // Homogeneous Transforms
 
+    cog_data arm_cog_data_; // Arm CoG data to debug in higher levels.
+
     MatrixXd cam_vel_; // Camera velocities
 
     double target_dist_; // Euclidean distance to target.
 
-    double quad_height_; // Quadrotor ground distance.
+    MatrixXd obstacle_dist_; // Distance to a detected obstacle (0 = no obstacle). Provided externally.
 
     /**
     * \brief Compute Unmanned Aerial Manipulator Kinematics
@@ -234,10 +242,6 @@ class CQuadarm_Task_Priority_Ctrl
 
   public:
 
-    //TODO: these variables are public for debugging purposes
-    // they must be private and set get functions created.
-    cog_data arm_cog_data_; // Arm CoG data to debug in higher levels.
-
   	/**
     * \brief Constructor
     *
@@ -260,12 +264,13 @@ class CQuadarm_Task_Priority_Ctrl
     * Main public function call of Quadarm Task Priority Control.
     *
     */     
-    void quadarm_task_priority_ctrl(const double& quad_height,
-                                    const goal_obj& goal,
+    void quadarm_task_priority_ctrl(const goal_obj& goal,
+                                    const MatrixXd& obstacle_dist,
                                     ht& HT,
                                     arm& arm,
                                     const quadrotor& quad,
                                     ctrl_params& ctrl_params,
+                                    cog_data& arm_cog_data,
 									MatrixXd& robot_pos,
                                     MatrixXd& robot_vel);
     /**