diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index b913d1bf78543d61c6e12993c9d643d49c62b510..6a13c7ae37ec96d1f98647a4eb87e57f3e1ba727 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -14,14 +14,19 @@ CQuadarm_Task_Priority_Ctrl::~CQuadarm_Task_Priority_Ctrl()
}
// Main public function
-void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const goal_obj& goal,
+void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_height,
+ const goal_obj& goal,
const ht& HT,
const arm& arm,
ctrl_params& ctrl_params,
MatrixXd& joint_pos,
MatrixXd& robot_vel)
{
- // Goal related objects
+
+ // Current quadrotor height
+ this->quad_height_ = quad_height;
+
+ // Goal related objects
this->vel_.cam = goal.cam_vel;
this->target_dist_ = goal.target_dist;
// Homogeneous transformations
@@ -196,7 +201,7 @@ MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian()
void CQuadarm_Task_Priority_Ctrl::qa_control(){
// Underactuated Quadrotor
- MatrixXd J1(8,4),J2(8,2),V1(4,1),Jqa1(6,4+this->arm_.nj),Jqa2(6,2),Vtotal(4+this->arm_.nj,1);
+ MatrixXd J1(8,4),J2(8,2),V1(4,1),Jqa1(6,4+this->arm_.nj),Jqa2(6,2);
// 9 DOF Extracting wx and wy from quadrotor //////////////////
Jqa1.block(0,0,6,3) = this->Jqa_.block(0,0,6,3);
@@ -209,25 +214,34 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
MatrixXd Jqa1_pseudo(4+this->arm_.nj,6);
Jqa1_pseudo = calcPinv(Jqa1);
- MatrixXd Jqa1_geninverse(4+this->arm_.nj,6);
- weight_inverse(Jqa1,Jqa1_geninverse);
+ MatrixXd Jqa1_wpseudo(4+this->arm_.nj,6);
+ Jqa1_wpseudo = weight_jacvs_inverse(Jqa1);
// Control law ________________________________________________________________
+ // Secutiry task (Inflation radius)
+ MatrixXd JIR,JIR_pseudo,sigmaIR;
+ task_infrad(JIR,JIR_pseudo,sigmaIR);
+
+
// Primary control task (Visual Servo) ________________________
- //this->ctrl_params_.vs_vel = Jqa1_pseudo * (this->vel_.cam-Jqa2*this->ctrl_params_.v_rollpitch);
- this->ctrl_params_.vs_vel = Jqa1_geninverse * (this->vel_.cam-Jqa2*this->ctrl_params_.v_rollpitch);
+ MatrixXd sigmaVS = this->vel_.cam-Jqa2*this->ctrl_params_.v_rollpitch;
+ this->ctrl_params_.vs_vel = Jqa1_wpseudo * sigmaVS;
+
+
+ // Jqa1
+ // Jqa1_wpseudo
+ // sigmaVS
// Secondary control task (CoG alignement) ____________________
// Null space projector
//MatrixXd Nqa1 = (eye-(Jqa1_pseudo*Jqa1));
- MatrixXd Nqa1 = (eye-(Jqa1_geninverse*Jqa1));
+ MatrixXd Nqa1 = (eye-(Jqa1_wpseudo*Jqa1));
// Secondary task Jacobian and sigma
MatrixXd JG,JG_pseudo,sigmaG;
- sec_task_cog(JG,sigmaG);
- JG_pseudo = calcPinv(JG);
+ task_cog(JG,JG_pseudo,sigmaG);
// Gain
double lambdaG = this->ctrl_params_.cog_gain;
@@ -244,15 +258,15 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
MatrixXd Nqa1_G = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
MatrixXd Jqa1_G_pseudo = calcPinv(Jqa1_G);
// Third task after secondary aligning CoG
- //Nqa1_G = (eye-(Jqa1_G_pseudo*Jqa1_G));
+ Nqa1_G = (eye-(Jqa1_G_pseudo*Jqa1_G));
// Third task as secondary
- Nqa1_G = Nqa1;
+ //Nqa1_G = Nqa1;
// Sec task Jacobian and sigma
MatrixXd JL,JL_pseudo,sigmaL;
- sec_task_jl(JL,sigmaL);
- JL_pseudo = calcPinv(JL);
+ task_jl(JL,JL_pseudo,sigmaL);
+
// Gain
double lambdaL = this->ctrl_params_.jntlim_gain;
@@ -272,8 +286,10 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
this->ctrl_params_.jntlim_vel = MatrixXd::Zero(4+this->arm_.nj,1);
cout << "[Task Priority Control]: Secondary tasks not enabled" << endl;
}
- //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;
+
+ 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_.lambda_robot.array()*Vtotal.array();
// Check a singular configuration /////
@@ -287,7 +303,40 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
this->vel_.quadarm.block(0,0,3,1) = Vtotal.block(0,0,3,1);
this->vel_.quadarm.block(5,0,1+this->arm_.nj,1) = Vtotal.block(3,0,1+this->arm_.nj,1);
}
-void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG)
+void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& Jqa1,MatrixXd& Jqa1_wpseudo,MatrixXd& sigmaVS)
+{
+
+}
+void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo,MatrixXd& sigmaIR)
+{
+ // Get diagonal gain matrix
+ MatrixXd Kp=MatrixXd::Zero(3,3);
+ for (unsigned int ii = 0; ii < 3; ++ii)
+ Kp(ii,ii) = 1;
+
+ // Current distance to obstacle (initialize far away)
+ MatrixXd obs_dist = 100*MatrixXd::Ones(3,1);
+ obs_dist(2,0) = this->quad_height_; // ground distance
+
+ // Inflation radius
+ MatrixXd inf_rad = this->ctrl_params_.inf_radius*MatrixXd::Ones(3,1);
+
+ // Current error
+ MatrixXd e_infrad = obs_dist - inf_rad;
+ sigmaIR = e_infrad.transpose()*Kp*e_infrad;
+
+ // Task Jacobian
+ JIR = MatrixXd::Zero(1,4+this->arm_.nj);
+ MatrixXd Jtemp = Kp*e_infrad;
+ JIR.block(0,0,1,3) = -2*Jtemp.transpose();
+
+ JIR_pseudo = calcPinv(JIR);
+
+ 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;
+}
+void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,MatrixXd& sigmaG)
{
// Initializations
MatrixXd qa; // joint positions
@@ -454,9 +503,11 @@ void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG)
JG.block(0,4,1,this->arm_.nj)=2*this->ctrl_params_.cog_PGxy.transpose()*Jacob_temp.block(0,0,2,this->arm_.nj);
+ JG_pseudo = calcPinv(JG);
+
sigmaG = -(this->ctrl_params_.cog_PGxy.transpose()*this->ctrl_params_.cog_PGxy);
}
-void CQuadarm_Task_Priority_Ctrl::sec_task_jl(MatrixXd& JL,MatrixXd& sigmaL)
+void CQuadarm_Task_Priority_Ctrl::task_jl(MatrixXd& JL,MatrixXd& JL_pseudo,MatrixXd& sigmaL)
{
// Get diagonal gain matrix
MatrixXd AAL=MatrixXd::Zero(this->arm_.nj,this->arm_.nj);
@@ -481,79 +532,104 @@ void CQuadarm_Task_Priority_Ctrl::sec_task_jl(MatrixXd& JL,MatrixXd& sigmaL)
JL.block(0,0,1,4) = MatrixXd::Zero(1,4);
MatrixXd Jtemp = AAL*this->ctrl_params_.jntlim_pos_error;
JL.block(0,4,1,this->arm_.nj) = -2*Jtemp.transpose();
+
+ JL_pseudo = calcPinv(JL);
}
// MISC functions
-void CQuadarm_Task_Priority_Ctrl::weight_inverse(const MatrixXd& J, MatrixXd& J_inverse)
+MatrixXd CQuadarm_Task_Priority_Ctrl::weight_jacvs_inverse(const MatrixXd& J)
{
-
+ // W: Weighting matrix (motion distribution) ______________________
double w_min = this->ctrl_params_.vs_delta_gain(0,0);
double w_max = this->ctrl_params_.vs_delta_gain(1,0);
double alpha_min = this->ctrl_params_.vs_alpha_min;
double tmp = 2*3.14159*((this->target_dist_-w_min)/(w_max-w_min))-3.14159;
-
double alpha = (0.5*(1+alpha_min)) + (0.5*(1-alpha_min))*tanh(tmp);
- // double alpha = (0.5*alpha_min) + (0.5*alpha_min*tanh(tmp));
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)
{
- // Quadrotor values
+ // W Quadrotor values
if(ii<4)
W(ii,ii) = 1-alpha;
- // Arm values
+ // 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 temp = J*W.inverse()*J.transpose();
+ MatrixXd Winv = calcPinv(W);
+
+ // Only 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);
- J_inverse = W.inverse()*J.transpose()*temp.inverse();
+ return J_inverse;
}
MatrixXd CQuadarm_Task_Priority_Ctrl::calcPinv(const MatrixXd &a){
- // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
- const MatrixXd* m;
- MatrixXd t;
- MatrixXd m_pinv;
-
- // transpose so SVD decomp can work...
- if ( a.rows()<a.cols() ){
- t = a.transpose();
- m = &t;
- } else {
- m = &a;
- }
-
- // SVD
- //JacobiSVD<MatrixXd> svd = m->jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
- JacobiSVD<MatrixXd> svd = m->jacobiSvd(ComputeThinU | ComputeThinV);
- MatrixXd vSingular = svd.singularValues();
- // Build a diagonal matrix with the Inverted Singular values
- // The pseudo inverted singular matrix is easy to compute :
- // is formed by replacing every nonzero entry by its reciprocal (inversing).
- MatrixXd vPseudoInvertedSingular(svd.matrixV().cols(),1);
- for (int iRow =0; iRow<vSingular.rows(); iRow++) {
- // Todo : Put epsilon in parameter
- if ( fabs(vSingular(iRow))<=1e-10 ) {
- vPseudoInvertedSingular(iRow,0)=0.;
- }
- else {
- vPseudoInvertedSingular(iRow,0)=1./vSingular(iRow);
- }
- }
- // A little optimization here
- MatrixXd mAdjointU = svd.matrixU().adjoint().block(0,0,vSingular.rows(),svd.matrixU().adjoint().cols());
- // Pseudo-Inversion : V * S * U'
- m_pinv = (svd.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU ;
-
- // transpose back if necessary
- if ( a.rows()<a.cols() )
- return m_pinv.transpose();
-
- return m_pinv;
+ // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
+ const MatrixXd* m;
+ MatrixXd t;
+ MatrixXd m_pinv;
+
+ // transpose so SVD decomp can work...
+ if ( a.rows()<a.cols() ){
+ t = a.transpose();
+ m = &t;
+ } else {
+ m = &a;
+ }
+
+ // SVD
+ //JacobiSVD<MatrixXd> svd = m->jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
+ JacobiSVD<MatrixXd> svd = m->jacobiSvd(ComputeThinU | ComputeThinV);
+ MatrixXd vSingular = svd.singularValues();
+ // Build a diagonal matrix with the Inverted Singular values
+ // The pseudo inverted singular matrix is easy to compute :
+ // is formed by replacing every nonzero entry by its reciprocal (inversing).
+ MatrixXd vPseudoInvertedSingular(svd.matrixV().cols(),1);
+ for (int iRow =0; iRow<vSingular.rows(); iRow++) {
+ // Todo : Put epsilon in parameter
+ if ( fabs(vSingular(iRow))<=1e-10 ) {
+ vPseudoInvertedSingular(iRow,0)=0.;
+ }
+ else {
+ vPseudoInvertedSingular(iRow,0)=1./vSingular(iRow);
+ }
+ }
+ // A little optimization here
+ MatrixXd mAdjointU = svd.matrixU().adjoint().block(0,0,vSingular.rows(),svd.matrixU().adjoint().cols());
+ // Pseudo-Inversion : V * S * U'
+ m_pinv = (svd.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU ;
+
+ // transpose back if necessary
+ if ( a.rows()<a.cols() )
+ return m_pinv.transpose();
+
+ return m_pinv;
}
Matrix4d CQuadarm_Task_Priority_Ctrl::GetTransform(const Frame &f)
{
diff --git a/src/quadarm_task_priority_ctrl.h b/src/quadarm_task_priority_ctrl.h
index 124809e6b96e465d85bc916bdc69b60752654d2f..4d88ba4c7797904c76224545b350f91e52d77ddb 100644
--- a/src/quadarm_task_priority_ctrl.h
+++ b/src/quadarm_task_priority_ctrl.h
@@ -37,7 +37,7 @@ using namespace std;
// Goal related objects
typedef struct{
- MatrixXd cam_vel; // Desired camera velocity (end-effector)
+ MatrixXd cam_vel; // Desired camera velocity (end-effector)
double target_dist; // Target distance
}goal_obj;
@@ -86,7 +86,8 @@ typedef struct{
MatrixXd lambda_robot; // Robot proportional gains.
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.
+ MatrixXd vs_delta_gain; // Delta values (min and max) for gain matrix pseudo inverse W.
+ double inf_radius; // Security distance to an obstacle (Inflation radius) to prevent Collisions.
double cog_gain; // Gain of CoG alignment secondary task.
MatrixXd cog_vel; // Secondary task velocity.
MatrixXd cog_PGxy; // X and Y of the CoG r.t. Quadrotor body inertial frame.
@@ -126,7 +127,9 @@ class CQuadarm_Task_Priority_Ctrl
arm arm_; // Arm
- ctrl_params ctrl_params_; // Control Law parameters
+ ctrl_params ctrl_params_; // Control Law parameters.
+
+ double quad_height_; // Quadrotor ground distance.
/**
* \brief Compute Quadrotor and Arm Kinematics
@@ -169,30 +172,40 @@ class CQuadarm_Task_Priority_Ctrl
void qa_control();
/**
- * \brief Get weighted generalized Jacobian inverse
+ * \brief Task: Visual Servoing
*
- * Compute the Jacobian inverse weighted depending on target distance
+ * Compute vector and jacobians of the task corresponding to the visual servoing.
+ *
+ */
+ void task_vs(MatrixXd& Jqa1,MatrixXd& Jqa1_wpseudo,MatrixXd& sigmaVS);
+
+ /**
+ * \brief Task: Security inflation radius
+ *
+ * Compute vector and jacobians of the task to ensure no obstacles will
+ *
+ * fall inside the inflation radius (reactive behaviour).
*
*/
- void weight_inverse(const MatrixXd& J, MatrixXd& J_inverse);
+ void task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo,MatrixXd& sigmaIR);
/**
- * \brief Secondary task gravity alignment
+ * \brief Task: CoG alignment
*
- * Compute vector of the secondary task to vertically align the arm center of gravity
+ * Compute vector and jacobians of the task to vertically align the arm center of gravity
*
* with the quadrotor gravitational vector
*
*/
- void sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG);
+ void task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,MatrixXd& sigmaG);
/**
- * \brief Secondary task Arm joint limits avoidance
+ * \brief Task: Arm joint limits avoidance
*
- * Compute vector of the secondary task to avoid arm joint limits
+ * Compute vector and jacobians of the task to avoid arm joint limits
*
*/
- void sec_task_jl(MatrixXd& JG,MatrixXd& sigmaG);
+ void task_jl(MatrixXd& JL,MatrixXd& JL_pseudo,MatrixXd& sigmaL);
/**
* \brief Matrix pseudo-inverse
@@ -201,6 +214,14 @@ class CQuadarm_Task_Priority_Ctrl
*/
MatrixXd calcPinv(const MatrixXd &a);
+ /**
+ * \brief Get weighted generalized Jacobian inverse
+ *
+ * Compute the Jacobian inverse weighted depending on target distance
+ *
+ */
+ MatrixXd weight_jacvs_inverse(const MatrixXd& J);
+
/**
* \brief KDL Frame to Homogeneous transform
*
@@ -234,11 +255,12 @@ class CQuadarm_Task_Priority_Ctrl
* Main public function call of Quadarm Task Priority Control.
*
*/
- void quadarm_task_priority_ctrl(const goal_obj& goal,
+ void quadarm_task_priority_ctrl(const double& quad_height,
+ const goal_obj& goal,
const ht& HT,
const arm& arm,
ctrl_params& ctrl_params,
- MatrixXd& joint_pos,
+ MatrixXd& joint_pos,
MatrixXd& robot_vel);
/**
* \brief Write to file