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

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

//.........这里部分代码省略.........

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