diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 2e0101d8914b3e627a505ed820f3bde23fb6b9fc..bbe3105d685a27fe80192faba1320ae8eeabf627 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/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);
diff --git a/src/quadarm_task_priority_ctrl.h b/src/quadarm_task_priority_ctrl.h
index 78168363d18b852b73f805589550fe2068b1ef25..313f80a19499d054302a2d2c7100a32fdcf82a2e 100644
--- a/src/quadarm_task_priority_ctrl.h
+++ b/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);