diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 8d5322c48074b90575162a2fd6a0a3e238277a2d..afb2df387acd78720a20b1f0e9120da1112d8eaf 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -16,7 +16,7 @@ CQuadarm_Task_Priority_Ctrl::~CQuadarm_Task_Priority_Ctrl()
// Main public function
void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_height,
const goal_obj& goal,
- const ht& HT,
+ ht& HT,
const arm& arm,
const quadrotor& quad,
ctrl_params& ctrl_params,
@@ -43,14 +43,16 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_
this->quad_.pos = quad.pos;
uam_kinematics();
- qa_control();
+ uam_control();
// Arm positions increment
this->arm_.jnt_pos = this->arm_.jnt_pos+(this->uam_.vel.block(6,0,this->arm_.nj,1) * this->ctrl_params_.dt);
+
// Quadrotor positions increment
this->quad_.pos = this->quad_.pos+(this->uam_.vel.block(0,0,6,1) * this->ctrl_params_.dt);
// return values
+ HT = this->T_;
ctrl_params = this->ctrl_params_;
robot_pos.block(0,0,6,1) = this->quad_.pos;
robot_pos.block(6,0,this->arm_.nj,1) = this->arm_.jnt_pos;
@@ -200,10 +202,8 @@ MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian()
}
// Control
-void CQuadarm_Task_Priority_Ctrl::qa_control(){
+void CQuadarm_Task_Priority_Ctrl::uam_control(){
- // Control law ________________________________________________________________
-
// TASK 0: Security task (Inflation radius) _____________________
// task Jacobian and sigma
@@ -248,15 +248,9 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
// 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) ________________________
+ // TASK 3: 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 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;
@@ -265,8 +259,6 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
// Third task after secondary aligning CoG
- //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
@@ -277,9 +269,11 @@ 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 = NIR_VS * JL_pseudo * lambdaL * sigmaL; // As secondary
this->ctrl_params_.jntlim_vel = NIR_VS_G * JL_pseudo * lambdaL * sigmaL;
+ // Total velocities _______________________________
+
//******************************************************************
// Weighted sum of subtasks
// this->ctrl_params_.cog_vel = Nqa1 * (JG_pseudo * lambdaG * sigmaG + JL_pseudo * lambdaL * sigmaL);
@@ -294,9 +288,13 @@ 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_.jntlim_vel;
- Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel + this->ctrl_params_.jntlim_vel;
+
+ // Task 0 + 1
+ Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_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();
@@ -368,7 +366,7 @@ 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 = calcPinv(Jqa1);
JVS_pseudo = weight_jacvs_inverse(Jqa1);
// task velocity vector
diff --git a/src/quadarm_task_priority_ctrl.h b/src/quadarm_task_priority_ctrl.h
index 3749fd8bd443c486405c69387cd05d4c64596638..1417d1e9f355cb7a28294d658085d7b1dcd9f8c6 100644
--- a/src/quadarm_task_priority_ctrl.h
+++ b/src/quadarm_task_priority_ctrl.h
@@ -128,9 +128,9 @@ class CQuadarm_Task_Priority_Ctrl
quadrotor quad_; // Quadrotor
- ctrl_params ctrl_params_; // Control Law parameters.
+ ht T_; // Homogeneous Transforms
- ht T_; // Homogeneous Transforms
+ ctrl_params ctrl_params_; // Control Law parameters.
MatrixXd cam_vel_; // Camera velocities
@@ -176,7 +176,7 @@ class CQuadarm_Task_Priority_Ctrl
* Compute the motions of the quadrotor and arm joints
*
*/
- void qa_control();
+ void uam_control();
/**
* \brief Task: Visual Servoing
@@ -264,7 +264,7 @@ class CQuadarm_Task_Priority_Ctrl
*/
void quadarm_task_priority_ctrl(const double& quad_height,
const goal_obj& goal,
- const ht& HT,
+ ht& HT,
const arm& arm,
const quadrotor& quad,
ctrl_params& ctrl_params,