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