Added options to the launch file (jacvs weighted pseudo inverse and task priority)

src/quadarm_task_priority_ctrl.cpp

@@ -22,6 +22,7 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const goal_obj& goa
                                                              arm& arm,
                                                              const quadrotor& quad,
                                                              ctrl_params& ctrl_params,
+                                                             const options& options,
                                                              cog_data& arm_cog_data,
 																														 MatrixXd& robot_pos,
                                                              MatrixXd& robot_vel)
@@ -29,14 +30,14 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const goal_obj& goa
   // 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
   this->ctrl_params_ = ctrl_params;
+  // Algorithmic options
+  this->options_ = options;
   // Arm data
   this->arm_.chain = arm.chain;
   this->arm_.joint_info= arm.joint_info;
@@ -207,31 +208,28 @@ MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian()
 // Control
 void CQuadarm_Task_Priority_Ctrl::uam_control(){
  
-  // TASK 0: Security task (Inflation radius) _____________________
+  // Task Jacobians and sigmas ____________________________________
 
-  // task Jacobian and sigma
+  // Security task (Inflation radius: IR)
   MatrixXd JIR,JIR_pseudo,sigmaIR;
   task_infrad(JIR,JIR_pseudo,sigmaIR);
-  
-  // task velocity
-  this->ctrl_params_.ir_vel = JIR_pseudo * sigmaIR;
-
-  // TASK 1: Visual Servo _________________________________________
-  
-  // Null space projector
-  MatrixXd eye = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);
-  MatrixXd NIR = (eye-(JIR_pseudo*JIR));
-
-  // task Jacobian and sigma
+  // Visual Servo (VS)
   MatrixXd JVS,JVS_wpseudo,sigmaVS;
   task_vs(JVS,JVS_wpseudo,sigmaVS);
+  // CoG alignement (G)
+  MatrixXd JG,JG_pseudo,sigmaG;
+  task_cog(JG,JG_pseudo,sigmaG);
+  // Joint limits avoidance (L)
+  MatrixXd JL,JL_pseudo,sigmaL;
+  task_jl(JL,JL_pseudo,sigmaL);
 
-  // task velocity
-  this->ctrl_params_.vs_vel = NIR * JVS_wpseudo * sigmaVS;
+  // NULL space projectors ________________________________________
 
-  // TASK 2: CoG alignement ______________________________________
+  // IR 
+  MatrixXd eye = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);
+  MatrixXd NIR = (eye-(JIR_pseudo*JIR));
 
-  // Null space projector
+  // IR + VS 
   MatrixXd JIR_VS = MatrixXd::Zero(JIR.rows()+JVS.rows(),JVS.cols());
   JIR_VS.block(0,0,JIR.rows(),JIR.cols()) = JIR;
   JIR_VS.block(JIR.rows()-1,0,JVS.rows(),JVS.cols())=JVS; 
@@ -239,65 +237,96 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
   MatrixXd JIR_VS_pseudo = calcPinv(JIR_VS);
   NIR_VS = (eye-(JIR_VS_pseudo*JIR_VS));
 
-  // task Jacobian and sigma
-  MatrixXd JG,JG_pseudo,sigmaG;
-  task_cog(JG,JG_pseudo,sigmaG);
-  
-  // Gain
-  double lambdaG = this->ctrl_params_.cog_gain;
+  MatrixXd NIR_VS_G = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
+  MatrixXd NIR_VS_L = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
 
-  // task velocity
-  this->ctrl_params_.cog_vel = NIR_VS * JG_pseudo * lambdaG * sigmaG;
+  if (this->options_.task_prio == IRVSGL)
+  {
+    // IR + VS + G 
+    MatrixXd JIR_VS_G = MatrixXd::Zero(JIR_VS.rows()+JG.rows(),JIR_VS.cols());
+    JIR_VS_G.block(0,0,JIR_VS.rows(),JIR_VS.cols()) = JIR_VS;
+    JIR_VS_G.block(JIR_VS.rows()-1,0,JG.rows(),JG.cols())=JG; 
+    MatrixXd JIR_VS_G_pseudo = calcPinv(JIR_VS_G);
+    NIR_VS_G = (eye-(JIR_VS_G_pseudo*JIR_VS_G));
+  }
+  else if (this->options_.task_prio == IRVSLG)
+  {
+    // IR + VS + L  
+    MatrixXd JIR_VS_L = MatrixXd::Zero(JIR_VS.rows()+JL.rows(),JIR_VS.cols());
+    JIR_VS_L.block(0,0,JIR_VS.rows(),JIR_VS.cols()) = JIR_VS;
+    JIR_VS_L.block(JIR_VS.rows()-1,0,JL.rows(),JL.cols())=JL; 
+    MatrixXd JIR_VS_L_pseudo = calcPinv(JIR_VS_L);
+    NIR_VS_L = (eye-(JIR_VS_L_pseudo*JIR_VS_L));
+  }
 
