uncalibvs_sim_alg.cpp

Go to the documentation of this file.

#include "uncalibvs_sim_alg.h"

// Class Constructor
Uncalibvs_simAlgorithm::Uncalibvs_simAlgorithm(void) {
   base_rtw();
   initZeros();
}

// Class Destructor
Uncalibvs_simAlgorithm::~Uncalibvs_simAlgorithm(void){
}

// Save Current Config
void Uncalibvs_simAlgorithm::config_update(Config& new_cfg, uint32_t level){
  this->lock();

  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}

// Uncalibvs_simAlgorithm Public API
void Uncalibvs_simAlgorithm::uncalib_vs_sim(const bool& traditional, const bool& random_points, const bool& quadrotor, const bool& arm_unina, const bool& arm_catec, const bool& output_files, const std::string& path, const double& lambda, const Eigen::MatrixXf& lambda_robot, const double& final_z,  const Eigen::Matrix4f& cTo, const double& secs, const Eigen::MatrixXf& V2, KDL::Chain& chain, Eigen::MatrixXf& QQ,  const Eigen::Matrix3f& Rquad_inertial, Eigen::MatrixXf& Vel_quad){
  this->lock();
  
  traditional_=traditional;
  random_points_=random_points;
  quadrotor_=quadrotor;
  arm_unina_=arm_unina;
  arm_catec_=arm_catec;
  output_files_=output_files;
  path_=path;
  cTo_=cTo;
  secs_=secs;
  V2_=V2;
  chain_=chain;
  QQ_=QQ;
  lambda_=lambda;
  lambda_robot_=lambda_robot;
  final_z_=final_z;
  Rquad_inertial_=Rquad_inertial;
  
  if (secs_==0) {
   base_rtw();
   initZeros();
  }
  // Desired position of the object r.t. camera (invert to see the camera r.t. object)
  //T0c_x_.block(0,0,3,3) << -0.995, -0.0998, 0, 0.0998, -0.995, 0, 0, 0, 1;
  T0c_x_.block(0,0,3,3) << -1, 0, 0, 0, -1, 0, 0, 0, 1;
  T0c_x_.col(3) << 0, 0.3, -final_z_, 1;
  T0c_x_.row(3) << 0, 0, 0, 1;
  //   T0c_x_.block(0,0,3,3) << -1, 0, 0, 0, -1, 0, 0, 0, 1;
  //   T0c_x_.col(3) << 0, 0, -1, 1;
  //   T0c_x_.row(3) << 0, 0, 0, 1;
  
  T0c_0_=cTo_.inverse();

 
  // if(output_files_){
  //   const char * outputFilename       = path_.c_str();
  //   std::ofstream myfile;
  //   myfile.open (outputFilename, std::ios::out | std::ios::app | std::ios::binary);
  //   myfile << cTo_;
  //   myfile << "\n \n";
  //   myfile.close();
  // }
  
  //camera update
  //T0c_new = diTj_int + T0c_0;
  //Tc0 = T0c_new.inverse();

  T0c_new_=T0c_0_;

  // Extracting values 

  Eigen::MatrixXf cam_pose(6,1);
  Eigen::Matrix3f Rot;
  cam_pose(0,0)=T0c_new_(0,3);
  cam_pose(1,0)=T0c_new_(1,3);
  cam_pose(2,0)=T0c_new_(2,3);
  Rot=T0c_new_.block(0,0,3,3);
  cam_pose.block(3,0,3,1)=Rot.eulerAngles(0,1,2);

  if(output_files_){
    const char * outputFilename  = "/home/asantamaria/Desktop/poses.txt";
    std::ofstream myfile;
    myfile.open (outputFilename, std::ios::out | std::ios::app | std::ios::binary);
    myfile << cam_pose;
    myfile << "\n \n";
    myfile.close();

  }

  Tc0_=cTo_;
  //Actual features
  //Features transform r.t.camera 
  cP_ = Tc0_ * P0_;
  //image_projection(f,ff,u0,v0,cP);

  //features_vector(cP);
  ccP_=cP_;

  features_vector();
  S_ = s_;

  //Desired features_vector
  Eigen::Matrix4f Tc0_x;
  Tc0_x=T0c_x_.inverse();
  cP_x_ = Tc0_x * P0_; 
  //image_projection(f,ff,u0,v0,cP_x);     
  //features_vector(cP);
  ccP_=cP_x_;
   
  features_vector();
  S_x_ = s_;

  //Computation of image jacobian
  //Object coordinates r.t. camera frame (in this case world)
  pj_ = Tc0_ * pn_;
 
  //image_projection(f,f,u0,v0,pj);
  image_projection(ff_,ff_);

  //noise add
  Eigen::MatrixXf ran(2,n_);
  ran.setRandom();
  uv_ = uv_ + noise_ * ran;

  //Jacobian
  if (traditional_){
    compute_jacobian_t();
  }
  else {
    compute_jacobian_u();  
  }

  //Diferential change of T0c under Vc
  K_control(); 

  Eigen::Matrix4f diTj;
  diTj=Eigen::Matrix4f::Zero();
  
  diTj = T0c_new_ * V_mod_;

  //Integration aproximation
  diTj_int_ = diTj_int_ + diTj * secs_;
 
  if (quadrotor_){ 

    if (this->arm_unina_ || this->arm_catec_){
      qa_kinematics();
      qa_control();
      //Arm positions
      QQ_.block(6,0,this->nj_,1) = QQ_.block(6,0,this->nj_,1)+(Velqa_.block(6,0,this->nj_,1) * secs_);
      QQ=QQ_;
      
    }
    else {
      q_control();
    }
  }

  Vel_quad=Vel_;
  
  
  this->unlock();
}

