Working with unina arm and IRI arm v2. VS as first task and Joint limits as...

Working with unina arm and IRI arm v2. VS as first task and Joint limits as second (CoG needs to be fixed and joint limits switched to third task)

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;

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
     *