C++ Tree::getSegments方法代码示例

std::string getDOFName(KDL::Tree & tree, const unsigned int index)
{
    std::string retVal = "";
    for (KDL::SegmentMap::const_iterator it=tree.getSegments().begin(); it!=tree.getSegments().end(); ++it)
    {
        if( GetTreeElementQNr(it->second) == index )
        {
            retVal = GetTreeElementSegment(it->second).getJoint().getName();
        }
    }

    return retVal;
}

std::vector<std::string> getLinkAttachedToFixedJoints(KDL::Tree & tree)
{
    std::vector<std::string> ret;
    for(KDL::SegmentMap::const_iterator seg=tree.getSegments().begin();
        seg != tree.getSegments().end();
        seg++)
    {
        if( GetTreeElementSegment(seg->second).getJoint().getType() == KDL::Joint::None )
        {
            ret.push_back(GetTreeElementSegment(seg->second).getName());
        }
    }
    return ret;
}

bool leatherman::getSegmentOfJoint(const KDL::Tree &tree, std::string joint, std::string &segment)
{ 
  KDL::SegmentMap smap = tree.getSegments();
  for(std::map<std::string, KDL::TreeElement>::const_iterator iter = smap.begin(); iter != smap.end(); ++iter)
  { 
    if(iter->second.segment.getJoint().getName().compare(joint) == 0)
    { 
      segment = iter->second.segment.getName();
      return true;
    }
  }
  return false;
}

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;
}

bool JacoIKSolver::initFromURDF(const std::string urdf, const std::string root_name,
                            const std::string tip_name, unsigned int max_iter, std::string error)
{
    urdf::Model robot_model;
    KDL::Tree tree;

    if (!robot_model.initFile(urdf))
    {
        error += "Could not initialize robot model";
        return false;
    }

    if (!kdl_parser::treeFromFile(urdf, tree))
    {
        error += "Could not initialize tree object";
        return false;
    }

    if (tree.getSegment(root_name) == tree.getSegments().end())
    {
        error += "Could not find root link '" + root_name + "'.";
        return false;
    }

    if (tree.getSegment(tip_name) == tree.getSegments().end())
    {
        error += "Could not find tip link '" + tip_name + "'.";
        return false;
    }

    if (!tree.getChain(root_name, tip_name, chain_))
    {
        error += "Could not initialize chain object";
        return false;
    }

    // Get the joint limits from the robot model

    q_min_.resize(chain_.getNrOfJoints());
    q_max_.resize(chain_.getNrOfJoints());
    q_seed_.resize(chain_.getNrOfJoints());

    joint_names_.resize(chain_.getNrOfJoints());

    unsigned int j = 0;
    for(unsigned int i = 0; i < chain_.getNrOfSegments(); ++i)
    {
        const KDL::Joint& kdl_joint = chain_.getSegment(i).getJoint();
        if (kdl_joint.getType() != KDL::Joint::None)
        {
//            std::cout << chain_.getSegment(i).getName() << " -> " << kdl_joint.getName() << " -> " << chain_.getSegment(i + 1).getName() << std::endl;

            boost::shared_ptr<const urdf::Joint> joint = robot_model.getJoint(kdl_joint.getName());
            if (joint && joint->limits)
            {
                q_min_(j) = joint->limits->lower;
                q_max_(j) = joint->limits->upper;
                q_seed_(j) = (q_min_(j) + q_max_(j)) / 2;
            }
            else
            {
                q_min_(j) = -1e9;
                q_max_(j) = 1e9;
                q_seed_(j) = 0;

            }

            joint_names_[j] = kdl_joint.getName();

            ++j;
        }
    }
;
    
    fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(chain_)); 
    ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv(chain_));
    ik_solver_.reset(new KDL::ChainIkSolverPos_NR_JL(chain_, q_min_, q_max_, *fk_solver_, *ik_vel_solver_, max_iter));
    std::cout << "Using normal solver" << std::endl;
    
    jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));

    return true;
}

