bool kdl_urdf_tools::initialize_kinematics_from_urdf(
    const std::string &robot_description,
    const std::string &root_link,
    const std::string &tip_link,
    unsigned int &n_dof,
    KDL::Chain &kdl_chain,
    KDL::Tree &kdl_tree,
    urdf::Model &urdf_model)
{
  if(robot_description.length() == 0) {
    ROS_ERROR("URDF string is empty.");
    return false;
  }

  // Construct an URDF model from the xml string
  urdf_model.initString(robot_description);

  // Get a KDL tree from the robot URDF
  if (!kdl_parser::treeFromUrdfModel(urdf_model, kdl_tree)){
    ROS_ERROR("Failed to construct kdl tree");
    return false;
  }

  // Populate the KDL chain
  if(!kdl_tree.getChain(root_link, tip_link, kdl_chain))
  {
    ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
    ROS_ERROR_STREAM("  "<<root_link<<" --> "<<tip_link);
    ROS_ERROR_STREAM("  Tree has "<<kdl_tree.getNrOfJoints()<<" joints");
    ROS_ERROR_STREAM("  Tree has "<<kdl_tree.getNrOfSegments()<<" segments");
    ROS_ERROR_STREAM("  The segments are:");

    KDL::SegmentMap segment_map = kdl_tree.getSegments();
    KDL::SegmentMap::iterator it;

    for( it=segment_map.begin();
        it != segment_map.end();
        it++ )
    {
      ROS_ERROR_STREAM( "    "<<(*it).first);
    }

    return false;
  }

  // Store the number of degrees of freedom of the chain
  n_dof = kdl_chain.getNrOfJoints();

  return true;
}
TorqueEstimationTree::TorqueEstimationTree(KDL::Tree& icub_kdl,
                                          std::vector< kdl_format_io::FTSensorData > ft_sensors,
                                          std::vector< std::string > ft_serialization,
                                          std::string fixed_link, unsigned int verbose)
{
    yarp::sig::Vector q_max(icub_kdl.getNrOfJoints(),1000.0);
    yarp::sig::Vector q_min(icub_kdl.getNrOfJoints(),-1000.0);

    KDL::CoDyCo::TreeSerialization serial(icub_kdl);

    std::vector<std::string> dof_serialization;

    for(int i = 0; i < serial.getNrOfDOFs(); i++ )
    {
        dof_serialization.push_back(serial.getDOFName(i));
    }

    TorqueEstimationConstructor(icub_kdl,ft_sensors,dof_serialization,ft_serialization,q_min,q_max,fixed_link,verbose);
}
void testFwdKinConsistency(std::string modelFilePath)
{
    std::cout << "Testing " << modelFilePath << std::endl;


    // Load iDynTree model and compute a default traversal
    ModelLoader loader;
    loader.loadModelFromFile(modelFilePath);
    iDynTree::Model model = loader.model();
    iDynTree::Traversal traversal;
    model.computeFullTreeTraversal(traversal);

    // Load KDL tree
    KDL::Tree tree;
    treeFromUrdfFile(modelFilePath,tree);
    KDL::CoDyCo::UndirectedTree undirected_tree(tree);
    KDL::CoDyCo::Traversal kdl_traversal;
    undirected_tree.compute_traversal(kdl_traversal);

    // A KDL::Tree only supports 0 and 1 DOFs joints, and
    // KDL::Tree::getNrOfJoints does not count the 0 dof joints, so
    // it is actually equivalent to iDynTree::Model::getNrOfDOFs
    ASSERT_EQUAL_DOUBLE(tree.getNrOfJoints(),model.getNrOfDOFs());

    // Input : joint positions and base position with respect to world
    iDynTree::FreeFloatingPos baseJntPos(model);
    iDynTree::FreeFloatingVel baseJntVel(model);
    iDynTree::FreeFloatingAcc baseJntAcc(model);

    baseJntPos.worldBasePos() =  getRandomTransform();
    baseJntVel.baseVel() = getRandomTwist();
    baseJntAcc.baseAcc() =  getRandomTwist();

    for(unsigned int jnt=0; jnt < baseJntPos.getNrOfPosCoords(); jnt++)
    {
        baseJntPos.jointPos()(jnt) = getRandomDouble();
        baseJntVel.jointVel()(jnt) = getRandomDouble();
        baseJntAcc.jointAcc()(jnt) = getRandomDouble();
    }


    // Build a map between KDL joints and iDynTree joints because we are not sure
    // that the joint indices will match
    std::vector<int> kdl2idyntree_joints;
    kdl2idyntree_joints.resize(undirected_tree.getNrOfDOFs());

    for(unsigned int dofIndex=0; dofIndex < undirected_tree.getNrOfDOFs(); dofIndex++)
    {
        std::string dofName = undirected_tree.getJunction(dofIndex)->getName();
        int idyntreeJointIndex = model.getJointIndex(dofName);
        kdl2idyntree_joints[dofIndex] = idyntreeJointIndex;
    }


    KDL::JntArray jntKDL(undirected_tree.getNrOfDOFs());
    KDL::JntArray jntVelKDL(undirected_tree.getNrOfDOFs());
    KDL::JntArray jntAccKDL(undirected_tree.getNrOfDOFs());

    KDL::Frame  worldBaseKDL;
    KDL::Twist  baseVelKDL;
    KDL::Twist  baseAccKDL;

    ToKDL(baseJntPos,worldBaseKDL,jntKDL,kdl2idyntree_joints);
    ToKDL(baseJntVel,baseVelKDL,jntVelKDL,kdl2idyntree_joints);
    ToKDL(baseJntAcc,baseAccKDL,jntAccKDL,kdl2idyntree_joints);

    // Output : link (for iDynTree) or frame positions with respect to world
    std::vector<KDL::Frame> framePositions(undirected_tree.getNrOfLinks());

    iDynTree::LinkPositions linkPositions(model);


    // Build a map between KDL links and iDynTree links because we are not sure
    // that the link indices will match
    std::vector<int> idynTree2KDL_links;
    idynTree2KDL_links.resize(model.getNrOfLinks());

    for(unsigned int linkIndex=0; linkIndex < model.getNrOfLinks(); linkIndex++)
    {
        std::string linkName = model.getLinkName(linkIndex);
        int kdlLinkIndex = undirected_tree.getLink(linkName)->getLinkIndex();
        idynTree2KDL_links[linkIndex] = kdlLinkIndex;
    }

    // Compute position forward kinematics with old KDL code and with the new iDynTree code
    KDL::CoDyCo::getFramesLoop(undirected_tree,
                               jntKDL,
                               kdl_traversal,
                               framePositions,
                               worldBaseKDL);

    ForwardPositionKinematics(model,traversal,baseJntPos,linkPositions);

    // Check results
    for(unsigned int travEl = 0; travEl  < traversal.getNrOfVisitedLinks(); travEl++)
    {
        LinkIndex linkIndex = traversal.getLink(travEl)->getIndex();
        ASSERT_EQUAL_TRANSFORM_TOL(linkPositions(linkIndex),ToiDynTree(framePositions[idynTree2KDL_links[linkIndex]]),1e-1);
    }

    // Compution velocity & acceleration kinematics
    std::vector<KDL::Twist> kdlLinkVel(undirected_tree.getNrOfLinks());
    std::vector<KDL::Twist> kdlLinkAcc(undirected_tree.getNrOfLinks());
    KDL::CoDyCo::rneaKinematicLoop(undirected_tree,jntKDL,jntVelKDL,jntAccKDL,kdl_traversal,
                                                   baseVelKDL,baseAccKDL,kdlLinkVel,kdlLinkAcc);

    iDynTree::LinkVelArray linksVels(model);
    iDynTree::LinkAccArray linksAcc(model);

    ForwardVelAccKinematics(model,traversal,baseJntPos,baseJntVel,baseJntAcc,linksVels,linksAcc);

    // Check results
    for(unsigned int travEl = 0; travEl  < traversal.getNrOfVisitedLinks(); travEl++)
    {
        LinkIndex linkIndex = traversal.getLink(travEl)->getIndex();

        ASSERT_EQUAL_VECTOR(linksVels(linkIndex).asVector(),
                            ToiDynTree(kdlLinkVel[idynTree2KDL_links[linkIndex]]).asVector());
        ASSERT_EQUAL_VECTOR(linksAcc(linkIndex).asVector(),
                            ToiDynTree(kdlLinkAcc[idynTree2KDL_links[linkIndex]]).asVector());
    }

    // Compute position, velocity and acceleration
    LinkPositions linksPos_check(model);
    iDynTree::LinkVelArray linksVels_check(model);
    iDynTree::LinkAccArray linksAcc_check(model);

    ForwardPosVelAccKinematics(model,traversal,baseJntPos, baseJntVel, baseJntAcc,
                               linksPos_check,linksVels_check,linksAcc_check);

    for(unsigned int travEl = 0; travEl  < traversal.getNrOfVisitedLinks(); travEl++)
    {
        LinkIndex linkIndex = traversal.getLink(travEl)->getIndex();
        ASSERT_EQUAL_TRANSFORM(linkPositions(linkIndex),
                               linksPos_check(linkIndex));
        ASSERT_EQUAL_VECTOR(linksVels(linkIndex).asVector(),
                            linksVels_check(linkIndex).asVector());
        ASSERT_EQUAL_VECTOR(linksAcc(linkIndex).asVector(),
                             linksAcc(linkIndex).asVector());
    }

    // Compute torques (i.e. inverse dynamics)
    LinkNetExternalWrenches fExt(model);
    LinkInternalWrenches f(model);
    FreeFloatingGeneralizedTorques baseWrenchJointTorques(model);
    KDL::JntArray jntTorques(model.getNrOfDOFs());
    KDL::Wrench   baseWrench;

    std::vector<KDL::Wrench> fExtKDL(undirected_tree.getNrOfLinks());
    std::vector<KDL::Wrench> fKDL(undirected_tree.getNrOfLinks());

    // First set to zero all the fExtKDL, fKDL wrenches : then
    // the following loop will set the one that correspond to "real"
    // iDynTree links to the same value we used in iDynTree
    for(unsigned int linkKDL = 0; linkKDL < undirected_tree.getNrOfLinks(); linkKDL++)
    {
        fKDL[linkKDL]    = KDL::Wrench::Zero();
        fExtKDL[linkKDL] = KDL::Wrench::Zero();
    }

    for(unsigned int link = 0; link < model.getNrOfLinks(); link++ )
    {
        fExt(link) = getRandomWrench();
        fExtKDL[idynTree2KDL_links[link]] = ToKDL(fExt(link));
        fKDL[idynTree2KDL_links[link]] = KDL::Wrench::Zero();
    }

    bool ok = RNEADynamicPhase(model,traversal,
                               baseJntPos.jointPos(),
                               linksVels,linksAcc,fExt,f,baseWrenchJointTorques);

    int retVal = KDL::CoDyCo::rneaDynamicLoop(undirected_tree,jntKDL,kdl_traversal,
                                 kdlLinkVel,kdlLinkAcc,
                                 fExtKDL,fKDL,jntTorques,baseWrench);

    ASSERT_EQUAL_DOUBLE(ok,true);
    ASSERT_EQUAL_DOUBLE(retVal,0);

    /***
     * This check is commented out because KDL::CoDyCo::rneaDynamicLoop returned
     * a reverse baseWrench.. still need to be investigated.
     * (The - is necessary for consistency with the mass matrix..)
     *
     */
    //ASSERT_EQUAL_VECTOR(baseWrenchJointTorques.baseWrench().asVector(),
    //                    ToiDynTree(baseWrench).asVector());

    for(int link = 0; link < model.getNrOfLinks(); link++ )
    {
        ASSERT_EQUAL_VECTOR(f(link).asVector(),ToiDynTree(fKDL[idynTree2KDL_links[link]]).asVector());
    }

    for(int dof = 0; dof < model.getNrOfDOFs(); dof++ )
    {
        ASSERT_EQUAL_DOUBLE(jntTorques(dof),baseWrenchJointTorques.jointTorques()(kdl2idyntree_joints[dof]));
    }

    // Check mass matrix
    iDynTree::FreeFloatingMassMatrix massMatrix(model);
    iDynTree::LinkInertias crbis(model);
    ok = CompositeRigidBodyAlgorithm(model,traversal,
                                     baseJntPos.jointPos(),
                                     crbis,massMatrix);

    KDL::CoDyCo::FloatingJntSpaceInertiaMatrix massMatrixKDL(undirected_tree.getNrOfDOFs()+6);
    std::vector<KDL::RigidBodyInertia> Ic(undirected_tree.getNrOfLinks());

    retVal = KDL::CoDyCo::crba_floating_base_loop(undirected_tree,kdl_traversal,jntKDL,Ic,massMatrixKDL);

    ASSERT_IS_TRUE(ok);
    ASSERT_EQUAL_DOUBLE_TOL(retVal,0,1e-8);

    // Check composite rigid body inertias
    for(int link = 0; link < model.getNrOfLinks(); link++ )
    {
         ASSERT_EQUAL_MATRIX(crbis(link).asMatrix(),ToiDynTree(Ic[idynTree2KDL_links[link]]).asMatrix());
    }

    std::cerr << "iDynTree " << massMatrix.toString() << std::endl;
    std::cerr << "massMatrix " << massMatrixKDL.data << std::endl;


    // Check CRBA algorithm
    for(int ii=0; ii < model.getNrOfDOFs()+6; ii++ )
    {
        for( int jj=0; jj < model.getNrOfDOFs()+6; jj++ )
        {
            int idyntreeII = kdl2idyntreeFloatingDOFMapping(ii,kdl2idyntree_joints);
            int idyntreeJJ = kdl2idyntreeFloatingDOFMapping(jj,kdl2idyntree_joints);
            ASSERT_EQUAL_DOUBLE_TOL(massMatrix(idyntreeII,idyntreeJJ),massMatrixKDL(ii,jj),1e-8);
        }
    }

    return;
}
bool Kinematics::build_chain(FINGERS f, const KDL::Tree &kdl_tree_){
    std::string root_name, tip_name;
    se_finger finger_info;
    std::string finger_name;

    try{
        finger_info = root_tip.at(f);
        root_name = finger_info.root_name;
        tip_name  = finger_info.tip_name;
    }catch(const std::out_of_range& oor){
        ROS_ERROR("Kinematics::build_chain no such finger type");
        return false;
    }

  //  joint_limits& joint_limits_ = finger_joint_limits.at(f);
    KDL::Chain& kdl_chain_      = kdl_chains[f];
    finger_name                 = f_enum2str.at(f);

    // Populate the KDL chain
    if(!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
    {
        ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
        ROS_ERROR_STREAM("  "<<root_name<<" --> "<<tip_name);
        ROS_ERROR_STREAM("  Tree has "<<kdl_tree_.getNrOfJoints()<<" joints");
        ROS_ERROR_STREAM("  Tree has "<<kdl_tree_.getNrOfSegments()<<" segments");
        ROS_ERROR_STREAM("  The segments are:");

        KDL::SegmentMap segment_map = kdl_tree_.getSegments();
        KDL::SegmentMap::iterator it;

        for( it=segment_map.begin(); it != segment_map.end(); it++ )
            ROS_ERROR_STREAM( "    "<<(*it).first);

        return false;
    }
    ROS_INFO("=== Loading %s KChain ===",finger_name.c_str());
    ROS_INFO("root: %s\t tip: %s",root_name.c_str(),tip_name.c_str());
    ROS_INFO("Number of segments: %d",kdl_chain_.getNrOfSegments());
    ROS_INFO("Number of joints in chain: %d",kdl_chain_.getNrOfJoints());

    // Parsing joint limits from urdf model
  /*  boost::shared_ptr<const urdf::Link> link_ = model.getLink(tip_name);
    boost::shared_ptr<const urdf::Joint> joint_;
    joint_limits_.min.resize(kdl_chain_.getNrOfJoints());
    joint_limits_.max.resize(kdl_chain_.getNrOfJoints());
    joint_limits_.center.resize(kdl_chain_.getNrOfJoints());*/

    /*int index;

    for (int i = 0; i < kdl_chain_.getNrOfJoints() && link_; i++)
    {
        joint_ = model.getJoint(link_->parent_joint->name);
        index = kdl_chain_.getNrOfJoints() - i - 1;

        joint_limits_.min(index) = joint_->limits->lower;
        joint_limits_.max(index) = joint_->limits->upper;
        joint_limits_.center(index) = (joint_limits_.min(index) + joint_limits_.max(index))/2;

        link_ = model.getLink(link_->getParent()->name);
    }
*/
  /*  std::cout<< "segment names" << std::endl;
    int index;
    for(std::size_t i = 0; i < kdl_chain_.getNrOfJoints();i++){
      //  std::cout<< kdl_chain_.segments[i].getName() << "  " << kdl_chain_.segments[i].getJoint().getName() << "  " << kdl_chain_.segments[i].getJoint().getTypeName() <<  std::endl;
        joint_ = model.getJoint(kdl_chain_.segments[i].getJoint().getName());
        index = i;
        switch(joint_->type){
        case urdf::Joint::REVOLUTE:
        {
            joint_limits_.min(i) = joint_->limits->lower;
            joint_limits_.max(i) = joint_->limits->upper;
            joint_limits_.center(i) = (joint_limits_.min(i) + joint_limits_.max(i))/2;
            ROS_INFO("joint name: %s \t type: %d \t %f %f",joint_->name.c_str(),joint_->type,joint_->limits->lower,joint_->limits->upper);
            break;
        }
        case urdf::Joint::FIXED:
        {
            ROS_INFO("joint name: %s \t type: %d",joint_->name.c_str(),joint_->type);
            break;
        }
        default:
        {
            std::cerr<< "Kinematics::build_chain no such joint type: " << joint_->type << " implemented" << std::endl;
            break;
        }
        }

    }*/

}
void drawMyTree(KDL::Tree& a_tree)
{

    //graphviz stuff
    /****************************************/
    Agraph_t *g;
    GVC_t *gvc;

    /* set up a graphviz context */
    gvc = gvContext();
    /* Create a simple digraph */
    g = agopen("robot-structure", AGDIGRAPH);

    //create vector to hold nodes
    std::vector<Agnode_t*> nodeVector;
    nodeVector.resize(a_tree.getSegments().size());
//    printf("size of segments in tree map %d\n", a_tree.getSegments().size());
//    printf("size of segments in tree %d\n", a_tree.getNrOfSegments());

    //create vector to hold edges
    std::vector<Agedge_t*> edgeVector;
    edgeVector.resize(a_tree.getNrOfJoints() + 1);
    int jointIndex = a_tree.getNrOfJoints() + 1;
//    printf("size of joint array %d %d\n", jointIndex, a_tree.getNrOfJoints());

    int segmentIndex = 0;
    //    fill in the node vector by iterating over tree segments
    for (SegmentMap::const_iterator iter = a_tree.getSegments().begin(); iter != a_tree.getSegments().end(); ++iter)

    {
        //it would have been very useful if one could access list of joints of a tree
        //list of segments is already possible
        int stringLength = iter->second.segment.getName().size();
        char name[stringLength + 1];
        strcpy(name, iter->second.segment.getName().c_str());
        //q_nr returned is the same value for the root and the its child. this is a bug
        nodeVector[iter->second.q_nr] = agnode(g, name);
        agsafeset(nodeVector[iter->second.q_nr], "color", "red", "");
        agsafeset(nodeVector[iter->second.q_nr], "shape", "box", "");
        std::cout << "index parent " << iter->second.q_nr << std::endl;
        std::cout << "name parent " << iter->second.segment.getName() << std::endl;
        std::cout << "joint name parent " << iter->second.segment.getJoint().getName() << std::endl;
        std::cout << "joint type parent " << iter->second.segment.getJoint().getType() << std::endl;
        //        if (iter->second.segment.getJoint().getType() == Joint::None) //equals to joint type None
        //        {
        //            int stringLength = iter->second.segment.getJoint().getName().size();
//                    char name[stringLength + 1];
        //            strcpy(name, iter->second.segment.getJoint().getName().c_str());
        //            edgeVector[iter->second.q_nr] = agedge(g, nodeVector[iter->second.q_nr], nodeVector[iter->second.q_nr]);
        //            agsafeset(edgeVector[iter->second.q_nr], "label", name, "");
        //        }
        if (segmentIndex < a_tree.getSegments().size())
            segmentIndex++;

    }



    //fill in edge vector by iterating over joints in the tree
    for (SegmentMap::const_iterator iter = a_tree.getSegments().begin(); iter != a_tree.getSegments().end(); ++iter)

    {
        //TODO: Fix node-edge connection relation
        int stringLength = iter->second.segment.getJoint().getName().size();
//        std::cout << "Joint name " << iter->second.segment.getJoint().getName() << std::endl;
        char name[stringLength + 1];
        strcpy(name, iter->second.segment.getJoint().getName().c_str());
        for (std::vector<KDL::SegmentMap::const_iterator>::const_iterator childIter = iter->second.children.begin(); childIter != iter->second.children.end(); childIter++)
        {
            edgeVector[iter->second.q_nr] = agedge(g, nodeVector[iter->second.q_nr], nodeVector[(*childIter)->second.q_nr]);
            agsafeset(edgeVector[iter->second.q_nr], "label", name, "");
        }

        //            if (jointIndex != 0)
        //            {
        //                edgeVector[jointIndex] = agedge(g, nodeVector[segmentIndex], nodeVector[jointIndex]);
        //                agsafeset(edgeVector[jointIndex], "label", name, "");
        //            }

    }



    /* Compute a layout using layout engine from command line args */
    //  gvLayoutJobs(gvc, g);
    gvLayout(gvc, g, "dot");

    /* Write the graph according to -T and -o options */
    //gvRenderJobs(gvc, g);
    gvRenderFilename(gvc, g, "ps", "test-invdynamics2.ps");

    /* Free layout data */
    gvFreeLayout(gvc, g);

    /* Free graph structures */
    agclose(g);

    gvFreeContext(gvc);
    /* close output file, free context, and return number of errors */
    return;
}
bool TreeKinematics::readJoints(urdf::Model &robot_model,
                                KDL::Tree &kdl_tree,
                                std::string &tree_root_name,
                                unsigned int &nr_of_jnts,
                                KDL::JntArray &joint_min,
                                KDL::JntArray &joint_max,
                                KDL::JntArray &joint_vel_max)
{
  KDL::SegmentMap segmentMap;
  segmentMap = kdl_tree.getSegments();
  tree_root_name = kdl_tree.getRootSegment()->second.segment.getName();
  nr_of_jnts = kdl_tree.getNrOfJoints();
  ROS_DEBUG( "the tree's number of joints: [%d]", nr_of_jnts );
  joint_min.resize(nr_of_jnts);
  joint_max.resize(nr_of_jnts);
  joint_vel_max.resize(nr_of_jnts);
  info_.joint_names.resize(nr_of_jnts);
  info_.limits.resize(nr_of_jnts);

  // The following walks through all tree segments, extracts their joints except joints of KDL::None and extracts
  // the information about min/max position and max velocity of the joints not of type urdf::Joint::UNKNOWN or
  // urdf::Joint::FIXED
  ROS_DEBUG("Extracting all joints from the tree, which are not of type KDL::Joint::None.");
  boost::shared_ptr<const urdf::Joint> joint;
  for (KDL::SegmentMap::const_iterator seg_it = segmentMap.begin(); seg_it != segmentMap.end(); ++seg_it)
  {
    if (seg_it->second.segment.getJoint().getType() != KDL::Joint::None)
    {
      // check, if joint can be found in the URDF model of the robot
      joint = robot_model.getJoint(seg_it->second.segment.getJoint().getName().c_str());
      if (!joint)
      {
        ROS_FATAL("Joint '%s' has not been found in the URDF robot model!", joint->name.c_str());
        return false;
      }
      // extract joint information
      if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
      {
        ROS_DEBUG( "getting information about joint: [%s]", joint->name.c_str() );
        double lower = 0.0, upper = 0.0, vel_limit = 0.0;
        unsigned int has_pos_limits = 0, has_vel_limits = 0;

        if ( joint->type != urdf::Joint::CONTINUOUS )
        {
          ROS_DEBUG("joint is not continuous.");
          lower = joint->limits->lower;
          upper = joint->limits->upper;
          has_pos_limits = 1;
          if (joint->limits->velocity)
          {
            has_vel_limits = 1;
            vel_limit = joint->limits->velocity;
            ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
          }
          else
          {
            has_vel_limits = 0;
            vel_limit = 0.0;
            ROS_DEBUG("joint has no velocity limit.");
          }
        }
        else
        {
          ROS_DEBUG("joint is continuous.");
          lower = -M_PI;
          upper = M_PI;
          has_pos_limits = 0;
          if(joint->limits && joint->limits->velocity)
          {
            has_vel_limits = 1;
            vel_limit = joint->limits->velocity;
            ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
          }
          else
          {
            has_vel_limits = 0;
            vel_limit = 0.0;
            ROS_DEBUG("joint has no velocity limit.");
          }
        }

        joint_min(seg_it->second.q_nr) = lower;
        joint_max(seg_it->second.q_nr) = upper;
        joint_vel_max(seg_it->second.q_nr) = vel_limit;
        ROS_DEBUG("pos_min = %f, pos_max = %f, vel_max = %f", lower, upper, vel_limit);

        info_.joint_names[seg_it->second.q_nr] = joint->name;
        info_.limits[seg_it->second.q_nr].joint_name = joint->name;
        info_.limits[seg_it->second.q_nr].has_position_limits = has_pos_limits;
        info_.limits[seg_it->second.q_nr].min_position = lower;
        info_.limits[seg_it->second.q_nr].max_position = upper;
        info_.limits[seg_it->second.q_nr].has_velocity_limits = has_vel_limits;
        info_.limits[seg_it->second.q_nr].max_velocity = vel_limit;
      }
    }
  }
  return true;
}
Exemple #7
0
void IkSolver::init(const KDL::Tree&                     tree,
                    const std::vector<std::string>&      endpoint_names,
                    const std::vector<EndpointCoupling>& endpoint_couplings)
{
  // Preconditions
  assert(endpoint_names.size() == endpoint_couplings.size() && "endpoints and coupling vectors size mismatch");
  // TODO: Verify that all endpoints are contained in tree

  // Enpoint names vector
  endpoint_names_ = endpoint_names;

  // Problem size
  const size_t q_dim = tree.getNrOfJoints(); // Joint space dimension
  size_t x_dim = 0;                          // Task space dimension, value assigned below

  // Populate coupled directions vector
  coupled_dirs_.resize(endpoint_names_.size());
  for (size_t i = 0; i < coupled_dirs_.size(); ++i)
  {
    if ((endpoint_couplings[i] & Position) == Position)
    {
      coupled_dirs_[i].push_back(0);
      coupled_dirs_[i].push_back(1);
      coupled_dirs_[i].push_back(2);
      x_dim += 3;
    }
    if ((endpoint_couplings[i] & Orientation) == Orientation)
    {
      coupled_dirs_[i].push_back(3);
      coupled_dirs_[i].push_back(4);
      coupled_dirs_[i].push_back(5);
      x_dim += 3;
    }
  }

  // Initialize kinematics solvers
  fk_solver_.reset(new FkSolver(tree));
  jac_solver_.reset(new JacSolver(tree));
  inverter_.reset(new Inverter(x_dim, q_dim));

  // Matrix inversion parameters TODO: Expose!
  inverter_->setLsInverseThreshold(1e-5); // NOTE: Magic values
  inverter_->setDlsInverseThreshold(1e-4);
  inverter_->setMaxDamping(0.05);

  // Default values of position solver parameters
  delta_twist_max_ = Eigen::NumTraits<double>::highest();
  delta_joint_pos_max_ = Eigen::NumTraits<double>::highest();
  velik_gain_      = 1.0;
  eps_             = Eigen::NumTraits<double>::epsilon();
  max_iter_        = 1;

  // Populate map from joint names to KDL tree indices
  joint_name_to_idx_.clear();
  const KDL::SegmentMap& tree_segments = tree.getSegments();
  for (KDL::SegmentMap::const_iterator it = tree_segments.begin(); it != tree_segments.end(); ++it)
  {
    const KDL::Joint& joint = it->second.segment.getJoint();
    if (joint.getType() != KDL::Joint::None)
    {
      joint_name_to_idx_[joint.getName()] = it->second.q_nr;
    }
  }

  // Joint space weights updater
  limits_avoider_.reset(new JointPositionLimitsAvoider(q_dim));
  limits_avoider_->setSmoothing(0.8); // NOTE: Magic value

  // Preallocate IK resources
  delta_twist_ = VectorXd::Zero(x_dim);
  delta_q_     = VectorXd::Zero(q_dim);
  q_tmp_       = VectorXd::Zero(q_dim);

  jacobian_     = Eigen::MatrixXd(x_dim, q_dim);
  jacobian_tmp_ = KDL::Jacobian(q_dim);

  Wx_ = VectorXd::Ones(x_dim);

  q_posture_           = KDL::JntArray(q_dim);
  nullspace_projector_ = Eigen::MatrixXd(q_dim, q_dim);
  identity_qdim_       = Eigen::MatrixXd::Identity(q_dim, q_dim);
}