Skip to content
GitLab
Commit 0164598a authored by Angel Santamaria-Navarro's avatar Angel Santamaria-Navarro
Browse files

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

parent b61ef4c7
Hide whitespace changes
Inline Side-by-side
src/quadarm_task_priority_ctrl.cpp
View file @ 0164598a
......@@ -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
View file @ 0164598a
......@@ -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);
......
Supports Markdown
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