kdlHelper.cpp

#include "header/kdlHelper.h"
KDLHelper::KDLHelper(std::string filename, std::string link0_name,
                     std::string endEffector_name, std::string vehicle_name,
                     std::string tool_name)
{

  TiXmlDocument file_xml(filename);
  file_xml.LoadFile();

  // parse URDF arm model

  urdf::Model model;
  if (!model.initXml(&file_xml)){
     std::cerr << "[KDL_HELPER] Failed to parse urdf robot model\n" ;
     return ;
  }
  if (!kdl_parser::treeFromUrdfModel(model, tree)){
     std::cerr << "[KDL_HELPER] Failed to construct kdl tree\n";
     return ;
  }

  nJoints = 0;
  this->link0_name = link0_name;
  this->endEffector_name = endEffector_name;
  this->vehicle_name = vehicle_name;
  this->tool_name = tool_name;

}

KDLHelper::~KDLHelper(){

  delete eePose_solver;
  delete jacobEE_solver;
  delete jacobTool_solver;

}



int KDLHelper::setEESolvers(){

  //generate the chain from link0 to final (end effector)
  KDL::Chain kinChain;
  tree.getChain(link0_name, endEffector_name, kinChain);

  nJoints = kinChain.getNrOfJoints();
  if (nJoints != ARM_DOF){
    std::cerr << "[KDL_HELPER] Something wrong with urdf:\n" <<
                 "I have found from urdf file " << nJoints << " joints " <<
                 "but from define list file I read " << ARM_DOF <<". Please check\n";
    return -1;
  }

  std::cout << "[KDL_HELPER] found " << nJoints << " joints from " << link0_name
            << " to " << endEffector_name << "\n";

  eePose_solver = new KDL::ChainFkSolverPos_recursive(kinChain);
  jacobEE_solver = new KDL::ChainJntToJacSolver(kinChain);


  return 0;
}



int KDLHelper::setToolSolvers(Eigen::Matrix4d eeTtool_eigen){

  KDL::Chain kinChain;

  tree.getChain(vehicle_name, endEffector_name, kinChain);
  // now kinChain is a chain for only the arm

  nJoints = kinChain.getNrOfJoints();
  if (nJoints != ARM_DOF){
    std::cerr << "[KDL_HELPER] Something wrong with urdf:\n" <<
                 "I have found from urdf file " << nJoints << " joints " <<
                 "but from define list file I read " << ARM_DOF <<". Please check\n";
    return -1;
  }


  // now we add a fake joint to add to kinChain a segment which is for the tool
  //(that is fixed respect to the ee)
  KDL::Frame eeTtool_kdl = CONV::transfMatrix_eigen2kdl(eeTtool_eigen);

  kinChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::None), eeTtool_kdl));

  //even if we add a segment, the number of joint of kin chain remain 4. Probably because we add a fixed joint.
  //however, this is good.

  jacobTool_solver = new KDL::ChainJntToJacSolver(kinChain);

}

int KDLHelper::setToolSolvers(){
  if (tool_name.size() == 0 ){
    std::cerr << "[KDL_HELPER] tool_name not inserted in costructor, insert it or use the other setTool\n";
    return -1;
  }

  KDL::Chain kinChain;
  tree.getChain(vehicle_name, tool_name, kinChain);

  nJoints = kinChain.getNrOfJoints();
  if (nJoints != ARM_DOF){
    std::cerr << "[KDL_HELPER] Something wrong with urdf:\n" <<
                 "I have found from urdf file " << nJoints << " joints " <<
                 "but from define list file I read " << ARM_DOF <<". Please check\n";
    return -1;
  }

  jacobTool_solver = new KDL::ChainJntToJacSolver(kinChain);


}

int KDLHelper::getEEpose(std::vector<double> jointPos, Eigen::Matrix4d *eePose_eigen){

  KDL::JntArray jointPositions = KDL::JntArray(nJoints);
  for(unsigned int i=0; i<nJoints; i++){
    jointPositions(i)=jointPos[i];
  }
  KDL::Frame eePose_kdl;
  eePose_solver->JntToCart(jointPositions, eePose_kdl);

  *eePose_eigen = CONV::transfMatrix_kdl2eigen(eePose_kdl);

  return 0;

}