void Uncalibvs_simAlgorithm::base_rtw(){

  if (random_points_){  
    n_=9;
    // Points of the object
    pn_ = Eigen::MatrixXf::Zero(4,n_);
    pn_=1*pn_.setRandom();
    pn_.row(3) = Eigen::VectorXf::Ones(n_);

   
    cj_0_= Eigen::MatrixXf::Zero(3,3); 
    cj_0_(0,0)=pn_.row(0).mean();
    cj_0_(1,0)=pn_.row(1).mean();
    cj_0_(2,0)=pn_.row(2).mean();

    // Computing the base points
    Eigen::MatrixXf q(3,n_);
    q=pn_.block(0,0,3,n_);
    q.colwise()-=cj_0_.col(0);

    // Assemble the correlation matrix Q = q * q'
    Eigen::Matrix3f Q = (q * q.transpose());

    // Compute the Singular Value Decomposition
    Eigen::JacobiSVD<Eigen::Matrix3f> svd (Q, Eigen::ComputeFullU | Eigen::ComputeFullV);
    //Eigen::Matrix3f u = svd.matrixU ();
    Eigen::Matrix3f v = svd.matrixV ();

    cj_0_.col(1)=cj_0_.col(0)+v.col(0);
    //with this library the second column of v appear sign changed
    cj_0_.col(2)=cj_0_.col(0)-v.col(1);

    // check the planarity
    if ( svd.singularValues()(2)<0.00001){
      planar_=true; 
    }
    else {
      planar_=false; 
    }

    Eigen::MatrixXf dd1(3,1),dd2(3,1),dd3(3,1);       
    if (planar_){
      std::cout << "\n" << \
      "**************************************** Planar case ***************************************" \
      << std::endl;  
      P0_= Eigen::MatrixXf::Zero(4,3);  
      P0_.block(0,0,3,3)=cj_0_;
      P0_.row(3) << 1, 1, 1;  
   
      Eigen::MatrixXf D(3,2),DD(2,2);
    
      dd1=cj_0_.col(1).array()-cj_0_.col(0).array();
      dd2=cj_0_.col(2).array()-cj_0_.col(0).array();
      D.col(0)=dd1;
      D.col(1)=dd2;
      DD=D.transpose()*D;
    
      // alfas linear combination of vectors
      alfas_= Eigen::MatrixXf::Zero(3,n_);
      // alfas.resize(3,n);
      alfas_.block(1,0,2,4)=(DD.inverse()*D.transpose()) * q;
      //alfas.block(1,0,2,4)=D.inverse() * q;
      alfas_.row(0)=Eigen::MatrixXf::Ones(1,n_)-alfas_.block(1,0,2,n_).colwise().sum();
    }  
    else{
      std::cout << "\n" << \
      "**************************************** NON Planar case ***************************************" \
      << std::endl;
    
      // resizing the base points to 4
      Eigen::Matrix3f Temp;
      Temp=cj_0_;
      cj_0_.resize(3,4);
      cj_0_.block(0,0,3,3)=Temp;
    
      cj_0_.col(3)=cj_0_.col(0)+v.col(2);
      P0_= Eigen::MatrixXf::Zero(4,4); 
      // P0.resize(4,4);
      P0_.block(0,0,3,4)=cj_0_;
      P0_.row(3) << 1, 1, 1, 1;

      dd1=cj_0_.col(1).array()-cj_0_.col(0).array();
      dd2=cj_0_.col(2).array()-cj_0_.col(0).array();
      dd3=cj_0_.col(3).array()-cj_0_.col(0).array();

      Eigen::MatrixXf D(3,3),DD(2,2);
      D.col(0)=dd1;
      D.col(1)=dd2;
      D.col(2)=dd3;
      DD=D.transpose()*D;
      //alfas linear combination of vectors
      alfas_= Eigen::MatrixXf::Zero(4,n_);
      // alfas.resize(4,n);
      alfas_.block(1,0,3,n_)=(DD.inverse()*D.transpose()) * q;
      // alfas.block(1,0,3,4)=D.inverse() * q;
      alfas_.row(0)=Eigen::MatrixXf::Ones(1,n_)-alfas_.block(1,0,3,n_).colwise().sum();  
    }
  }
  else {
    n_=9;
  
    //original points
    pn_ = Eigen::MatrixXf::Zero(4,n_);
    pn_.row(0) << -0.124725, -0.431413, 0.375723, 0.658402, -0.29928, 0.314608, -0.203127, -0.0350187, -0.70468;
    pn_.row(1) << 0.86367, 0.477069, -0.668052, -0.339326, 0.37334, 0.717353, 0.629534, -0.56835, 0.762124;
    pn_.row(2) << 0.86162, 0.279958, -0.119791, -0.542064, 0.912936, -0.12088, 0.368437, 0.900505, 0.282161;
    pn_.row(3) << 1, 1, 1, 1, 1, 1, 1, 1, 1;

    cj_0_= Eigen::MatrixXf::Zero(3,4);
  
    cj_0_.row(0) << -0.0499455, -0.559669, 0.127189, 0.791961;
    cj_0_.row(1) << 0.249707, 0.962512, 0.884623, 0.547683;
    cj_0_.row(2) << 0.313654, 0.795411, -0.438347, 0.763547;

  
    //base points
    P0_ = Eigen::MatrixXf::Zero(4,4);
  
    P0_.row(0) << -0.0499456, -0.559669, 0.127189, 0.791961;
    P0_.row(1) << 0.249707, 0.962513, 0.884623, 0.547683;
    P0_.row(2) << 0.313654, 0.795411, -0.438347, 0.763547;
    P0_.row(3) << 1, 1, 1, 1;

    //alfas: linear combination of vectors
    alfas_= Eigen::MatrixXf::Zero(4,n_); 
    alfas_.row(0) << -0.0707542, 0.826173, 2.37142, 1.76233, 0.815866, 0.122787, 0.443096, 2.23324, 0.495841;
    alfas_.row(1) << 0.73974, 0.340274, -1.07997, -1.19318, 0.503927, -0.0618209, 0.375215, -0.308004, 0.683816;
    alfas_.row(2) << -0.0355018, 0.102124, -0.181349, 0.394987, -0.41633, 0.68826, 0.172827, -0.958066, 0.233048;
    alfas_.row(3) << 0.366516, -0.268571, -0.110102, 0.0358625, 0.0965375, 0.250773, 0.00886206, 0.0328262, -0.412705;
    
  //  //original points
  //     pn_ = Eigen::MatrixXf::Zero(4,n_);
  //     pn_.row(0) << 0.0970, 0.1089, 0.0919, 0.1033, 0.0829, 0.1032, 0.0997, 0.1109, 0.1008;
  //     pn_.row(1) << 0.1029, 0.0885, 0.0706, 0.0925, 0.0990, 0.1031, 0.0984, 0.1111, 0.0879;
  //     pn_.row(2) << 0.0921, 0.0893, 0.1144, 0.1137, 0.0976, 0.0914, 0.1063, 0.0914, 0.0889;
  //     pn_.row(3) << 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000;
  // 
  //     cj_0_= Eigen::MatrixXf::Zero(3,4);
  //   
  //     cj_0_.row(0) << 0.0100, -0.0208, 0.0801, 0.0743;
  //     cj_0_.row(1) << 0.0095, -0.0647, -0.0505, 0.0393;
  //     cj_0_.row(2) << 0.0098, 0.0694, -0.0287, 0.0804;
  // 
  //   
  //     //base points
  //     P0_ = Eigen::MatrixXf::Zero(4,4);
  //   
  //     P0_.row(0) << 0.0100, -0.0208, 0.0801, 0.0743;
  //     P0_.row(1) << 0.0095, -0.0647, -0.0505, 0.0393;
  //     P0_.row(2) << 0.0098, 0.0694, -0.0287, 0.0804;
  //     P0_.row(3) << 1.0000, 1.0000, 1.0000, 1.0000;
  // 
  //     //alfas: linear combination of vectors
  //     alfas_= Eigen::MatrixXf::Zero(4,n_); 
  //     alfas_.row(0) << 1.1706, 0.9225, 0.6814, 0.7986, 1.2252, 1.1152, 0.9649, 1.1180, 1.0036;
  //     alfas_.row(1) << -0.0879, -0.0345, 0.3005, 0.0990, 0.0174, -0.1131, 0.0219, -0.1960, -0.0072;
  //     alfas_.row(2) << -0.0446, 0.1363, 0.0285, -0.0207, -0.1405, 0.0009, -0.0524, 0.0073, 0.0852;
  //     alfas_.row(3) << -0.0381, -0.0243, -0.0104, 0.1232, -0.1020, -0.0030, 0.0656, 0.0707, -0.0816;
  //     
    
    planar_=false;
  }
  //   ROS_INFO_STREAM(pn_);
  //   ROS_INFO_STREAM(cj_0_);
  //   ROS_INFO_STREAM(P0_);
  //   ROS_INFO_STREAM(alfas_);
}

