still not working. shit!

src/quadarm_task_priority_ctrl.cpp

@@ -104,7 +104,6 @@ void CQuadarm_Task_Priority_Ctrl::uam_kinematics(){
 
   // Whole robot Jacobian
   this->uam_.jacob = MatrixXd::Zero(6,6+this->arm_.nj);
-
   this->uam_.jacob.block(0,0,6,6) = Rot*Jb2c*Ji2b;
   this->uam_.jacob.block(0,6,6,this->arm_.nj)=Rot*Ja;
 }
@@ -120,9 +119,8 @@ Matrix4d CQuadarm_Task_Priority_Ctrl::arm_f_kinematics()
   // resize the joint state vector in non-realtime
   this->arm_.jnt_pos_kdl.resize(this->arm_.nj);
 
-  for(unsigned int ii=0; ii < this->arm_.nj; ii++){
+  for(unsigned int ii=0; ii < this->arm_.nj; ii++)
     this->arm_.jnt_pos_kdl.data(ii) = this->arm_.jnt_pos(ii,0);
-  }
 
   // computes Cartesian pose in realtime From Base_link to Arm tip
   Frame kdlTarm;
@@ -164,7 +162,6 @@ MatrixXd CQuadarm_Task_Priority_Ctrl::arm_jacobian()
       Ja(ii,jj) = this->arm_.jacobian.data(ii,jj);
     }
   }
-
   return Ja;
 }
 MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian()
@@ -356,15 +353,16 @@ void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,Mat
   Jqa1.block(0,0,6,3) = this->uam_.jacob.block(0,0,6,3);
   Jqa1.block(0,3,6,1+this->arm_.nj) = this->uam_.jacob.block(0,5,6,1+this->arm_.nj);
   Jqa2 = this->uam_.jacob.block(0,3,6,2);
-  MatrixXd eye = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);
 
   // task jacobian
   JVS = Jqa1;
 
   // task jacobian pseudo inverse 
-  // JVS_pseudo = calcPinv(Jqa1);
+  //JVS_pseudo = calcPinv(Jqa1);
   JVS_pseudo = weight_jacvs_inverse(Jqa1);
 
+  //std::cout << "No weighted pseudo inverse: @line:" << __LINE__ << std::endl;
+
   // task velocity vector
   sigmaVS = this->cam_vel_-Jqa2*this->ctrl_params_.v_rollpitch;
 }