minor change

src/quadarm_task_priority_ctrl.cpp

@@ -311,19 +311,23 @@ void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG)
   vector<Segment> arm_segment(js);
   arm_segment.reserve(js);
   for (unsigned int ii = 1; ii < js; ++ii)
-  {
     arm_segment.at(ii) = this->arm_.chain.getSegment(ii);
-  }
 
   // Get joint positions
   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(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,0) = 1;
+  Tbase(2,2) = 1;
+  Tbase(3,3) = 1;
+
 
   // Origin of the base frame
   MatrixXd pini(4,1);
@@ -336,13 +340,16 @@ void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG)
   // Origin of the initial frame
   frame_link.at(0) = T_base_to_joint.at(0) * pini;
 
+  // Debug
+  //std::cout << "**************" << std::endl;
+
   for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
   {
     // Get mass of each link
     mass(jj,0) = arm_segment.at(jj+1).getInertia().getMass();
     // Joint frames  
     joint_pose.at(jj) = arm_segment.at(jj+1).pose(qa(jj,0));
-    // relative Homogenous transforms (HT)
+    // relative Homogeneous transforms (HT)
     Transf.at(jj) = GetTransform(joint_pose.at(jj));
     // HT r.t. base link
     T_base_to_joint.at(jj+1) = T_base_to_joint.at(jj) * Transf.at(jj);
@@ -358,11 +365,23 @@ void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG)
     arm_cg = arm_cg + link_cg.at(jj);
     // Sum of link mass  
     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;
   }
+  // Debug
+  //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;
+
   // Rotation between quadrotor body and inertial frames  
   Matrix3d Rib;
   // Rib = AngleAxisd(0, Vector3d::UnitZ()) * AngleAxisd(this->ctrl_params_.q_rollpitch(1,0), Vector3d::UnitY()) * AngleAxisd(this->ctrl_params_.q_rollpitch(0,0), Vector3d::UnitX());
@@ -374,10 +393,10 @@ void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG)
   // This change is done considering arm base frame orientation
   // TODO: Work with quadrotor orientation
   //std::cout << "******" << std::endl << cog_arm_in_qi << std::endl;
-  //this->ctrl_params_.cog_PGxy = cog_arm_in_qi.block(0,0,2,1);
-  this->ctrl_params_.cog_PGxy = MatrixXd::Zero(2,1);
-  this->ctrl_params_.cog_PGxy(0,0) = cog_arm_in_qi(2,0);
-  this->ctrl_params_.cog_PGxy(1,0) = 	cog_arm_in_qi(1,0);
+  this->ctrl_params_.cog_PGxy = cog_arm_in_qi.block(0,0,2,1);
+  //this->ctrl_params_.cog_PGxy = MatrixXd::Zero(2,1);
+  //this->ctrl_params_.cog_PGxy(0,0) = cog_arm_in_qi(2,0);
+  //this->ctrl_params_.cog_PGxy(1,0) = 	cog_arm_in_qi(1,0);
 
   MatrixXd X_partial;