Skip to content
Snippets Groups Projects
Commit 12570ced authored by asantamaria's avatar asantamaria
Browse files

added inflation radius first task

parent dda9c08d
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/quadarm_task_priority_ctrl.cpp
+ 53
− 39
View file @ 12570ced
......@@ -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){
......
src/quadarm_task_priority_ctrl.h
+ 1
− 0
View file @ 12570ced
......@@ -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.
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment