Exemple #1
0
bool framesFromKDLTree(const KDL::Tree& tree,
                       std::vector<std::string>& framesNames,
                       std::vector<std::string>& parentLinkNames)
{
    framesNames.clear();
    parentLinkNames.clear();

    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( GetTreeElementChildren(seg->second).size() == 0 &&
            GetTreeElementSegment(seg->second).getJoint().getType() == KDL::Joint::None &&
            GetTreeElementSegment(seg->second).getInertia().getMass() == 0.0 )
        {
            std::string frameName = GetTreeElementSegment(seg->second).getName();
            std::string parentLinkName = GetTreeElementSegment(GetTreeElementParent(seg->second)->second).getName();
            framesNames.push_back(frameName);
            parentLinkNames.push_back(parentLinkName);

            //also check parent
            KDL::Segment parent = GetTreeElementSegment(GetTreeElementParent(seg->second)->second);
            if (parent.getJoint().getType() == KDL::Joint::None &&
                parent.getInertia().getMass() == 0.0)
            {
                framesNames.push_back(parentLinkName);
            }
        }
    }

    return true;
}
void JointStateConverter::addChildren(const KDL::SegmentMap::const_iterator segment)
{
  const std::string& root = GetTreeElementSegment(segment->second).getName();

  const std::vector<KDL::SegmentMap::const_iterator>& children = GetTreeElementChildren(segment->second);
  for (unsigned int i=0; i<children.size(); i++){
    const KDL::Segment& child = GetTreeElementSegment(children[i]->second);
    robot_state_publisher::SegmentPair s(GetTreeElementSegment(children[i]->second), root, child.getName());
    if (child.getJoint().getType() == KDL::Joint::None){
      segments_fixed_.insert(std::make_pair(child.getJoint().getName(), s));
      ROS_DEBUG("Adding fixed segment from %s to %s", root.c_str(), child.getName().c_str());
    }
    else{
      segments_.insert(std::make_pair(child.getJoint().getName(), s));
      ROS_DEBUG("Adding moving segment from %s to %s", root.c_str(), child.getName().c_str());
    }
    addChildren(children[i]);
  }
}
 bool isBaseLinkFake(const KDL::Tree & tree)
 {
     KDL::SegmentMap::const_iterator root = tree.getRootSegment();
     bool return_value;
     if (GetTreeElementChildren(root->second).size() == 1 && GetTreeElementSegment(GetTreeElementChildren(root->second)[0]->second).getJoint().getType() == Joint::None) {
         return_value = true;
     } else {
         return_value = false;
     }
     return return_value;
 }
Exemple #4
0
void Robot::propagateTree(
    const KDL::SegmentMap::const_iterator segment, const KDL::Frame frame,
    const std::unordered_map<std::string, double> &joint_state) {
  const std::string &root = GetTreeElementSegment(segment->second).getName();

  auto joint = GetTreeElementSegment(segment->second).getJoint();

  double joint_angle = (joint.getType() == KDL::Joint::None)
                           ? 0.0
                           : joint_state.at(joint.getName());

  KDL::Frame child_frame = frame * (segment->second.segment.pose(joint_angle));
  link_frames_[root] = child_frame;

  const std::vector<KDL::SegmentMap::const_iterator> &children =
      GetTreeElementChildren(segment->second);
  for (unsigned int i = 0; i < children.size(); i++) {
    propagateTree(children[i], child_frame, joint_state);
  }
}
	Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it)
	{
		//gets the frame for the current element (segment)
        const TreeElementType& currentElement = it->second;
        Frame currentFrame = GetTreeElementSegment(currentElement).pose(q_in(GetTreeElementQNr(currentElement)));

		SegmentMap::const_iterator rootIterator = tree.getRootSegment();
		if(it == rootIterator){
			return currentFrame;	
		}
		else{
            SegmentMap::const_iterator parentIt = GetTreeElementParent(currentElement);
			return recursiveFk(q_in, parentIt) * currentFrame;
		}
	}
 double computeMass(const Tree & tree) {
     
     double total_mass = 0.0;
     
     //create necessary vectors
     SegmentMap::const_iterator root;
     
     root = tree.getRootSegment();
     
     SegmentMap sm = tree.getSegments();
        
     for( SegmentMap::const_iterator i=sm.begin(); i!=sm.end(); ++i ) {
         //root has no mass
         if( i != root ) {
            total_mass += GetTreeElementSegment(i->second).getInertia().getMass();
         }
     }
     
     return total_mass;
 }
Exemple #7
0
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();
        return false;
    }
    */
    return true;
}