diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 909108102b582b57b74bd165a740eb96584b704d..b913d1bf78543d61c6e12993c9d643d49c62b510 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -33,7 +33,6 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const goal_obj& goa
this->arm_.joint_info= arm.joint_info;
this->arm_.jnt_pos = arm.jnt_pos;
-
// Arm DOFs
this->arm_.nj = this->arm_.chain.getNrOfJoints();
@@ -155,6 +154,7 @@ MatrixXd CQuadarm_Task_Priority_Ctrl::arm_jacobian()
Ja(ii,jj) = this->arm_.jacobian.data(ii,jj);
}
}
+
return Ja;
}
MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian()
@@ -221,8 +221,8 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
// Secondary control task (CoG alignement) ____________________
// Null space projector
- MatrixXd Nqa1 = (eye-(Jqa1_pseudo*Jqa1));
- //MatrixXd Nqa1 = (eye-(Jqa1_geninverse*Jqa1));
+ //MatrixXd Nqa1 = (eye-(Jqa1_pseudo*Jqa1));
+ MatrixXd Nqa1 = (eye-(Jqa1_geninverse*Jqa1));
// Secondary task Jacobian and sigma
MatrixXd JG,JG_pseudo,sigmaG;
@@ -243,7 +243,11 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
Jqa1_G.block(Jqa1.rows()-1,0,JG.rows(),JG.cols())=JG;
MatrixXd Nqa1_G = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
MatrixXd Jqa1_G_pseudo = calcPinv(Jqa1_G);
- Nqa1_G = (eye-(Jqa1_G_pseudo*Jqa1_G));
+ // Third task after secondary aligning CoG
+ //Nqa1_G = (eye-(Jqa1_G_pseudo*Jqa1_G));
+
+ // Third task as secondary
+ Nqa1_G = Nqa1;
// Sec task Jacobian and sigma
MatrixXd JL,JL_pseudo,sigmaL;
@@ -268,7 +272,8 @@ 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_.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 /////
@@ -317,17 +322,12 @@ void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG)
qa = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1);
//Fixed transform from arm_base to base_link
- Matrix4d Tbase = Matrix4d::Zero();
- //Tbase(3,3) = 1;
- //Tbase(0,2) = -1;
- //Tbase(1,1) = 1;
- //Tbase(2,0) = 1;
-
- Tbase(0,0) = 1;
- Tbase(1,1) = 1;
- Tbase(2,2) = 1;
- Tbase(3,3) = 1;
-
+ // Matrix4d Tbase = Matrix4d::Zero();
+ // Tbase(0,0) = -1;
+ // Tbase(1,1) = 1;
+ // Tbase(2,2) = -1;
+ // Tbase(3,3) = 1;
+ Matrix4d Tbase = Matrix4d::Identity();
// Origin of the base frame
MatrixXd pini(4,1);
@@ -342,6 +342,7 @@ void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG)
// Debug
//std::cout << "**************" << std::endl;
+ this->arm_cog_data_.link.resize(this->arm_.nj);
for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
{
@@ -367,20 +368,34 @@ void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG)
this->arm_.mass = this->arm_.mass + mass(jj,0);
//Debug
- //std::cout << "num segments: " << arm_segment.size() << std::endl;
- //std::cout << "Link: " << jj << " name:" << arm_segment.at(jj+1).getName() << std::endl;
- //std::cout << "Transform: " << std::endl;
- //std::cout << T_base_to_joint.at(jj+1) <<std::endl;
+ // std::cout << "num segments: " << arm_segment.size() << std::endl;
+ // std::cout << "Link: " << jj << " name:";
+ // std::cout << arm_segment.at(jj+1).getName();
+ // std::cout << " mass: " << mass(jj,0);
+ // std::cout << " link_cog:" << std::endl;
+ // std::cout << frame_link.at(jj)+dist.at(jj) << std::endl;
+ // std::cout << "Transform: " << std::endl;
+ // std::cout << T_base_to_joint.at(jj+1) <<std::endl;
+
+ // Fill link information
+ this->arm_cog_data_.link.at(jj).origin = T_base_to_joint.at(jj+1);
+ this->arm_cog_data_.link.at(jj).cog = (frame_link.at(jj+1)-frame_link.at(jj))/2;
+ this->arm_cog_data_.link.at(jj).mass = mass(jj,0);
}
// Debug
- //std::cout << "mass: " << this->arm_.mass << std::endl;
- //std::cout << "arm_cg: " << arm_cg << std::endl;
+ // std::cout << "mass: " << this->arm_.mass << std::endl;
+ // std::cout << "arm_cg: " << arm_cg << std::endl;
// vector from arm base to CoG
this->ctrl_params_.cog_arm = arm_cg/this->arm_.mass;
// Debug
- //std::cout << "cog: " << this->ctrl_params_.cog_arm << std::endl;
+ // std::cout << "cog: " << this->ctrl_params_.cog_arm << std::endl;
+
+ // Fill arm CoG information
+ this->arm_cog_data_.arm_mass = this->arm_.mass;
+ this->arm_cog_data_.arm_cog = this->ctrl_params_.cog_arm;
+
// Rotation between quadrotor body and inertial frames
Matrix3d Rib;
diff --git a/src/quadarm_task_priority_ctrl.h b/src/quadarm_task_priority_ctrl.h
index a9d04217ec93817aca7579d5574e8d7a7dc178d8..124809e6b96e465d85bc916bdc69b60752654d2f 100644
--- a/src/quadarm_task_priority_ctrl.h
+++ b/src/quadarm_task_priority_ctrl.h
@@ -99,6 +99,17 @@ typedef struct{
MatrixXd v_rollpitch; // Quadrotor roll and pitch angular velocities.
}ctrl_params;
+typedef struct{
+ MatrixXd origin; // Origin coordinates
+ MatrixXd cog; // CoG coordinates (3D vector) w.r.t. link origin
+ double mass; // Mass
+}link_obj;
+
+typedef struct{
+ vector<link_obj> link; // Vector of link CoG relative to base
+ double arm_mass; // Arm Mass;
+ MatrixXd arm_cog; // Arm CoG coordinates w.r.t. arm base link;
+}cog_data;
class CQuadarm_Task_Priority_Ctrl
{
@@ -199,6 +210,8 @@ class CQuadarm_Task_Priority_Ctrl
public:
+ cog_data arm_cog_data_; // Arm CoG data to debug in higher levels.
+
/**
* \brief Constructor
*