void Uncalibvs_simAlgorithm::initZeros(){

  if (planar_){
  //features generic
  s_= Eigen::MatrixXf::Zero(6,1);
  //actual features
  S_= Eigen::MatrixXf::Zero(6,1);
  //desired features
  S_x_= Eigen::MatrixXf::Zero(6,1);
  //Actual points camera frame
  cP_= Eigen::MatrixXf::Zero(4,3);
  //Desired points camera frame
  cP_x_= Eigen::MatrixXf::Zero(4,3);
  //Points generic camera frame
  ccP_= Eigen::MatrixXf::Zero(4,3);
  //Image Jacobian
  J_= Eigen::MatrixXf::Zero(6,6);
  diff_=Eigen::MatrixXf::Zero(6,1);
  }
  else {


  //features generic
  s_= Eigen::MatrixXf::Zero(8,1);
  //actual features
  S_= Eigen::MatrixXf::Zero(8,1);
  //desired features
  S_x_= Eigen::MatrixXf::Zero(8,1);
  //Actual points camera frame
  cP_= Eigen::MatrixXf::Zero(4,4);
  //Desired points camera frame
  cP_x_= Eigen::MatrixXf::Zero(4,4);
  //Points generic camera frame
  ccP_= Eigen::MatrixXf::Zero(4,4);
  //Image Jacobian
  J_= Eigen::MatrixXf::Zero(8,6);
  diff_=Eigen::MatrixXf::Zero(8,1);
  }


  //image coordinates
  uv_= Eigen::MatrixXf::Zero(2,n_);  
  //Initial integral condition
  diTj_int_=Eigen::Matrix4f::Zero();
  //Velocity commands
  Vel_= Eigen::MatrixXf::Zero(6,1);  
  //lambda=0.125;
  u0_=640;
  v0_=480;
  ff_=400;
  ff_old_=ff_;
  noise_=0;
  phi_old_=0;
  
  //Arm initial positions
  //q_ = Eigen::MatrixXf::Zero(11,1);
}

void Uncalibvs_simAlgorithm::image_projection(const float& fu, const float& fv){  

  //image plane projection
  uv_.row(0) = (pj_.row(0).array()/pj_.row(2).array()) * fu + u0_;
  uv_.row(1) = (pj_.row(1).array()/pj_.row(2).array()) * fv + v0_;  
}

void Uncalibvs_simAlgorithm::features_vector(){

  
  if (planar_){
    Eigen::ArrayXXf xx(1,3),yy(1,3);
    //features vector
    xx=(ccP_.row(0).array()/ccP_.row(2).array());
    yy=(ccP_.row(1).array()/ccP_.row(2).array());  
    //reshape (actually Eigen has no reshape function)
    s_(0,0)=xx(0,0);
    s_(1,0)=yy(0,0);
    s_(2,0)=xx(0,1);
    s_(3,0)=yy(0,1);
    s_(4,0)=xx(0,2);
    s_(5,0)=yy(0,2);
  }
  else {
    Eigen::ArrayXXf xx(1,4),yy(1,4);
    //features vector
    xx=(ccP_.row(0).array()/ccP_.row(2).array());
    yy=(ccP_.row(1).array()/ccP_.row(2).array());  
    //reshape (actually Eigen has no reshape function)
    s_(0,0)=xx(0,0);
    s_(1,0)=yy(0,0);
    s_(2,0)=xx(0,1);
    s_(3,0)=yy(0,1);
    s_(4,0)=xx(0,2);
    s_(5,0)=yy(0,2);    
    s_(6,0)=xx(0,3);  
    s_(7,0)=yy(0,3);
  }
}

void Uncalibvs_simAlgorithm::compute_jacobian_t(){
  if (planar_){
    Eigen::ArrayXXf xx(1,3),yy(1,3),zz(1,3);
    //features vector
    xx=(cP_.row(0).array()/cP_.row(2).array());
    yy=(cP_.row(1).array()/cP_.row(2).array());  
    zz=(cP_.row(2).array());
    //Computing Image Jacobian
    float xj,yj,zj;
    for (int i=0; i<3; ++i){
      xj=xx(0,i);
      yj=yy(0,i);
      zj=zz(0,i);
      J_.row((i*2)) << (-1.0/zj), 0.0, (xj/zj), (xj*yj), -(1.0 + (xj*xj)), yj;
      J_.row((i*2)+1) << 0.0, (-1.0/zj), (yj/zj), (1.0 + (yj*yj)), -(xj*yj), -xj;
    }
  }
  else {
    Eigen::ArrayXXf xx(1,4),yy(1,4),zz(1,4);
    //features vector
    xx=(cP_.row(0).array()/cP_.row(2).array());
    yy=(cP_.row(1).array()/cP_.row(2).array());  
    zz=(cP_.row(2).array());
    //Computing Image Jacobian
    float xj,yj,zj;
    for (int i=0; i<4; ++i){
      xj=xx(0,i);
      yj=yy(0,i);
      zj=zz(0,i);
      J_.row((i*2)) << (-1.0/zj), 0.0, (xj/zj), (xj*yj), -(1.0 + (xj*xj)), yj;
      J_.row((i*2)+1) << 0.0, (-1.0/zj), (yj/zj), (1.0 + (yj*yj)), -(xj*yj), -xj;
    }         
  }

  // Frame changes 
  Eigen::MatrixXf Jtemp(J_.rows(),J_.cols()); 
  Jtemp=J_;
  J_.col(0)=-Jtemp.col(1);
  J_.col(1)=-Jtemp.col(0);
  J_.col(2)=-Jtemp.col(2);
  J_.col(3)=Jtemp.col(5);
  J_.col(4)=Jtemp.col(4);
  J_.col(5)=Jtemp.col(3); 
}

