diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 0b6bec2c079fd6583df0578dffd9edde2a449be1..2e0101d8914b3e627a505ed820f3bde23fb6b9fc 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -16,22 +16,23 @@ 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,
+void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const goal_obj& goal,
+ const MatrixXd& obstacle_dist,
ht& HT,
arm& arm,
const quadrotor& quad,
ctrl_params& ctrl_params,
+ cog_data& arm_cog_data,
MatrixXd& robot_pos,
MatrixXd& robot_vel)
{
-
- // Current quadrotor height
- this->quad_height_ = quad_height;
-
// 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
@@ -57,9 +58,11 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_
HT = this->T_; // DEBUG. TODO: make it constant and do not use this trans. outside.
arm.T_base_to_joint = this->arm_.T_base_to_joint; // DEBUG. TODO: make it constant and do not use this trans. outside.
ctrl_params = this->ctrl_params_;
+ arm_cog_data = this->arm_cog_data_;
robot_pos.block(0,0,6,1) = this->quad_.pos;
robot_pos.block(6,0,this->arm_.nj,1) = this->arm_.jnt_pos;
robot_vel = this->uam_.vel;
+
}
// Kinematics
@@ -314,32 +317,32 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
}
void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo,MatrixXd& sigmaIR)
{
- // Distance to the obstacle (currently detecting only the ground)
- MatrixXd d_obs = MatrixXd::Zero(3,1);
- d_obs(2,0) = this->quad_height_;
-
// Inflation radius
MatrixXd inf_radius = MatrixXd::Zero(1,1);
inf_radius(0,0) = this->ctrl_params_.inf_radius;
// Task vector
- sigmaIR = d_obs.transpose()*d_obs-inf_radius;
+ MatrixXd obs_euc_dist(1,1);
+ obs_euc_dist(0,0) = std::sqrt(std::pow(this->obstacle_dist_(0,0),2)+std::pow(this->obstacle_dist_(1,0),2)+std::pow(this->obstacle_dist_(2,0),2));
+ sigmaIR = inf_radius-obs_euc_dist;
// Task Jacobian
JIR = MatrixXd::Zero(1,4+this->arm_.nj);
- MatrixXd Jtemp = d_obs;
+ MatrixXd Jtemp = this->obstacle_dist_;
JIR.block(0,0,1,3) = 2*Jtemp.transpose();
// Bloquing matrix
MatrixXd H = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
for (unsigned int ii = 0; ii < 3; ++ii)
- if (d_obs(ii,0) < inf_radius(0,0) && d_obs(ii,0) > 0.0)
+ {
+ if (this->obstacle_dist_(ii,0) < inf_radius(0,0) && this->obstacle_dist_(ii,0) > 0.0)
{
- cout << "WARNING!! Inflation Radius violation. DOF: " << ii << " Inf. radius: " << inf_radius(0,0) << " Obstacle distance: " << d_obs(ii,0) << endl;
+ cout << "WARNING!! Inflation Radius violation. DOF: " << ii << " Inf. radius: " << inf_radius(0,0) << " Obstacle distance: " << this->obstacle_dist_(ii,0) << endl;
H(ii,ii) = 1.0;
}
-
+ }
+
MatrixXd Hinv = calcPinv(H);
// weighting matrix
@@ -348,9 +351,11 @@ void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo
// cout << "xxxxxx Task velocities xxxxxx" << endl;
// cout << JIR_pseudo*sigmaIR << endl;
+ // cout << "______ Sigma (IR) _____" << endl;
+ // cout << sigmaIR.transpose() << endl;
// cout << "______ Task Jacobian _____" << endl;
// cout << JIR << endl;
- // cout << "_______ Pseudo Inverse of Task Jacobian ____" << endl;
+ // cout << "_______ Pseudo Inverse of Task Jacobian (IR) ____" << endl;
// cout << JIR_pseudo << endl;
// cout << "+++++++++" << endl;
}
diff --git a/src/quadarm_task_priority_ctrl.h b/src/quadarm_task_priority_ctrl.h
index 5ebb18c5db507fd5e49a9928417299b14fb42a67..78168363d18b852b73f805589550fe2068b1ef25 100644
--- a/src/quadarm_task_priority_ctrl.h
+++ b/src/quadarm_task_priority_ctrl.h
@@ -35,12 +35,6 @@ using namespace Eigen;
using namespace KDL;
using namespace std;
-// Goal related objects
-typedef struct{
- MatrixXd cam_vel; // Desired camera velocity (end-effector)
- double target_dist; // Target distance
-}goal_obj;
-
// Homogeneous Transforms
typedef struct{
Matrix4d tag_in_cam; // Tag in camera frames.
@@ -51,6 +45,22 @@ typedef struct{
Matrix4d armbase_in_baselink; // Camera in base_link frames.
}ht;
+// VS Goal related objects
+typedef struct{
+ MatrixXd cam_vel; // Desired camera velocity (end-effector)
+ double target_dist; // Target distance
+}goal_obj;
+
+// CoG related objects
+typedef struct{
+ vector<MatrixXd> link_cog; // Vector of link CoG relative to base
+ 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.
@@ -108,10 +118,6 @@ typedef struct{
MatrixXd v_rollpitch; // Quadrotor roll and pitch angular velocities.
}ctrl_params;
-typedef struct{
- vector<MatrixXd> link_cog; // Vector of link CoG relative to base
- MatrixXd arm_cog; // Arm CoG coordinates w.r.t. arm base link;
-}cog_data;
class CQuadarm_Task_Priority_Ctrl
{
@@ -128,11 +134,13 @@ class CQuadarm_Task_Priority_Ctrl
ht T_; // Homogeneous Transforms
+ cog_data arm_cog_data_; // Arm CoG data to debug in higher levels.
+
MatrixXd cam_vel_; // Camera velocities
double target_dist_; // Euclidean distance to target.
- double quad_height_; // Quadrotor ground distance.
+ MatrixXd obstacle_dist_; // Distance to a detected obstacle (0 = no obstacle). Provided externally.
/**
* \brief Compute Unmanned Aerial Manipulator Kinematics
@@ -234,10 +242,6 @@ class CQuadarm_Task_Priority_Ctrl
public:
- //TODO: these variables are public for debugging purposes
- // they must be private and set get functions created.
- cog_data arm_cog_data_; // Arm CoG data to debug in higher levels.
-
/**
* \brief Constructor
*
@@ -260,12 +264,13 @@ class CQuadarm_Task_Priority_Ctrl
* Main public function call of Quadarm Task Priority Control.
*
*/
- void quadarm_task_priority_ctrl(const double& quad_height,
- const goal_obj& goal,
+ void quadarm_task_priority_ctrl(const goal_obj& goal,
+ const MatrixXd& obstacle_dist,
ht& HT,
arm& arm,
const quadrotor& quad,
ctrl_params& ctrl_params,
+ cog_data& arm_cog_data,
MatrixXd& robot_pos,
MatrixXd& robot_vel);
/**