diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 29a9655c213a02ad5103a1868adab9cc4f3bfa71..60c76a91193290af41d34a0694fea9f01f31ed4c 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -266,8 +266,8 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
   double lambdaL = this->ctrl_params_.jntlim_gain;
 
   // task velocity
-  this->ctrl_params_.jntlim_vel = NIR_VS * JL_pseudo * lambdaL * sigmaL; // As secondary
-  //this->ctrl_params_.jntlim_vel = NIR_VS_G * JL_pseudo * lambdaL * sigmaL;
+  //this->ctrl_params_.jntlim_vel = NIR_VS * JL_pseudo * lambdaL * sigmaL; // As secondary
+  this->ctrl_params_.jntlim_vel = NIR_VS_G * JL_pseudo * lambdaL * sigmaL;
 
   // Total velocities _______________________________
 
@@ -287,10 +287,12 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
 
   MatrixXd Vtotal(4+this->arm_.nj,1);
 
+  // Task 0 + 1 + 2
+  //Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel; 
   // Task 0 + 1 + 3
-  Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.jntlim_vel; 
+  //Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.jntlim_vel; 
   // All tasks
-  //Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel + this->ctrl_params_.jntlim_vel; 
+  Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel + this->ctrl_params_.jntlim_vel; 
 
   Vtotal = this->ctrl_params_.lambda_robot.array()*Vtotal.array();
 
@@ -425,7 +427,7 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
     // HT r.t. base link
     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).block(0,2,3,1);
+    IIIrdcolRot_b_x.at(jj+1) = T_base_to_joint.at(jj+1).inverse().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