-  // TASK 3: Joint limits ________________________
-  
-  // Augmented null space projector from Augmented Jacobian
-  MatrixXd JIR_VS_G = MatrixXd::Zero(JIR_VS.rows()+JG.rows(),JIR_VS.cols());
-  JIR_VS_G.block(0,0,JIR_VS.rows(),JIR_VS.cols()) = JIR_VS;
-  JIR_VS_G.block(JIR_VS.rows()-1,0,JG.rows(),JG.cols())=JG; 
-  MatrixXd NIR_VS_G = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
-  MatrixXd JIR_VS_G_pseudo = calcPinv(JIR_VS_G);
+  // Total velocities __________________________________________________________
 
-  // Third task after secondary aligning CoG
-  NIR_VS_G = (eye-(JIR_VS_G_pseudo*JIR_VS_G));
+  // IR
+  this->ctrl_params_.ir_vel = JIR_pseudo * sigmaIR;
 
-  // task Jacobian and sigma
-  MatrixXd JL,JL_pseudo,sigmaL;
-  task_jl(JL,JL_pseudo,sigmaL);
-  
-  // Gain
-  double lambdaL = this->ctrl_params_.jntlim_gain;
+  // VS
+  this->ctrl_params_.vs_vel = NIR * JVS_wpseudo * sigmaVS;
+
+  MatrixXd Vtotal(4+this->arm_.nj,1);
 
-  // task velocity
-  //this->ctrl_params_.jntlim_vel = NIR_VS * JL_pseudo * lambdaL * sigmaL; // As secondary
-  this->ctrl_params_.jntlim_vel = NIR_VS_G * JL_pseudo * lambdaL * sigmaL;
+  switch  (this->options_.task_prio){
+    case IRVS:
+    {
+      Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel;
+      break;
+    }
+    case IRVSG:
+    {
+      // G
+      double lambdaG = this->ctrl_params_.cog_gain;
+      this->ctrl_params_.cog_vel = NIR_VS * JG_pseudo * lambdaG * sigmaG;
+      // Vtotal = IR + VS + G
+      Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel;
+      break;
+    }
+    case IRVSGL:
+    {
+      // G
+      double lambdaG = this->ctrl_params_.cog_gain;
+      this->ctrl_params_.cog_vel = NIR_VS * JG_pseudo * lambdaG * sigmaG;
+      // L
+      double lambdaL = this->ctrl_params_.jntlim_gain;
+      this->ctrl_params_.jntlim_vel = NIR_VS_G * JL_pseudo * lambdaL * sigmaL;
+      // Vtotal = IR + VS + G + L
+      Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel + this->ctrl_params_.jntlim_vel;
+      break;
+    }
+    case IRVSL:
+    {
+      // L
+      double lambdaL = this->ctrl_params_.jntlim_gain;
+      this->ctrl_params_.jntlim_vel = NIR_VS * JL_pseudo * lambdaL * sigmaL; 
+      // Vtotal = IR + VS + L
+      Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.jntlim_vel;
+      break;
+    }
+    case IRVSLG:
+    {
+      // L
+      double lambdaL = this->ctrl_params_.jntlim_gain;
+      this->ctrl_params_.jntlim_vel = NIR_VS * JL_pseudo * lambdaL * sigmaL; 
+      // G
+      double lambdaG = this->ctrl_params_.cog_gain;
+      this->ctrl_params_.cog_vel = NIR_VS_L * JG_pseudo * lambdaG * sigmaG;            
+      // Vtotal = IR + VS + L + G
+      Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.jntlim_vel + this->ctrl_params_.cog_vel;
+      break;
+    }
+  }
 
-  // Total velocities _______________________________
+  Vtotal = this->ctrl_params_.lambda_robot.array()*Vtotal.array();
 
   //******************************************************************
-  // Weighted sum of subtasks
+  // Weighted sum of subtasks (OLD: comparison purposes)
   // this->ctrl_params_.cog_vel = NIR * (JG_pseudo * lambdaG * sigmaG + JL_pseudo * lambdaL * sigmaL);
   // this->ctrl_params_.jntlim_vel = MatrixXd::Zero(9,1);
 
   //******************************************************************
