diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 703a1f0643f97443cc45dbd3ea641f84fb090e52..67a5bbb8d0e63fd57b4a4b668ded6e5c2bc9b82b 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -428,6 +428,7 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
     T_base_to_joint.at(jj+1) = T_base_to_joint.at(jj) * Transf.at(jj);
     // 3rd column of rotation matrix
     IIIrdcolRot_b_x.at(jj+1) = T_base_to_joint.at(jj+1).inverse().block(0,2,3,1);
+    // 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
     link_origin.at(jj+1) = T_base_to_joint.at(jj+1).col(3);
     // Distance to the middle of the link jj
@@ -472,7 +473,7 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
   // Debug
   // std::cout << "mass: " << this->arm_.mass << std::endl;
   // std::cout << "arm_cg: " << arm_cg << std::endl;
-  // std::cout << "cog: " << this->arm_cog_data_.arm_cog << std::endl;
+  //std::cout << "cog: " << this->arm_cog_data_.arm_cog << std::endl;
 
   // Rotation between quadrotor body and inertial frames  
   // TODO: If attitude is estimated, then it could be roll and pitch