diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 5e3fcff43429b50698254410b0427fcfe7aa079d..b029889dc698343c4b4af6c31cadf82fb8d3e334 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -202,28 +202,39 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
// Control law ________________________________________________________________
- // TASK: Security task (Inflation radius) _____________________
+ // TASK 0: Security task (Inflation radius) _____________________
// task Jacobian and sigma
MatrixXd JIR,JIR_pseudo,sigmaIR;
task_infrad(JIR,JIR_pseudo,sigmaIR);
+ // task velocity
+ this->ctrl_params_.ir_vel = JIR_pseudo * sigmaIR;
- // TASK: Visual Servo _________________________________________
+ // TASK 1: Visual Servo _________________________________________
+ // Null space projector
+ MatrixXd eye = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);
+ MatrixXd Nqa1 = (eye-(JIR_pseudo*JIR));
+
// task Jacobian and sigma
MatrixXd JVS,JVS_wpseudo,sigmaVS;
task_vs(JVS,JVS_wpseudo,sigmaVS);
// task velocity
- this->ctrl_params_.vs_vel = JVS_wpseudo * sigmaVS;
+ this->ctrl_params_.vs_vel = Nqa1 * JVS_wpseudo * sigmaVS;
- // TASK: CoG alignement ______________________________________
+ // TASK 2: CoG alignement ______________________________________
// Null space projector
- MatrixXd eye = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);
- MatrixXd Nqa1 = (eye-(JVS_wpseudo*JVS));
-
+ //MatrixXd Nqa1 = (eye-(JVS_wpseudo*JVS));
+ 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;
+ MatrixXd NIR_VS = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
+ 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);
@@ -232,20 +243,29 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
double lambdaG = this->ctrl_params_.cog_gain;
// task velocity
- this->ctrl_params_.cog_vel = Nqa1 * JG_pseudo * lambdaG * sigmaG;
+ // 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) ________________________
// 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 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;
+ MatrixXd NIR_VS_G = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
+ MatrixXd JIR_VS_G_pseudo = calcPinv(JIR_VS_G);
+
+
// Third task after secondary aligning CoG
- Nqa1_G = (eye-(JVS_G_pseudo*JVS_G));
+ //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
MatrixXd JL,JL_pseudo,sigmaL;
@@ -255,7 +275,8 @@ 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 = Nqa1_G * JL_pseudo * lambdaL * sigmaL;
+ this->ctrl_params_.jntlim_vel = NIR_VS_G * JL_pseudo * lambdaL * sigmaL;
//******************************************************************
// Weighted sum of subtasks
@@ -271,8 +292,10 @@ 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_.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;
+
Vtotal = this->ctrl_params_.lambda_robot.array()*Vtotal.array();
// Check a singular configuration /////
@@ -330,11 +353,22 @@ void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo
MatrixXd Jtemp = Kp*e_infrad;
JIR.block(0,0,1,3) = -2*Jtemp.transpose();
- JIR_pseudo = calcPinv(JIR);
+
+ // Bloquing matrix
+ MatrixXd H = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
for (unsigned int ii = 0; ii < 3; ++ii)
if (e_infrad(ii,0) < 0)
- cout << "WARNING!! DOF: " << ii << " Obstacle distance: " << obs_dist << " Inf. radius: " << inf_rad << endl;
+ {
+ cout << "WARNING!! Inflation Radius violation. DOF: " << ii << " Inf. radius: " << inf_rad(ii,0) << " Obstacle distance: " << obs_dist(ii,0) << endl;
+ H(ii,ii) = 1.0;
+ }
+
+ MatrixXd Hinv = calcPinv(H);
+
+ // weighting matrix
+ MatrixXd temp = JIR*Hinv*JIR.transpose();
+ JIR_pseudo = Hinv*JIR.transpose()*calcPinv(temp);
}
void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,MatrixXd& sigmaG)
{
@@ -549,9 +583,6 @@ MatrixXd CQuadarm_Task_Priority_Ctrl::weight_jacvs_inverse(const MatrixXd& J)
MatrixXd W = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
- // H: Blocking matrix (security measure) __________________________
- MatrixXd H = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);
-
// Create matrices
for (unsigned int ii = 0; ii < 4+this->arm_.nj; ++ii)
{
@@ -561,31 +592,14 @@ MatrixXd CQuadarm_Task_Priority_Ctrl::weight_jacvs_inverse(const MatrixXd& J)
// W Arm values
else
W(ii,ii) = alpha;
-
- // H distance to ground
- // if(ii == 2 && this->quad_height_ < this->ctrl_params_.inf_radius)
- // {
- // H(ii,ii) = 0;
- // cout << "WARNING: ground distance: " << this->quad_height_ << " Security measure active. Limit height: " << this->ctrl_params_.inf_radius << endl;
- // }
}
MatrixXd Winv = calcPinv(W);
- // Only weighting matrix
+ // weighting matrix
MatrixXd temp = J*Winv*J.transpose();
MatrixXd J_inverse = Winv*J.transpose()*calcPinv(temp);
- // cout << "+++++++++++" << endl;
- // cout << W << endl;
- // cout << calcPinv(W) << endl;
- // cout << H*calcPinv(W)*H.transpose() << endl;
-
-
- // // Weighting and security measure
- // MatrixXd temp = J*H*Winv*H.transpose()*J.transpose();
- // J_inverse = H*Winv*H.transpose()*J.transpose()*calcPinv(temp);
-
return J_inverse;
}
MatrixXd CQuadarm_Task_Priority_Ctrl::calcPinv(const MatrixXd &a){
diff --git a/src/quadarm_task_priority_ctrl.h b/src/quadarm_task_priority_ctrl.h
index 0de288fe2bc651db522d5acb5575b3154d43ea4c..ecad8cfd4663519977aa29ed6295988f0e68145d 100644
--- a/src/quadarm_task_priority_ctrl.h
+++ b/src/quadarm_task_priority_ctrl.h
@@ -84,6 +84,7 @@ typedef struct{
double dt; // time interval.
double stabil_gain; // Gain of kinematics stabilization secondary task.
MatrixXd lambda_robot; // Robot proportional gains.
+ MatrixXd ir_vel; // Inflation radius velocity.
MatrixXd vs_vel; // Primary task velocity.
double vs_alpha_min; // Alpha value for gain matrix pseudo inverse.
MatrixXd vs_delta_gain; // Delta values (min and max) for gain matrix pseudo inverse W.