-  //Total velocity __________________________________________
-  if (!this->ctrl_params_.enable_sec_task) 
-  {
-    this->ctrl_params_.cog_vel = MatrixXd::Zero(4+this->arm_.nj,1);
-    this->ctrl_params_.jntlim_vel = MatrixXd::Zero(4+this->arm_.nj,1);
-    cout << "[Task Priority Control]: Secondary tasks not enabled" << endl;
-  }
-
-  MatrixXd Vtotal(4+this->arm_.nj,1);
-
-  // Task 0 + 1 + 2
-  //Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel; 
-  // Task 0 + 1 + 3
-  //Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.jntlim_vel; 
-  // All tasks
-  Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel + this->ctrl_params_.jntlim_vel; 
-
-  Vtotal = this->ctrl_params_.lambda_robot.array()*Vtotal.array();
 
   // // Debug
   // std::cout << "Task velocities" << std::endl;
@@ -373,8 +402,10 @@ void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,Mat
   JVS = Jqa1;
 
   // task jacobian pseudo inverse 
-  //JVS_pseudo = calcPinv(Jqa1);
-  JVS_pseudo = weight_jacvs_inverse(Jqa1);
+  if (this->options_.jacvs_weight_pinv)
+    JVS_pseudo = jacvs_weight_pinv(Jqa1);
+  else
+    JVS_pseudo = calcPinv(Jqa1);
 
   // task velocity vector
   sigmaVS = this->cam_vel_-Jqa2*this->ctrl_params_.v_rollpitch;
@@ -571,7 +602,7 @@ void CQuadarm_Task_Priority_Ctrl::task_jl(MatrixXd& JL,MatrixXd& JL_pseudo,Matri
 }
 
 // MISC functions
-MatrixXd CQuadarm_Task_Priority_Ctrl::weight_jacvs_inverse(const MatrixXd& J)
+MatrixXd CQuadarm_Task_Priority_Ctrl::jacvs_weight_pinv(const MatrixXd& J)
 {
   // W: Weighting matrix (motion distribution) ______________________
   double w_min = this->ctrl_params_.vs_delta_gain(0,0);

src/quadarm_task_priority_ctrl.h

@@ -57,10 +57,6 @@ typedef struct{
    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.
@@ -97,7 +93,6 @@ typedef struct{
 
 // Control Law parameters
 typedef struct{
-    bool enable_sec_task; // Enable secondary task.
     double dt; // time interval.
     double stabil_gain; // Gain of kinematics stabilization secondary task.
     MatrixXd lambda_robot; // Robot proportional gains.
@@ -118,6 +113,19 @@ typedef struct{
     MatrixXd v_rollpitch; // Quadrotor roll and pitch angular velocities.
 }ctrl_params;
 
+enum task_seq { 
+    IRVS,        // 1: Inflation Radius, 2: Visual Servo
+    IRVSG,       // 1: Inflation Radius, 2: Visual Servo, 3: CoG alignement
+    IRVSGL,      // 1: Inflation Radius, 2: Visual Servo, 3: CoG alignement, 4: Joint limits avoidance
+    IRVSL,       // 1: Inflation Radius, 2: Visual Servo, 3: Joint limits avoidance
+    IRVSLG,      // 1: Inflation Radius, 2: Visual Servo, 3: Joint limits avoidance, 4: CoG alignement
+};
+
+typedef struct{
+   bool jacvs_weight_pinv; // If using weighted pseudo inverse of the visual servo Jacobian
+   task_seq task_prio; // Task priorization
+}options;
+
 
 class CQuadarm_Task_Priority_Ctrl
 {
@@ -132,6 +140,8 @@ class CQuadarm_Task_Priority_Ctrl
 
     ctrl_params ctrl_params_; // Control Law parameters.
 
+    options options_; // Possible algorithmic options.
+
     ht T_; // Homogeneous Transforms
 
     cog_data arm_cog_data_; // Arm CoG data to debug in higher levels.
@@ -231,7 +241,7 @@ class CQuadarm_Task_Priority_Ctrl
     * Compute the Jacobian inverse weighted depending on target distance
     *
     */ 
-    MatrixXd weight_jacvs_inverse(const MatrixXd& J);
+    MatrixXd jacvs_weight_pinv(const MatrixXd& J);
 
     /**
     * \brief KDL Frame to Homogeneous transform
@@ -270,6 +280,7 @@ class CQuadarm_Task_Priority_Ctrl
                                     arm& arm,
                                     const quadrotor& quad,
                                     ctrl_params& ctrl_params,
+                                    const options& options,
                                     cog_data& arm_cog_data,
 									MatrixXd& robot_pos,
                                     MatrixXd& robot_vel);