bool treeToUrdfModel(const KDL::Tree& tree, const std::string & robot_name, urdf::ModelInterface& robot_model)
{
    robot_model.clear();
    robot_model.name_ = robot_name;

    //Add all links
    KDL::SegmentMap::iterator seg;
    KDL::SegmentMap segs;
    KDL::SegmentMap::const_iterator root_seg;
    root_seg = tree.getRootSegment();
    segs = tree.getSegments();
    for( seg = segs.begin(); seg != segs.end(); seg++ ) {
        if (robot_model.getLink(seg->first))
        {
            std::cerr << "[ERR] link " << seg->first << " is not unique." << std::endl;
            robot_model.clear();
            return false;
        }
        else
        {
            urdf::LinkPtr link;
            resetPtr(link, new urdf::Link);

            //add name
            link->name = seg->first;

            //insert link
            robot_model.links_.insert(make_pair(seg->first,link));
            std::cerr << "[DEBUG] successfully added a new link " << link->name << std::endl;
        }

        //inserting joint
        //The fake root segment has no joint to add
        if( seg->first != root_seg->first ) {
            KDL::Joint jnt;
            jnt = GetTreeElementSegment(seg->second).getJoint();
            if (robot_model.getJoint(jnt.getName()))
            {
                std::cerr << "[ERR] joint " <<  jnt.getName() << " is not unique." << std::endl;
                robot_model.clear();
                return false;
            }
            else
            {
                urdf::JointPtr joint;
                urdf::LinkPtr link = robot_model.links_[seg->first];
                //This variable will be set by toUrdf
                KDL::Frame H_new_old_successor;
                KDL::Frame H_new_old_predecessor = getH_new_old(GetTreeElementSegment(GetTreeElementParent(seg->second)->second));
                urdf::resetPtr(joint, new urdf::Joint());

                //convert joint
                *joint = toUrdf(jnt, GetTreeElementSegment(seg->second).getFrameToTip(),H_new_old_predecessor,H_new_old_successor);

                //insert parent
                joint->parent_link_name = GetTreeElementParent(seg->second)->first;

                //insert child
                joint->child_link_name = seg->first;

                //insert joint
                robot_model.joints_.insert(make_pair(seg->first,joint));
                std::cerr << "[DEBUG] successfully added a new joint" << jnt.getName() << std::endl;

                //add inertial, taking in account an eventual change in the link frame
                resetPtr(link->inertial, new urdf::Inertial());
                *(link->inertial) = toUrdf(H_new_old_successor * GetTreeElementSegment(seg->second).getInertia());
            }
        }

    }

    // every link has children links and joints, but no parents, so we create a
    // local convenience data structure for keeping child->parent relations
    std::map<std::string, std::string> parent_link_tree;
    parent_link_tree.clear();

    // building tree: name mapping
    //try
    //{
        robot_model.initTree(parent_link_tree);
    //}
    /*
    catch(ParseError &e)
    {
        logError("Failed to build tree: %s", e.what());
        robot_model.clear();
        return false;
    }*/

    // find the root link
    //try
    //{
        robot_model.initRoot(parent_link_tree);
    //}
    /*
    catch(ParseError &e)
    {
        logError("Failed to find root link: %s", e.what());
        robot_model.reset();
//.........这里部分代码省略.........

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;
}

void computeTemplatedDynamicsForTree(KDL::Tree& twoBranchTree, KDL::Vector& grav, std::vector<kdle::JointState>& jointState,
                                     std::vector<kdle::SegmentState>& linkState, std::vector<kdle::SegmentState>& linkState2)
{
    printf("Templated dynamics values for Tree \n");
    kdle::transform<kdle::kdl_tree_iterator, kdle::pose> _comp1;
    kdle::transform<kdle::kdl_tree_iterator, kdle::twist> _comp2;
    kdle::transform<kdle::kdl_tree_iterator, kdle::accTwist> _comp3;
    kdle::balance<kdle::kdl_tree_iterator, kdle::force> _comp4;
    kdle::project<kdle::kdl_tree_iterator, kdle::wrench> _comp5;
    
#ifdef VERBOSE_CHECK_MAIN
    std::cout << "Transform initial state" << std::endl << linkState[0].X << std::endl;
    std::cout << "Twist initial state" << std::endl << linkState[0].Xdot << std::endl;
    std::cout << "Acc Twist initial state" << std::endl << linkState[0].Xdotdot << std::endl;
    std::cout << "Wrench initial state" << std::endl << linkState[0].F << std::endl << std::endl;
#endif

#ifdef VERBOSE_CHECK_MAIN
    std::cout << "Transform L1" << linkState[1].X << std::endl;
    std::cout << "Twist L1" << linkState[1].Xdot << std::endl;
    std::cout << "Acc Twist L1" << linkState[1].Xdotdot << std::endl;
    std::cout << "Wrench L1" << linkState[1].F << std::endl << std::endl;
#endif

#ifdef VERBOSE_CHECK_MAIN
    std::cout << "Transform L2" << linkState[2].X << std::endl;
    std::cout << "Twist L2" << linkState[2].Xdot << std::endl;
    std::cout << "Acc Twist L2" << linkState[2].Xdotdot << std::endl;
    std::cout << "Wrench L2" << linkState[2].F << std::endl << std::endl;
#endif

    //typedef Composite<kdle::func_ptr(myTestComputation), kdle::func_ptr(myTestComputation) > compositeType0;
    typedef kdle::Composite< kdle::transform<kdle::kdl_tree_iterator, kdle::twist>, kdle::transform<kdle::kdl_tree_iterator, kdle::pose> > compositeType1;
    typedef kdle::Composite< kdle::balance<kdle::kdl_tree_iterator, kdle::force>, kdle::transform<kdle::kdl_tree_iterator, kdle::accTwist> > compositeType2;
    typedef kdle::Composite<compositeType2, compositeType1> compositeType3;

//    compositeType1 composite1 = kdle::compose(_comp2, _comp1);
    compositeType3 composite2 = kdle::compose(kdle::compose(_comp4, _comp3), kdle::compose(_comp2, _comp1));

    //kdle::DFSPolicy<KDL::Tree> mypolicy;
    kdle::DFSPolicy<KDL::Tree, kdle::inward> mypolicy1;
    kdle::DFSPolicy<KDL::Tree, kdle::outward> mypolicy2;

    std::cout << std::endl << std::endl << "FORWARD TRAVERSAL" << std::endl << std::endl;

//    traverseGraph(twoBranchTree, composite2, mypolicy2)(jointState, jointState, linkState, linkState2);
     traverseGraph(twoBranchTree, composite2, mypolicy2)(jointState, linkState, linkState2);

#ifdef VERBOSE_CHECK_MAIN
    std::cout << std::endl << std::endl << "LSTATE" << std::endl << std::endl;
    for (unsigned int i = 0; i < twoBranchTree.getNrOfSegments(); i++)
    {
        std::cout << linkState[i].segmentName << std::endl;
        std::cout << std::endl << linkState[i].X << std::endl;
        std::cout << linkState[i].Xdot << std::endl;
        std::cout << linkState[i].Xdotdot << std::endl;
        std::cout << linkState[i].F << std::endl;
    }
    std::cout << std::endl << std::endl << "LSTATE2" << std::endl << std::endl;
     for (unsigned int i = 0; i < twoBranchTree.getNrOfSegments(); i++)
    {
        std::cout << linkState2[i].segmentName << std::endl;
        std::cout << std::endl << linkState2[i].X << std::endl;
        std::cout << linkState2[i].Xdot << std::endl;
        std::cout << linkState2[i].Xdotdot << std::endl;
        std::cout << linkState2[i].F << std::endl;
    }
#endif

    std::vector<kdle::SegmentState> linkState3;
    linkState3.resize(twoBranchTree.getNrOfSegments()+1);
    std::cout << std::endl << std::endl << "REVERSE TRAVERSAL" << std::endl << std::endl;
    std::vector<kdle::JointState> jstate1;
    jstate1.resize(twoBranchTree.getNrOfSegments() + 1);
    traverseGraph(twoBranchTree, _comp5, mypolicy1)(jointState, jstate1, linkState2, linkState3);
    //version 1 traversal
    //traverseGraph(twoBranchTree, kdl_extensions::func_ptr(myTestComputation), mypolicy)(1, 2, 3);    
#ifdef VERBOSE_CHECK_MAIN
    std::cout << std::endl << std::endl << "LSTATE3" << std::endl << std::endl;
    for (KDL::SegmentMap::const_reverse_iterator iter = twoBranchTree.getSegments().rbegin(); iter != twoBranchTree.getSegments().rend(); ++iter)
    {
        std::cout << std::endl << iter->first << std::endl;
        std::cout << linkState3[iter->second.q_nr].X << std::endl;
        std::cout << linkState3[iter->second.q_nr].Xdot << std::endl;
        std::cout << linkState3[iter->second.q_nr].Xdotdot << std::endl;
        std::cout << linkState3[iter->second.q_nr].F << std::endl;
        std::cout << "Joint index and torque " << iter->second.q_nr << "  " << jstate1[iter->second.q_nr].torque << std::endl;
    }
#endif
    return;
}