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