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