working with arm joint limits

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,12 +287,10 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
 
   MatrixXd Vtotal(4+this->arm_.nj,1);
 
-  // Task 0 + 1
-  //Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_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();
 
@@ -484,7 +482,6 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
   MatrixXd cog_arm_in_qi = Rib * this->arm_cog_data_.arm_cog.block(0,0,3,1);
   this->ctrl_params_.cog_PGxy = cog_arm_in_qi.block(0,0,2,1);
 
-
   // Partial CoG and jacobian of each link (augmented links: from current to end-effector)
   MatrixXd CoG_partial;
   MatrixXd Jj_cog(3,this->arm_.nj);