void Uncalibvs_simAlgorithm::compute_jacobian_u(){
  if (planar_) { 
    Eigen::MatrixXf m;
    m=Eigen::MatrixXf::Zero((2*n_),9);
    
    //Generate M and check the rank
    for (int i=0; i<n_; ++i){
      for (int j=0; j<3; ++j){
        //u rows
        m((2*i),(j*3))=alfas_(j,i);
        m((2*i),(j*3)+2)=alfas_(j,i)*(u0_-uv_(0,i));
        //v rows
        m((2*i)+1,(j*3)+1)=alfas_(j,i);
        m((2*i)+1,(j*3)+2)=alfas_(j,i)*(v0_-uv_(1,i));
      }   
    }
    
    // Assemble the correlation matrix M = m' * m
    Eigen::MatrixXf M(9,9),UU(9,9),VV(9,9);
    M = (m.transpose() * m);

    // Compute the Singular Value Decomposition
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigensolver(M);
    Eigen::VectorXf v(9,1);
    v =eigensolver.eigenvectors().col(0);

    //Distance equations
    int g=0;
    Eigen::Vector3f d,va,vb,vc,w;
    Eigen::Vector2f b;
    Eigen::MatrixXf L(3,2);
    L=Eigen::MatrixXf::Zero(3,2);
    
    for (int i=0; i<2; ++i){
      for (int j=i+1; j<3; ++j){
        va=v.block(i*3,0,3,1);
        vb=v.block(j*3,0,3,1);
        vc=va-vb;
        L(g,0)=(vc.topRows(2).transpose()*vc.topRows(2));
        L(g,1)= (vc(2)*vc(2));
        w = cj_0_.col(i)-cj_0_.col(j);
        d(g,0)=(w.transpose() * w);
        g=g+1;
      }
    }
  
    Eigen::Matrix2f LL;
    Eigen::MatrixXf L_inv(3,2);

    LL=(L.transpose()*L);
    L_inv=LL.inverse()*L.transpose();
    b(0)= L_inv.row(0) * d;
    b(1)= L_inv.row(1) * d;

    //Obtaining Beta and ff

    float Beta=+sqrt(fabs(b(0)));
    ff_=+sqrt(fabs(b(1)))/Beta;

    ff_old_=ff_;

    Eigen::VectorXf C2(9,1);

    C2=Beta*v;

    //Check the sqrt correct sign
    Eigen::Matrix3f check;
    check.col(0)=C2.block(0,0,3,1);
    check.col(1)=C2.block(3,0,3,1);
    check.col(2)=C2.block(6,0,3,1);
    
    if (check.determinant()>0){
      C2=-C2;  
    }
    C2(2,0)=C2(2,0)*ff_;
    C2(5,0)=C2(5,0)*ff_;
    C2(8,0)=C2(8,0)*ff_;

    //Computing Image Jacobian
    float xj,yj,zj;
    for (int i=0; i<3; ++i){
      xj=C2((i*3),0);
      yj=C2((i*3)+1,0);
      zj=C2((i*3)+2,0);
      J_.row((i*2)) << (-1.0/zj), 0.0, (xj/zj), (xj*yj), -(1.0 + (xj*xj)), yj;
      J_.row((i*2)+1) << 0.0, (-1.0/zj), (yj/zj), (1.0 + (yj*yj)), -(xj*yj), -xj;
    }
  }

  else {
    Eigen::MatrixXf m;
    m=Eigen::MatrixXf::Zero((2*n_),12);

    //Generate M and check the rank
    for (int i=0; i<n_; ++i){
      for (int j=0; j<4; ++j){
        //u rows
        m((2*i),(j*3))=alfas_(j,i);
        m((2*i),(j*3)+2)=alfas_(j,i)*(u0_-uv_(0,i));
    
        //v rows
        m((2*i)+1,(j*3)+1)=alfas_(j,i);
        m((2*i)+1,(j*3)+2)=alfas_(j,i)*(v0_-uv_(1,i));
      }   
    }

    // Assemble the correlation matrix M = m' * m
    Eigen::MatrixXf M(12,12),UU(12,12),VV(12,12);
    M = (m.transpose() * m);

    // Compute the Singular Value Decomposition
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigensolver(M);
    Eigen::VectorXf v(12,1);
    v =eigensolver.eigenvectors().col(0);

    //Distance equations
    int g=0;
    Eigen::Vector3f va,vb,vc,w;
    Eigen::VectorXf d(6,1);
    Eigen::Vector2f b;
    Eigen::MatrixXf L;
    L=Eigen::MatrixXf::Zero(6,2);
    
    for (int i=0; i<3; ++i){
      for (int j=i+1; j<4; ++j){
        va=v.block(i*3,0,3,1);
        vb=v.block(j*3,0,3,1);
        vc=va-vb;
        L(g,0)=(vc.topRows(2).transpose()*vc.topRows(2));
        L(g,1)= (vc(2)*vc(2));
        w = cj_0_.col(i)-cj_0_.col(j);
        d(g,0)=(w.transpose() * w);
        g=g+1;
      }
    }

    Eigen::Matrix2f LL;
    Eigen::MatrixXf L_inv(6,2);

    LL=(L.transpose()*L);
    L_inv=LL.inverse()*L.transpose();
    b(0)= L_inv.row(0) * d;
    b(1)= L_inv.row(1) * d;
    
    //Obtaining Beta and ff
    float Beta=+sqrt(fabs(b(0)));
    ff_=+sqrt(fabs(b(1)))/Beta;

    ff_old_=ff_;
    
    Eigen::VectorXf C2(12,1);

    C2=Beta*v;

    //Check the sqrt correct sign
    Eigen::Vector3f cc1,cc2,cc3;
    cc1=(C2.block(3,0,3,1)-C2.block(0,0,3,1));
    cc2=(C2.block(6,0,3,1)-C2.block(0,0,3,1));
    cc3=(C2.block(9,0,3,1)-C2.block(0,0,3,1));
     
    Eigen::Vector3f ccc1,ccc2,ccc3;
    ccc1=cc1.cross(cc2);
    ccc2=cc1.cross(cc3);
    ccc3=cc2.cross(cc3);
     
    float oo1,oo2,oo3; 
    oo1=(ccc1.transpose()*cc3);
    oo2=(ccc2.transpose()*cc2);
    oo3=(ccc3.transpose()*cc1);
     
    if ((oo1 < 0) || (oo2>0) || (oo3<0)){
      C2=-C2;  
    }
    C2(2,0)=C2(2,0)*ff_;
    C2(5,0)=C2(5,0)*ff_;
    C2(8,0)=C2(8,0)*ff_;
    C2(11,0)=C2(11,0)*ff_;
    
    //Computing Image Jacobian
    float xj,yj,zj;
    for (int i=0; i<4; ++i){
      xj=C2((i*3),0);
      yj=C2((i*3)+1,0);
      zj=C2((i*3)+2,0);
      J_.row((i*2)) << (-1.0/zj), 0.0, (xj/zj), (xj*yj), -(1.0 + (xj*xj)), yj;
      J_.row((i*2)+1) << 0.0, (-1.0/zj), (yj/zj), (1.0 + (yj*yj)), -(xj*yj), -xj;
    }   
  }

  // Frame changes 
  Eigen::MatrixXf Jtemp(J_.rows(),J_.cols()); 
  Jtemp=J_;
  J_.col(0)=Jtemp.col(1);
  J_.col(1)=Jtemp.col(0);
  J_.col(2)=-Jtemp.col(2);
  J_.col(3)=Jtemp.col(5);
  J_.col(4)=Jtemp.col(4);
  J_.col(5)=Jtemp.col(3);  
}

void Uncalibvs_simAlgorithm::K_control(){
  if (planar_) {
    // features error
    diff_=S_.array()-S_x_.array();
    
    //Proportional control
    Eigen::MatrixXf J_pseudo(6,6);
    J_pseudo=Eigen::MatrixXf::Zero(6,6);
    J_pseudo=J_.inverse();
    Vel_=-lambda_*(J_pseudo*diff_);
  }

  else {
    // features error
    diff_=S_.array()-S_x_.array();
    
    Eigen::MatrixXf JJ(6,6);

    JJ=J_.transpose()*J_;
    Eigen::MatrixXf J_pseudo(8,6);
    J_pseudo=Eigen::MatrixXf::Zero(8,6);
    J_pseudo=JJ.inverse()*J_.transpose();  

    //Proportional control
    Vel_=-lambda_*(J_pseudo*diff_);
  }
  
  Eigen::MatrixXf v_linear(3,1),v_angular(3,1);
  Eigen::Matrix3f skew_v_angular;
    
  //Lineal Velocity
  v_linear = Vel_.block(0,0,3,1);
    
  //Angular velocity
  v_angular = Vel_.block(3,0,3,1);

  //Skew-symmetric matrix 
  skew_v_angular.row(0) << 0, -v_angular(2), v_angular(1);
  skew_v_angular.row(1) << v_angular(2), 0, -v_angular(0);
  skew_v_angular.row(2) << -v_angular(1), v_angular(0), 0;

  //Using modified velocity matrix 
  V_mod_.block(0,0,3,3)=skew_v_angular;
  V_mod_.block(0,3,3,1)=v_linear;
  V_mod_.row(3) << 0, 0, 0, 0; 

  // Extracting values 
  if(output_files_){
    const char * outputFilename  = "/home/asantamaria/Desktop/features_error.txt";
    std::ofstream myfile;
    myfile.open (outputFilename, std::ios::out | std::ios::app | std::ios::binary);
    myfile << diff_;
    myfile << "\n \n";
    myfile.close();
  }
}

void Uncalibvs_simAlgorithm::q_control(){
  // Underactuated Quadrotor /////////////////////////////////////////////        
  
  Eigen::MatrixXf J1(8,4),J2(8,2),V1(4,1);
      
  J1.col(0)=J_.col(0);
  J1.col(1)=J_.col(1);
  J1.col(2)=J_.col(2);
  J1.col(3)=J_.col(5);

  J2.col(0)=J_.col(3);
  J2.col(1)=J_.col(4);

  Eigen::MatrixXf JJ1(4,4);
  Eigen::MatrixXf J1_pseudo(4,8);
  //pseudo inverse of J1
  JJ1=J1.transpose()*J1;
  J1_pseudo=JJ1.inverse()*J1.transpose();  
 
  //V1=-J1_pseudo*((lambda*diff)+(J2*V2_));
  V1=-J1_pseudo*lambda_*(diff_+(J2*V2_));
      
  Eigen::MatrixXf Velq(6,1);
  Velq=Eigen::MatrixXf::Zero(6,1);

  Velq.block(0,0,3,1)=V1.block(0,0,3,1);
  Velq(5,0)=V1(3,0);
      
  Vel_=Velq.block(0,0,6,1);
}

void Uncalibvs_simAlgorithm::qa_kinematics(){
  //Initialize Quadrotor and Arm terms
  // constructs the kdl solvers in non-realtime
  jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(chain_));
  jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));
   
  // Create joint array
  this->nj_ = chain_.getNrOfJoints();
  jnt_pos_ = KDL::JntArray(this->nj_);
  // resizes the joint state vectors in non-realtime
  jnt_pos_.resize(this->nj_);
  jacobian_.resize(this->nj_);
   
  for(unsigned int i=0; i < this->nj_; i++){
    jnt_pos_.data(i) = QQ_(6+i,0);
  }

  // computes Cartesian pose in realtime
  KDL::Frame kdlTarm;
  jnt_to_pose_solver_->JntToCart(jnt_pos_, kdlTarm);
    
  double ro,pit,ya;
  kdlTarm.M.GetEulerZYX(ya,pit,ro);
    
  Eigen::Matrix4f Tarm, Tiner;
  Eigen::Matrix3f Rarm;
  // euler convention zyx    
  Rarm = Eigen::AngleAxisf(ro, Eigen::Vector3f::UnitZ()) \
        * Eigen::AngleAxisf(pit, Eigen::Vector3f::UnitY()) \
        * Eigen::AngleAxisf(ya, Eigen::Vector3f::UnitX());
 
  Tarm.block(0,0,3,3)=Rarm;
  Tarm(0,3) = (double)kdlTarm.p.data[0];
  Tarm(1,3) = (double)kdlTarm.p.data[1];
  Tarm(2,3) = (double)kdlTarm.p.data[2];
  Tarm.row(3) << 0, 0, 0, 1;
   
  // Tiner=Tc0_*Tarm.inverse();
  Tiner=T0c_new_*Tarm.inverse();
    
  //Quadrotor linear positions
  QQ_.block(0,0,3,1) = Tiner.block(0,3,3,1);
  Eigen::Matrix3f Riner;
  Riner=Tiner.block(0,0,3,3);
  //Quadrotor angles
  QQ_.block(3,0,3,1) = Riner.eulerAngles(0,1,2);
   
  // compute Jacobian in realtime
  jnt_to_jac_solver_->JntToJac(jnt_pos_, jacobian_);
  
  Jqa_=Eigen::MatrixXf::Zero(6,6+this->nj_);
  
  // Quadrotor Jacobian

  //Jqa_.block(0,0,6,6)=(Eigen::MatrixXf::Identity(6,6));

  double psi,theta,phi;
  psi=QQ_(3,0);
  theta=QQ_(4,0);
  phi=QQ_(5,0);
  
  Eigen::MatrixXf Jq(6,6);
  Jq(0,0)=cos(psi)*cos(theta);
  Jq(0,1)=sin(phi)*cos(psi)*sin(theta)-cos(phi)*sin(psi);
  Jq(0,2)=cos(phi)*cos(psi)*sin(theta)+sin(phi)*sin(psi);
  Jq(1,0)=sin(psi)*cos(theta);
  Jq(1,1)=sin(phi)*sin(psi)*sin(theta)+cos(phi)*cos(psi);

  Jq(1,2)=cos(phi)*sin(psi)*sin(theta)-sin(phi)*cos(psi);
  Jq(2,0)=-sin(theta);
  Jq(2,1)=sin(phi)*cos(theta);
  Jq(2,2)=cos(phi)*cos(theta);
  Jq.block(0,3,3,3)=Eigen::MatrixXf::Zero(3,3);
  Jq.block(3,0,3,3)=Eigen::MatrixXf::Zero(3,3);
  Jq(3,3)=1;
  Jq(3,4)=sin(phi)*tan(theta);
  Jq(3,5)=cos(phi)*tan(theta);
  Jq(4,3)=0;
  Jq(4,4)=cos(phi);
  Jq(4,5)=-sin(phi);
  Jq(5,3)=0;
  Jq(5,4)=sin(phi)*cos(theta);
  Jq(5,5)=cos(phi)*cos(theta);

  Jqa_.block(0,0,6,6)=Jq.block(0,0,6,6);
  Jqa_.block(0,1,6,2)=-Jqa_.block(0,1,6,2);
  Jqa_.block(0,4,6,2)=-Jqa_.block(0,4,6,2);
  //convert from doubles to float
  for (int i=0; i<6; ++i){
    for (int j=0; j<(int)this->nj_; ++j){
      Jqa_(i,6+j)=(float)jacobian_.data(i,j);
    }
  }
  //ROS_INFO_STREAM(Jqa_);
}

void Uncalibvs_simAlgorithm::qa_control(){
  // Underactuated Quadrotor /////////////////////////////////////////////////// 

  Eigen::MatrixXf J1(8,4),J2(8,2),V1(4,1),Jqa1(6,4+this->nj_),Jqa2(6,2),Vtotal(4+this->nj_,1);
    
  // Computing secondary task ////////////////////////
  if (this->arm_unina_) v_null_space_unina();
  else if (this->arm_catec_) v_null_space_catec();

  // 9 DOF Extracting wx and wy from quadrotor //////////////////

  Jqa1.block(0,0,6,3)=Jqa_.block(0,0,6,3);
  Jqa1.block(0,3,6,1+this->nj_)=Jqa_.block(0,5,6,1+this->nj_);
  Jqa2=Jqa_.block(0,3,6,2);
  Eigen::MatrixXf eye(4+this->nj_,4+this->nj_);
  eye=Eigen::MatrixXf::Identity(4+this->nj_,4+this->nj_);

  // compute pseudo inverse /////////////
  Eigen::MatrixXf Jqa1_pseudo(4+this->nj_,6);
  Jqa1_pseudo = calcPinv(Jqa1);

  //ROS_INFO_STREAM(this->Vel_);

  // this->Vel_(0,0) = 0.1;
  // this->Vel_(1,0) = 0.1;
  // this->Vel_(2,0) = 0.1;
  // this->Vel_(3,0) = 0.1;
  // this->Vel_(4,0) = 0.1;
  // this->Vel_(5,0) = 0.1;

  // Control law ////////////////////////
  Vtotal = (Jqa1_pseudo * (Vel_-Jqa2*V2_)) + (eye-(Jqa1_pseudo*Jqa1))*Vqa0_; 

  Vtotal = this->lambda_robot_.array()*Vtotal.array();

  // Check a singular configuration /////
  if (std::isnan(Vtotal(0,0)))
  {
    Vtotal=Eigen::MatrixXf::Zero(4+this->nj_,1);
  } 

ROS_INFO_STREAM(Vel_);

  // Rearranging ////////////////////////
  Velqa_=Eigen::MatrixXf::Zero(6+this->nj_,1);
  Velqa_.block(0,0,3,1)=Vtotal.block(0,0,3,1);
  Velqa_.block(5,0,1+this->nj_,1)=Vtotal.block(3,0,1+this->nj_,1);
  Velqa_.block(6,0,1,1)=-Velqa_.block(6,0,1,1);
  Vel_=Velqa_.block(0,0,6,1);

  // 11DOF Without extracting wx and wy ///////////////////////////////////////////

  // Eigen::MatrixXf eye2(6+this->nj_,6+this->nj_);
  // eye2=Eigen::MatrixXf::Identity(6+this->nj_,6+this->nj_);
  // Eigen::MatrixXf Vqa0_2=Eigen::MatrixXf::Zero(11,1); 
  // Vqa0_2.block(0,0,4+this->nj_,1)=Vqa0_;

  // compute pseudo inverses ///////////
  // Eigen::MatrixXf Jqa_pseudo(6+this->nj_,6);
  // Jqa_pseudo = calcPinv(Jqa_);

  // Control law ///////////////////////
  // Velqa_=Jqa_pseudo*Vel_+(eye2-(Jqa_pseudo*Jqa_))*Vqa0_2;
  // Vel_=Velqa_.block(0,0,6,1);


  // Extracting values 
  if(output_files_){
    const char * outputFilename  = path_.c_str();
    std::ofstream myfile;
    myfile.open (outputFilename, std::ios::out | std::ios::app | std::ios::binary);
    myfile << Vtotal;
    myfile << "\n \n";
    myfile.close();
  }
}

void Uncalibvs_simAlgorithm::v_null_space_unina(){

  float ndof = 4+this->nj_;
  Eigen::MatrixXf w(this->nj_,1);
  Vqa0_= Eigen::MatrixXf::Zero(ndof,1);
  
  // num of segments
  // unsigned int js = chain_.getNrOfSegments();

  // joints positions
  //Eigen::MatrixXf qa(this->nj_,1);
  //qa = this->QQ_.block(6,0,this->nj_,1);

  // std::vector<KDL::Segment> segment(js);
  // std::vector<KDL::Frame> frame(this->nj_);
  // std::vector<Eigen::Matrix4f> transf(this->nj_);

  // segment.reserve(js);
  // frame.reserve(this->nj_);
  // transf.reserve(this->nj_);

  // segment.push_back(chain_.getSegment(0));
  // frame.push_back(seg[0].pose(qa(0,0)));
  // transf.push_back(GetTransform(frame[0]));
  // ROS_INFO_STREAM(frame[0].M);

  // for (unsigned int i = 0; i < 1; i++)
  // {
  //   seg.push_back(chain_.getSegment(i));

  //   // the first link is a fixed one and we are not intereseted in it
  //   if (i>0)
  //   {
  //     frame.push_back(seg[i].pose(qa(i-1,0)));
  //     transf.push_back(GetTransform(frame[i]));
  //     ROS_INFO_STREAM(transf[i]);
  //   }    
  // }
  KDL::Segment segment0,segment1,segment2,segment3,segment4,segment5;
  segment0 = chain_.getSegment(0);
  segment1 = chain_.getSegment(1);
  segment2 = chain_.getSegment(2);
  segment3 = chain_.getSegment(3);
  segment4 = chain_.getSegment(4);
  segment5 = chain_.getSegment(5);

  double dq=0.01;

  Eigen::MatrixXf qa_old(this->nj_,1),qa_new(this->nj_,1);
  qa_old=this->QQ_.block(6,0,this->nj_,1);
  qa_new=this->QQ_.block(6,0,this->nj_,1);

  for (unsigned int i = 0; i < this->nj_; ++i){

    qa_old(i,0)=qa_old(i,0)-dq;
    qa_new(i,0)=qa_new(i,0)+dq;

    KDL::Frame pose1_old,pose2_old,pose3_old,pose4_old,pose5_old;
    KDL::Frame pose1_new,pose2_new,pose3_new,pose4_new,pose5_new;
    pose1_old=segment1.pose(qa_old(0,0));
    pose2_old=segment2.pose(qa_old(1,0));
    pose3_old=segment3.pose(qa_old(2,0));
    pose4_old=segment4.pose(qa_old(3,0));
    pose5_old=segment5.pose(qa_old(4,0));
    pose1_new=segment1.pose(qa_new(0,0));
    pose2_new=segment2.pose(qa_new(1,0));
    pose3_new=segment3.pose(qa_new(2,0));
    pose4_new=segment4.pose(qa_new(3,0));
    pose5_new=segment5.pose(qa_new(4,0));

    Eigen::Matrix4f T1_old,T2_old,T3_old,T4_old,T5_old;
    Eigen::Matrix4f T1_new,T2_new,T3_new,T4_new,T5_new;
    T1_old=GetTransform(pose1_old);
    T2_old=GetTransform(pose2_old);
    T3_old=GetTransform(pose3_old);
    T4_old=GetTransform(pose4_old);
    T5_old=GetTransform(pose5_old);
    T1_new=GetTransform(pose1_new);
    T2_new=GetTransform(pose2_new);
    T3_new=GetTransform(pose3_new);
    T4_new=GetTransform(pose4_new);
    T5_new=GetTransform(pose5_new);

    Eigen::MatrixXf pini(4,1);
    pini.col(0) << 0,0,0,1;
  
    //Fixed transform between base_link and arm_base
    Eigen::Matrix4f Tbase=Eigen::Matrix4f::Zero();
    Tbase(0,2)=1;
    Tbase(1,1)=1;
    Tbase(2,0)=-1;
    Tbase(3,3)=1;

    Eigen::MatrixXf joint0_old(4,1),joint1_old(4,1),joint2_old(4,1),joint3_old(4,1),joint4_old(4,1),joint5_old(4,1);
    Eigen::MatrixXf joint0_new(4,1),joint1_new(4,1),joint2_new(4,1),joint3_new(4,1),joint4_new(4,1),joint5_new(4,1);
    joint0_old=Tbase*pini;
    joint1_old=Tbase*T1_old*pini;
    joint2_old=Tbase*T1_old*T2_old*pini;
    joint3_old=Tbase*T1_old*T2_old*T3_old*pini;
    joint4_old=Tbase*T1_old*T2_old*T3_old*T4_old*pini;
    joint5_old=Tbase*T1_old*T2_old*T3_old*T4_old*T5_old*pini;
    joint0_new=Tbase*pini;
    joint1_new=Tbase*T1_new*pini;
    joint2_new=Tbase*T1_new*T2_new*pini;
    joint3_new=Tbase*T1_new*T2_new*T3_new*pini;
    joint4_new=Tbase*T1_new*T2_new*T3_new*T4_new*pini;
    joint5_new=Tbase*T1_new*T2_new*T3_new*T4_new*T5_new*pini;

    // mass of each link
    double m0,m1,m2,m3,m4;
    m0=0;
    m1=0;
    m2=50;
    m3=50;
    m4=10;

    // middle link distances 
    Eigen::MatrixXf d0_old(3,1),d1_old(3,1),d2_old(3,1),d3_old(3,1),d4_old(3,1);
    Eigen::MatrixXf d0_new(3,1),d1_new(3,1),d2_new(3,1),d3_new(3,1),d4_new(3,1);
    d0_old=(joint1_old-joint0_old)/2;
    d1_old=(joint2_old-joint1_old)/2;
    d2_old=(joint3_old-joint2_old)/2;
    d3_old=(joint4_old-joint3_old)/2;
    d4_old=(joint5_old-joint4_old)/2;
    d0_new=(joint1_new-joint0_new)/2;
    d1_new=(joint2_new-joint1_new)/2;
    d2_new=(joint3_new-joint2_new)/2;
    d3_new=(joint4_new-joint3_new)/2;
    d4_new=(joint5_new-joint4_new)/2;

    Eigen::MatrixXf cg0_old,cg1_old,cg2_old,cg3_old,cg4_old;
    Eigen::MatrixXf cg0_new,cg1_new,cg2_new,cg3_new,cg4_new;

    cg0_old=m0*(joint0_old+d0_old);
    cg1_old=m1*(joint1_old+d1_old);
    cg2_old=m2*(joint2_old+d2_old);
    cg3_old=m3*(joint3_old+d3_old);
    cg4_old=m4*(joint4_old+d4_old);
    cg0_new=m0*(joint0_new+d0_new);
    cg1_new=m1*(joint1_new+d1_new);
    cg2_new=m2*(joint2_new+d2_new);
    cg3_new=m3*(joint3_new+d3_new);
    cg4_new=m4*(joint4_new+d4_new); 

    Eigen::MatrixXf rcm_old(3,1),rcm_new(3,1);
    rcm_old=(cg0_old+cg1_old+cg2_old+cg3_old+cg4_old)/(m0+m1+m2+m3+m4);
    rcm_new=(cg0_new+cg1_new+cg2_new+cg3_new+cg4_new)/(m0+m1+m2+m3+m4);

    w(i,0)=-(((rcm_new(0,0)*rcm_new(0,0)+rcm_new(1,0)*rcm_new(1,0))-(rcm_old(0,0)*rcm_old(0,0)+rcm_old(1,0)*rcm_old(1,0)))/(2*dq));

    // ROS_INFO_STREAM(T1_old);
    // ROS_INFO_STREAM(T2_old);
    // ROS_INFO_STREAM(T3_old);
    // ROS_INFO_STREAM(T4_old);
    // ROS_INFO_STREAM(T5_old);
    // ROS_INFO_STREAM(joint0_old);
    // ROS_INFO_STREAM(joint1_old);
    // ROS_INFO_STREAM(joint2_old);
    // ROS_INFO_STREAM(joint3_old);
    // ROS_INFO_STREAM(joint4_old);
    // ROS_INFO_STREAM(joint5_old);
    // ROS_INFO_STREAM(rcm_old);


  }

  Vqa0_.block(4,0,5,1)=100*w;
  // ROS_INFO_STREAM(Vqa0_);

  // // Stabilize quadrotor ///////////////////////////////////////////////
  //float dq = 0.001;

  //for (unsigned int i = 0; i < this->nj_; ++i)
  //{
  //     Eigen::MatrixXf q_old(this->nj_,1),q_new(this->nj_,1);
  //     Eigen::Matrix4f T_old,T_new,inv_T_old,inv_T_new;

  //     q_old=this->QQ_.block(6,0,this->nj_,1);
  //     q_old(i,0)=q_old(i,0)-dq;
  //     q_new=this->QQ_.block(6,0,this->nj_,1);
  //     q_new(i,0)=q_new(i,0)+dq;

  //     //Arm kinematics OLD values ///////////////////////////////
  //     for (unsigned int j = 0; j < this->nj_; ++j)
  //     {
  //       jnt_pos_.data(j) = q_old(j,0);
  //     }
  //     // computes Cartesian pose in realtime
  //     KDL::Frame kdlTarm;
  //     jnt_to_pose_solver_->JntToCart(jnt_pos_, kdlTarm);
  //     double ro,pit,ya;
  //     kdlTarm.M.GetEulerZYX(ya,pit,ro);
  //     Eigen::Matrix3f Rarm;
  //     // euler convention zyx    
  //     Rarm = Eigen::AngleAxisf(ya, Eigen::Vector3f::UnitZ()) \
  //          * Eigen::AngleAxisf(pit, Eigen::Vector3f::UnitY()) \
  //          * Eigen::AngleAxisf(ro, Eigen::Vector3f::UnitX());
  //     T_old.block(0,0,3,3)=Rarm;
  //     T_old(0,3) = (double)kdlTarm.p.data[0];
  //     T_old(1,3) = (double)kdlTarm.p.data[1];
  //     T_old(2,3) = (double)kdlTarm.p.data[2];
  //     T_old.row(3) << 0, 0, 0, 1;
  //     //Arm kinematics NEW values

  //     for (unsigned int j = 0; j < this->nj_; ++j)
  //     {
  //       jnt_pos_.data(j) = q_new(j,0);
  //     }
  //     // computes Cartesian pose in realtime
  //     jnt_to_pose_solver_->JntToCart(jnt_pos_, kdlTarm);
  //     kdlTarm.M.GetEulerZYX(ya,pit,ro);
  //     // euler convention zyx    
  //     Rarm = Eigen::AngleAxisf(ya, Eigen::Vector3f::UnitZ()) \
  //          * Eigen::AngleAxisf(pit, Eigen::Vector3f::UnitY()) \
  //          * Eigen::AngleAxisf(ro, Eigen::Vector3f::UnitX());
  //     T_new.block(0,0,3,3)=Rarm;
  //     T_new(0,3) = (double)kdlTarm.p.data[0];
  //     T_new(1,3) = (double)kdlTarm.p.data[1];
  //     T_new(2,3) = (double)kdlTarm.p.data[2];
  //     T_new.row(3) << 0, 0, 0, 1;
  //     inv_T_old = this->T0c_new_*T_old.inverse();
  //     inv_T_new = this->T0c_new_*T_new.inverse();

  //     Eigen::MatrixXf angles_old(3,1),angles_new(3,1);
  //     Eigen::Matrix3f Rot_old,Rot_new;
  //     double psi_old,psi_new,theta_old,theta_new;
  //     Rot_old = inv_T_old.block(0,0,3,3);
  //     Rot_new = inv_T_new.block(0,0,3,3);

  //     angles_old=Rot_old.eulerAngles(2,1,0);
  //     angles_new=Rot_new.eulerAngles(2,1,0);
    
  //     psi_old=angles_old(0);
  //     theta_old=angles_old(1);
  //     psi_new=angles_new(0);
  //     theta_new=angles_new(1);


  //     // ROS_INFO("Rots");
  //     // Eigen::MatrixXf angles(3,1);
  //     // angles=this->Rquad_inertial_.eulerAngles(2,1,0);
  //     // ROS_INFO_STREAM(angles_old);
  //     // ROS_INFO_STREAM(angles);


  //     w(i,0)=(((psi_new*psi_new)+(theta_new*theta_new))-((psi_old*psi_old)+(theta_old*theta_old)))/dq;
  // }

  // Vqa0_.block(4,0,this->nj_,1)=0*w;


  // Joint Limits ////////////////////////////////////////////////////77

  // Vqa0_.block(4,0,this->nj_,1)=Eigen::MatrixXf::Zero(this->nj_,1);

  // ROS_INFO_STREAM(w);
  
  // float qxma,qxmi,qxm,qyma,qymi,qym,qzma,qzmi,qzm,qyama,qyami,qyam,a0ma,a0mi,a0m,a1ma,a1mi,a1m,a2ma,a2mi,a2m,a3ma,a3mi,a3m,a4ma,a4mi,a4m;
    
  // //DOF's
  // float ndof=9;
            
  // //quadrotor Limits
  // qxma=10;
  // qxmi=-10;
  // qxm=0.0;
      
  // qyma=10;
  // qymi=-10;
  // qym=0.0;
      
  // qzma=10;
  // qzmi=1.0;
  // qzm=4.0;
      
  // qyama=3.1416;
  // qyami=-3.1416;
  // qyam=0.0;
      
  // //arm limits
  // a0ma=0.548;
  // a0mi=-0.548;
  // a0m=0.0;
  // //a0ma=0.3;
  // //a0mi=-0.3;
  // //a0m=0.0;
      
  // a1ma=1.57;
  // a1mi=-1.57;
  // a1m=0.0;
  // //a1ma=1.0;
  // //a1mi=-1.0;
  // //a1m=0.0;
      
  // a2ma=5.23;
  // a2mi=0;
  // a2m=2.615;
  // //a2ma=5.0;
  // //a2mi=0.0;
  // //a2m=2.5;      
       
  // a3ma=5.23;
  // a3mi=0;
  // a3m=2.615;
  // //a3ma=5.0;
  // //a3mi=0.0;
  // //a3m=2.5;
      
  // a4ma=3.1416;
  // a4mi=-3.1416;
  // a4m=0.0;
  // //a4ma=6.0;
  // //a4mi=0.0;
  // //a4m=3.0;
          
  // Vqa0_(0,0)=(-0/ndof)*((QQ_(0,0)-qxm)/((qxma-qxmi)*(qxma-qxmi)));
  // Vqa0_(1,0)=(-0/ndof)*((QQ_(1,0)-qym)/((qyma-qymi)*(qyma-qymi)));
  // Vqa0_(2,0)=(-50/ndof)*((QQ_(2,0)-qzm)/((qzma-qzmi)*(qzma-qzmi)));
  // Vqa0_(3,0)=(-30/ndof)*((QQ_(5,0)-qyam)/((qyama-qyami)*(qyama-qyami)));
  // Vqa0_(4,0)=(-100/ndof)*((QQ_(6,0)-a0m)/((a0ma-a0mi)*(a0ma-a0mi)));
  // Vqa0_(5,0)=(-100/ndof)*((QQ_(7,0)-a1m)/((a1ma-a1mi)*(a1ma-a1mi)));
  // Vqa0_(6,0)=(-100/ndof)*((QQ_(8,0)-a2m)/((a2ma-a2mi)*(a2ma-a2mi)));
  // Vqa0_(7,0)=(-100/ndof)*((QQ_(9,0)-a3m)/((a3ma-a3mi)*(a3ma-a3mi)));
  // Vqa0_(8,0)=(-30/ndof)*((QQ_(10,0)-a4m)/((a4ma-a4mi)*(a4ma-a4mi)));
}

void Uncalibvs_simAlgorithm::v_null_space_catec(){

  //DOF's
  float ndof=4+this->nj_;
  Vqa0_=Eigen::MatrixXf::Zero(ndof,1);
  // float qxma,qxmi,qxm,qyma,qymi,qym,qzma,qzmi,qzm,qyama,qyami,qyam,a0ma,a0mi,a0m,a1ma,a1mi,a1m,a2ma,a2mi,a2m,a3ma,a3mi,a3m,a4ma,a4mi,a4m,a5ma,a5mi,a5m;
            
  // //quadrotor Limits
  // qxma=10;
  // qxmi=-10;
  // qxm=0.0;
      
  // qyma=10;
  // qymi=-10;
  // qym=0.0;
      
  // qzma=10;
  // qzmi=1.0;
  // qzm=4.0;
      
  // qyama=3.1416;
  // qyami=-3.1416;
  // qyam=0.0;
      
  // //arm limits
  // a0ma=0.8726;
  // a0mi=-0.8726;
  // a0m=0.0;
  // //a0ma=0.3;
  // //a0mi=-0.3;
  // //a0m=0.0;
  
      
  // a1ma=3.14159;
  // a1mi=0.0;
  // a1m=1.5707;
  // //a1ma=1.0;
  // //a1mi=-1.0;
  // //a1m=0.0;
      
  // a2ma=0.0;
  // a2mi=-3.14159;
  // a2m=-1.5707;
  // //a2ma=5.0;
  // //a2mi=0.0;
  // //a2m=2.5;      
       
  // a3ma=2.6179;
  // a3mi=-2.6179;
  // a3m=0;
  // //a3ma=5.0;
  // //a3mi=0.0;
  // //a3m=2.5;
      
  // a4ma=1.5707;
  // a4mi=-1.5707;
  // a4m=0.0;
  // //a4ma=6.0;
  // //a4mi=0.0;
  // //a4m=3.0;

  // a5ma=2.6179;
  // a5mi=-2.6179;
  // a5m=0.0;
  
  // Vqa0_(0,0)=(-0/ndof)*((QQ_(0,0)-qxm)/((qxma-qxmi)*(qxma-qxmi)));
  // Vqa0_(1,0)=(-0/ndof)*((QQ_(1,0)-qym)/((qyma-qymi)*(qyma-qymi)));
  // Vqa0_(2,0)=(-0/ndof)*((QQ_(2,0)-qzm)/((qzma-qzmi)*(qzma-qzmi)));
  // Vqa0_(3,0)=(-0/ndof)*((QQ_(5,0)-qyam)/((qyama-qyami)*(qyama-qyami)));
  // Vqa0_(4,0)=(-0/ndof)*((QQ_(6,0)-a0m)/((a0ma-a0mi)*(a0ma-a0mi)));
  // Vqa0_(5,0)=(-0/ndof)*((QQ_(7,0)-a1m)/((a1ma-a1mi)*(a1ma-a1mi)));
  // Vqa0_(6,0)=(-0/ndof)*((QQ_(8,0)-a2m)/((a2ma-a2mi)*(a2ma-a2mi)));
  // Vqa0_(7,0)=(-0/ndof)*((QQ_(9,0)-a3m)/((a3ma-a3mi)*(a3ma-a3mi)));
  // Vqa0_(8,0)=(-0/ndof)*((QQ_(10,0)-a4m)/((a4ma-a4mi)*(a4ma-a4mi)));
  // Vqa0_(9,0)=(-0/ndof)*((QQ_(11,0)-a5m)/((a5ma-a5mi)*(a5ma-a5mi)));
}