int KDLHelper::getJacobianEE(std::vector<double> jointPos, Eigen::Matrix<double, 6, ARM_DOF> *jacobianEE_eigen){

  KDL::JntArray jointPositions = KDL::JntArray(nJoints);

  for(unsigned int i=0; i<nJoints; i++){
    jointPositions(i)=jointPos[i];
  }

  KDL::Jacobian jacobian_kdl;
  jacobian_kdl.resize(nJoints);
  jacobEE_solver->JntToJac(jointPositions, jacobian_kdl);

  *jacobianEE_eigen = CONV::jacobian_kdl2eigen(jacobian_kdl);

  return 0;
}

int KDLHelper::getJacobianTool(std::vector<double> jointPos, Eigen::Matrix<double, 6, ARM_DOF> *jacobianTool_eigen){

  KDL::JntArray jointPositions = KDL::JntArray(nJoints);

  for(unsigned int i=0; i<nJoints; i++){
    jointPositions(i)=jointPos[i];
  }

  KDL::Jacobian jacobian_kdl;
  jacobian_kdl.resize(nJoints); //il fixed non è considerato joint
  jacobTool_solver->JntToJac(jointPositions, jacobian_kdl);

  *jacobianTool_eigen = CONV::jacobian_kdl2eigen(jacobian_kdl);

  return 0;
}



int KDLHelper::getFixedFrame(std::string frameOrigin, std::string frameTarget, Eigen::Matrix4d *xTx_eigen){

  //generate the chain
  KDL::Chain kinChain;
  tree.getChain(frameOrigin, frameTarget, kinChain);

  int nJoints = kinChain.getNrOfJoints();
  if (nJoints != 0) {
    std::cerr << "[KDL_HELPER] Use this function only for FIXED things relative each other\n";
    return -1;
  }

  KDL::ChainFkSolverPos_recursive transfSolver(kinChain);

  //joint pos are needed to the function of the solver but they does not count for fixed thing
  KDL::JntArray jointPositions = KDL::JntArray(nJoints);
  for(unsigned int i=0; i<nJoints; i++){
    jointPositions(i)=0.0;
  }

  KDL::Frame xTx_kdl;
  transfSolver.JntToCart(jointPositions, xTx_kdl);

  *xTx_eigen = CONV::transfMatrix_kdl2eigen(xTx_kdl);

  return 0;
}


int KDLHelper::getNJoints(){return nJoints;}


















int KDLHelper::getToolpose(std::vector<double> jointPos, Eigen::Matrix4d eeTtool_eigen, Eigen::Matrix4d *toolPose_eigen){

  KDL::JntArray jointPositions = KDL::JntArray(nJoints);
  for(unsigned int i=0; i<nJoints; i++){
    jointPositions(i)=jointPos[i];
  }
  KDL::Frame toolPose_kdl;
  KDL::Chain kinChain;
  tree.getChain(link0_name, endEffector_name, kinChain);
  KDL::Frame eeTtool_kdl = CONV::transfMatrix_eigen2kdl(eeTtool_eigen);

  kinChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::None), eeTtool_kdl));
  KDL::ChainFkSolverPos_recursive toolPose_solver(kinChain);

  toolPose_solver.JntToCart(jointPositions, toolPose_kdl);

  *toolPose_eigen = CONV::transfMatrix_kdl2eigen(toolPose_kdl);

  return 0;

}











int KDLHelper::setToolSolvers_old(Eigen::Matrix4d eeTtool_eigen){

  KDL::Chain kinChain;

  tree.getChain(link0_name, endEffector_name, kinChain);
  // now kinChain is a chain for only the arm

  // now we add a fake joint to add to kinChain a segment which is for the tool
  //(that is fixed respect to the ee)
  KDL::Frame eeTtool_kdl = CONV::transfMatrix_eigen2kdl(eeTtool_eigen);

  kinChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::None), eeTtool_kdl));

  //even if we add a segment, the number of joint of kin chain remain 4. Probably because we add a fixed joint.
  //however, this is good.

  jacobTool_solver = new KDL::ChainJntToJacSolver(kinChain);

}