bug of transforms found. debugged until line 442

src/quadarm_task_priority_ctrl.cpp

@@ -408,20 +408,24 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
   pini.col(0) << 0,0,0,1;
 
   // Initial transform
-  // T_base_to_joint.at(0) = this->T_.armbase_in_baselink;
-  T_base_to_joint.at(0) = Matrix4d::Identity();
+  T_base_to_joint.at(0) = this->T_.armbase_in_baselink;
+  // T_base_to_joint.at(0) = Matrix4d::Identity();
 
   // 3rd column of rotation matrix
   IIIrdcolRot_b_x.at(0) = T_base_to_joint.at(0).block(0,2,3,1);
   // Origin of the initial frame
   frame_link.at(0) = T_base_to_joint.at(0) * pini;
 
-  // Debug
+  // Debug 
   this->arm_cog_data_.link.resize(this->arm_.nj);
   this->arm_.T_base_to_joint.resize(this->arm_.nj+1);
   this->arm_.T_base_to_joint.at(0) = T_base_to_joint.at(0);
   
-  std::cout << "**************" << std::endl;
+  // std::cout << "**************" << std::endl;
+  // std::cout << "______base_link to arm_base: " << std::endl;
+  // std::cout << T_base_to_joint.at(0).block(0,3,3,1) << std::endl;
+  // Matrix3d Rot = T_base_to_joint.at(0).block(0,0,3,3);
+  // std::cout << Rot.eulerAngles(0, 1, 2) << std::endl;
 
   for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
   {
@@ -432,10 +436,11 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
     // relative Homogeneous transforms (HT)
     Transf.at(jj) = GetTransform(joint_pose.at(jj));
 
-//TODO: Debugging till here
-
     // HT r.t. base link
     T_base_to_joint.at(jj+1) = T_base_to_joint.at(jj) * Transf.at(jj);
+
+//TODO: Debugging till here
+
     // 3rd column of rotation matrix
     IIIrdcolRot_b_x.at(jj+1) = T_base_to_joint.at(jj+1).block(0,2,3,1);
     // Origin of the link's frame r.t. base_link
@@ -458,27 +463,31 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
     // 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;
-    std::cout << "______joint: " << jj << std::endl;
-    std::cout << Transf.at(jj).block(0,3,3,1) << std::endl;
-    Matrix3d Rot = Transf.at(jj).block(0,0,3,3);
-    std::cout << Rot.eulerAngles(0, 1, 2) << std::endl;
+
+    // std::cout << "______joint: " << jj << std::endl;
+    // std::cout << ">> KDL: " << jj << std::endl;
+    // std::cout << joint_pose.at(jj) << std::endl;
+    // std::cout << "<< EIGEN matrix: " << jj << std::endl;
+    // std::cout << Transf.at(jj) << std::endl;
+    // std::cout << "trans: " << jj << std::endl;
+    // std::cout << Transf.at(jj).block(0,3,3,1) << std::endl;
+    // std::cout << "rot: " << jj << std::endl;
+    // Matrix3d Rot = Transf.at(jj).block(0,0,3,3);
+    // std::cout << Rot.eulerAngles(0,1,2) << 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);
     this->arm_.T_base_to_joint.at(jj+1) = T_base_to_joint.at(jj+1);
-    // this->arm_.T_base_to_joint.at(jj+1) = Transf.at(jj);
   }
 
-  // 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 << "mass: " << this->arm_.mass << std::endl;
+  // std::cout << "arm_cg: " << arm_cg << std::endl;
   // std::cout << "cog: " << this->ctrl_params_.cog_arm << std::endl;
 
   // Fill arm CoG information
@@ -653,7 +662,6 @@ MatrixXd CQuadarm_Task_Priority_Ctrl::calcPinv(const MatrixXd &a){
 }
 Matrix4d CQuadarm_Task_Priority_Ctrl::GetTransform(const Frame &f)
 {
-
   double yaw,pitch,roll;
   Matrix3d R;
   Matrix4d T;
@@ -662,12 +670,10 @@ Matrix4d CQuadarm_Task_Priority_Ctrl::GetTransform(const Frame &f)
   f.M.GetRPY(roll,pitch,yaw);
   
   // euler convention zyx    
-  R = Eigen::AngleAxisd(roll, Vector3d::UnitX()) \
+  R = Eigen::AngleAxisd(yaw, Vector3d::UnitZ()) \
     * Eigen::AngleAxisd(pitch, Vector3d::UnitY()) \
-    * Eigen::AngleAxisd(yaw, Vector3d::UnitZ());    
+    * Eigen::AngleAxisd(roll, Vector3d::UnitX());    
   T.block(0,0,3,3)=R;
-  // Vector3d kdltrans((double)f.p.x(),(double)f.p.y(),(double)f.p.z()); 
-  // T.block(0,3,3,1) = R.transpose()*kdltrans;    
   T(0,3) = (double)f.p.x();
   T(1,3) = (double)f.p.y();
   T(2,3) = (double)f.p.z();