Working with Inflation radius and Visual servo

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,
                                                              const goal_obj& goal,
-                                                             const ht& HT,
+                                                             ht& HT,
                                                              const arm& arm,
                                                              const quadrotor& quad,
                                                              ctrl_params& ctrl_params,
@@ -43,14 +43,16 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_
   this->quad_.pos = quad.pos;
 
   uam_kinematics();
-  qa_control();
+  uam_control();
 
   // Arm positions increment
   this->arm_.jnt_pos = this->arm_.jnt_pos+(this->uam_.vel.block(6,0,this->arm_.nj,1) * this->ctrl_params_.dt);
+
   // Quadrotor positions increment
   this->quad_.pos = this->quad_.pos+(this->uam_.vel.block(0,0,6,1) * this->ctrl_params_.dt);
 
   // return values
+  HT = this->T_;
   ctrl_params = this->ctrl_params_;
   robot_pos.block(0,0,6,1) = this->quad_.pos;
   robot_pos.block(6,0,this->arm_.nj,1) = this->arm_.jnt_pos;
@@ -200,10 +202,8 @@ MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian()
 }
 
 // Control
-void CQuadarm_Task_Priority_Ctrl::qa_control(){
+void CQuadarm_Task_Priority_Ctrl::uam_control(){
   
-  // Control law ________________________________________________________________
-
   // TASK 0: Security task (Inflation radius) _____________________
 
   // task Jacobian and sigma
@@ -248,15 +248,9 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
   // this->ctrl_params_.cog_vel = Nqa1 * JG_pseudo * lambdaG * sigmaG;
   this->ctrl_params_.cog_vel = NIR_VS * JG_pseudo * lambdaG * sigmaG;
 
-  // Third control task (Joint limits) ________________________
+  // TASK 3: Joint limits ________________________
   
   // Augmented null space projector from Augmented Jacobian
-  // MatrixXd JVS_G = MatrixXd::Zero(JVS.rows()+JG.rows(),JVS.cols());
-  // JVS_G.block(0,0,JVS.rows(),JVS.cols()) = JVS;
-  // JVS_G.block(JVS.rows()-1,0,JG.rows(),JG.cols())=JG; 
-  // MatrixXd Nqa1_G = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
-  // MatrixXd JVS_G_pseudo = calcPinv(JVS_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; 
@@ -265,8 +259,6 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
 
 
   // Third task after secondary aligning CoG
-  //Nqa1_G = (eye-(JVS_G_pseudo*JVS_G));
-  //Nqa1_G = Nqa1; // Third task as secondary
   NIR_VS_G = (eye-(JIR_VS_G_pseudo*JIR_VS_G));
 
   // task Jacobian and sigma
@@ -277,9 +269,11 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
   double lambdaL = this->ctrl_params_.jntlim_gain;
 
   // task velocity
-  //this->ctrl_params_.jntlim_vel = Nqa1_G * JL_pseudo * lambdaL * sigmaL;
+  //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;
 
+  // Total velocities _______________________________
+
   //******************************************************************
   // Weighted sum of subtasks
   // this->ctrl_params_.cog_vel = Nqa1 * (JG_pseudo * lambdaG * sigmaG + JL_pseudo * lambdaL * sigmaL);
@@ -294,9 +288,13 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
   }
 
   MatrixXd Vtotal(4+this->arm_.nj,1);
-  //Vtotal = this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel + this->ctrl_params_.jntlim_vel; 
-  //Vtotal = this->ctrl_params_.vs_vel + this->ctrl_params_.jntlim_vel; 
-  Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel + this->ctrl_params_.jntlim_vel; 
+
+  // Task 0 + 1
+  Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_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();
 
@@ -368,7 +366,7 @@ 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 = calcPinv(Jqa1);
   JVS_pseudo = weight_jacvs_inverse(Jqa1);
 
   // task velocity vector

src/quadarm_task_priority_ctrl.h

@@ -128,9 +128,9 @@ class CQuadarm_Task_Priority_Ctrl
 
     quadrotor quad_; // Quadrotor
 
-    ctrl_params ctrl_params_; // Control Law parameters.
+    ht T_; // Homogeneous Transforms
 
-  	ht T_; // Homogeneous Transforms
+    ctrl_params ctrl_params_; // Control Law parameters.
 
     MatrixXd cam_vel_; // Camera velocities
 
@@ -176,7 +176,7 @@ class CQuadarm_Task_Priority_Ctrl
     * Compute the motions of the quadrotor and arm joints
     *
     */ 
-    void qa_control();
+    void uam_control();
 
     /**
     * \brief Task: Visual Servoing
@@ -264,7 +264,7 @@ class CQuadarm_Task_Priority_Ctrl
     */     
     void quadarm_task_priority_ctrl(const double& quad_height,
                                     const goal_obj& goal,
-                                    const ht& HT,
+                                    ht& HT,
                                     const arm& arm,
                                     const quadrotor& quad,
                                     ctrl_params& ctrl_params,