Eigen::MatrixXf Uncalibvs_simAlgorithm::calcPinv(const Eigen::MatrixXf &a){
using namespace Eigen;
        // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
        const MatrixXf* m;
        MatrixXf t;
        MatrixXf m_pinv;

        // transpose so SVD decomp can work...
        if ( a.rows()<a.cols() ){
                t = a.transpose();
                m = &t;
        } else {
                m = &a;
        }

        // SVD
        //Eigen::JacobiSVD<MatrixXd> svd = m->jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::JacobiSVD<MatrixXf> svd = m->jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
        Eigen::MatrixXf vSingular = svd.singularValues();
        // Build a diagonal matrix with the Inverted Singular values
        // The pseudo inverted singular matrix is easy to compute :
        // is formed by replacing every nonzero entry by its reciprocal (inversing).
        Eigen::MatrixXf vPseudoInvertedSingular(svd.matrixV().cols(),1);
        for (int iRow =0; iRow<vSingular.rows(); iRow++) {
                                                         // Todo : Put epsilon in parameter
                if ( fabs(vSingular(iRow))<=1e-10 ) {
                        vPseudoInvertedSingular(iRow,0)=0.;
                }
                else {
                        vPseudoInvertedSingular(iRow,0)=1./vSingular(iRow);
                }
        }
        // A little optimization here
        Eigen::MatrixXf mAdjointU = svd.matrixU().adjoint().block(0,0,vSingular.rows(),svd.matrixU().adjoint().cols());
        // Pseudo-Inversion : V * S * U'
        m_pinv = (svd.matrixV() *  vPseudoInvertedSingular.asDiagonal()) * mAdjointU  ;

        // transpose back if necessary
        if ( a.rows()<a.cols() )
                return m_pinv.transpose();
        
        return m_pinv;
}

void Uncalibvs_simAlgorithm::pseudo_inverse(const Eigen::MatrixXf& M, const Eigen::MatrixXf& Y, Eigen::MatrixXf& X){
 // Performs the pseudo inverse such as: m*X=y -> X=(m⁻1*y)

 X = M.fullPivHouseholderQr().solve(Y);
 assert(Y.isApprox(M*X));
}

Eigen::Matrix4f Uncalibvs_simAlgorithm::GetTransform(const KDL::Frame &f){

  double ya,pi,ro;
  Eigen::Matrix3f R;
  Eigen::Matrix4f T;

  f.M.GetEulerZYX(ya,pi,ro);
  
  // euler convention zyx    
  R = Eigen::AngleAxisf(ya, Eigen::Vector3f::UnitZ()) \
    * Eigen::AngleAxisf(pi, Eigen::Vector3f::UnitY()) \
    * Eigen::AngleAxisf(ro, Eigen::Vector3f::UnitX());
  T.block(0,0,3,3)=R;
  T(0,3) = (double)f.p.data[0];
  T(1,3) = (double)f.p.data[1];
  T(2,3) = (double)f.p.data[2];
  T.row(3) << 0, 0, 0, 1;

  return T;
}