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 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 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; }
void TransformationCalculator::load_robot_model(std::string filepath, bool init_invalid){ clear_all(); bool success=false; success = kdl_parser::treeFromFile(filepath, kdl_tree_); if(!success){ is_initialized_ = false; throw("Could not load Model file"); } //Extract names of all joints defined in urdf file. //These are the names that are expected to be referred to in update function. KDL::SegmentMap map = kdl_tree_.getSegments(); std::string root_name = kdl_tree_.getRootSegment()->first; KDL::Joint joint; KDL::Segment segment; for(KDL::SegmentMap::const_iterator it = map.begin(); it!=map.end(); ++it){ #ifdef KDL_USE_NEW_TREE_INTERFACE segment = it->second->segment; #else segment = it->second.segment; #endif joint = segment.getJoint(); std::string j_name = joint.getName(); if(j_name == "NoName" || j_name == ""){ if(segment.getName() != root_name){ LOG_ERROR("Segment %s has an unnamed joint. This is not okay, but will skip it for now.", segment.getName().c_str()); } LOG_DEBUG("Skipping NonName joint of root segment"); continue; } if(is_fixed(joint)){ if(std::find(static_joint_names_.begin(), static_joint_names_.end(), j_name) == static_joint_names_.end()){ static_segment_names_.push_back(segment.getName()); static_joint_names_.push_back(j_name); joint_name2seg_name_[j_name] = segment.getName(); all_joint_names_.push_back(j_name); } } if(is_valid_joint(joint)){ if(std::find(moving_joint_names_.begin(), moving_joint_names_.end(), j_name) == moving_joint_names_.end()){ moving_joint_names_.push_back(j_name); joint_name2seg_name_[j_name] = segment.getName(); all_joint_names_.push_back(j_name); } } } //link_names_ = extract_keys(map); //Populate transforms map with identity transforms for(std::vector<std::string>::iterator it = moving_joint_names_.begin(); it != moving_joint_names_.end(); ++it) { std::string j_name = *it; std::string seg_name = joint_name2seg_name_[j_name]; moving_joints_transforms_[j_name] = base::samples::RigidBodyState(true); if(!init_invalid){ //initUnknow sets trasform to identity. If we donÄt want invalidating, set to identity. moving_joints_transforms_[j_name].initUnknown(); } moving_joints_transforms_[j_name].sourceFrame = seg_name; #ifdef KDL_USE_NEW_TREE_INTERFACE moving_joints_transforms_[j_name].targetFrame = get_tree_element(seg_name).parent->second->segment.getName(); #else moving_joints_transforms_[j_name].targetFrame = get_tree_element(seg_name).parent->second.segment.getName(); #endif } //Populate static transforms vector for(uint i=0; i<static_joint_names_.size(); i++){ std::string seg_name = static_segment_names_[i]; KDL::TreeElement tree_elem = get_tree_element(seg_name); KDL::Frame kdl_transform = tree_elem.segment.pose(0); std::string j_name = static_joint_names_[i]; base::samples::RigidBodyState rbs; convert(kdl_transform, rbs); //Skip root if(tree_elem.segment.getName() == kdl_tree_.getRootSegment()->first){ continue; } #ifdef KDL_USE_NEW_TREE_INTERFACE rbs.sourceFrame = tree_elem.parent->second->segment.getName(); #else rbs.sourceFrame = tree_elem.parent->second.segment.getName(); #endif rbs.targetFrame = tree_elem.segment.getName(); static_joints_transforms_[j_name] = rbs; } is_initialized_ = true; }
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; }