void srdf::Model::loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{
for (TiXmlElement* c_xml = robot_xml->FirstChildElement("disable_collisions"); c_xml; c_xml = c_xml->NextSiblingElement("disable_collisions"))
{
const char *link1 = c_xml->Attribute("link1");
const char *link2 = c_xml->Attribute("link2");
if (!link1 || !link2)
{
logError("A pair of links needs to be specified to disable collisions");
continue;
}
DisabledCollision dc;
dc.link1_ = boost::trim_copy(std::string(link1));
dc.link2_ = boost::trim_copy(std::string(link2));
if (!urdf_model.getLink(dc.link1_))
{
logError("Link '%s' is not known to URDF. Cannot disable collisons.", link1);
continue;
}
if (!urdf_model.getLink(dc.link2_))
{
logError("Link '%s' is not known to URDF. Cannot disable collisons.", link2);
continue;
}
const char *reason = c_xml->Attribute("reason");
if (reason)
dc.reason_ = std::string(reason);
disabled_collisions_.push_back(dc);
}
}
void srdf::Model::loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{
for (TiXmlElement* eef_xml = robot_xml->FirstChildElement("end_effector"); eef_xml; eef_xml = eef_xml->NextSiblingElement("end_effector"))
{
const char *ename = eef_xml->Attribute("name");
const char *gname = eef_xml->Attribute("group");
const char *parent = eef_xml->Attribute("parent_link");
const char *parent_group = eef_xml->Attribute("parent_group");
if (!ename)
{
logError("Name of end effector is not specified");
continue;
}
if (!gname)
{
logError("Group not specified for end effector '%s'", ename);
continue;
}
EndEffector e;
e.name_ = std::string(ename); boost::trim(e.name_);
e.component_group_ = std::string(gname); boost::trim(e.component_group_);
bool found = false;
for (std::size_t k = 0 ; k < groups_.size() ; ++k)
if (groups_[k].name_ == e.component_group_)
{
found = true;
break;
}
if (!found)
{
logError("End effector '%s' specified for group '%s', but that group is not known", ename, gname);
continue;
}
if (!parent)
{
logError("Parent link not specified for end effector '%s'", ename);
continue;
}
e.parent_link_ = std::string(parent); boost::trim(e.parent_link_);
if (!urdf_model.getLink(e.parent_link_))
{
logError("Link '%s' specified as parent for end effector '%s' is not known to the URDF", parent, ename);
continue;
}
if (parent_group)
{
e.parent_group_ = std::string(parent_group); boost::trim(e.parent_group_);
}
end_effectors_.push_back(e);
}
}
void srdf::Model::loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{
for (TiXmlElement* vj_xml = robot_xml->FirstChildElement("virtual_joint"); vj_xml; vj_xml = vj_xml->NextSiblingElement("virtual_joint"))
{
const char *jname = vj_xml->Attribute("name");
const char *child = vj_xml->Attribute("child_link");
const char *parent = vj_xml->Attribute("parent_frame");
const char *type = vj_xml->Attribute("type");
if (!jname)
{
logError("Name of virtual joint is not specified");
continue;
}
if (!child)
{
logError("Child link of virtual joint is not specified");
continue;
}
if (!urdf_model.getLink(boost::trim_copy(std::string(child))))
{
logError("Virtual joint does not attach to a link on the robot (link '%s' is not known)", child);
continue;
}
if (!parent)
{
logError("Parent frame of virtual joint is not specified");
continue;
}
if (!type)
{
logError("Type of virtual joint is not specified");
continue;
}
VirtualJoint vj;
vj.type_ = std::string(type); boost::trim(vj.type_);
std::transform(vj.type_.begin(), vj.type_.end(), vj.type_.begin(), ::tolower);
if (vj.type_ != "planar" && vj.type_ != "floating" && vj.type_ != "fixed")
{
logError("Unknown type of joint: '%s'. Assuming 'fixed' instead. Other known types are 'planar' and 'floating'.", type);
vj.type_ = "fixed";
}
vj.name_ = std::string(jname); boost::trim(vj.name_);
vj.child_link_ = std::string(child); boost::trim(vj.child_link_);
vj.parent_frame_ = std::string(parent); boost::trim(vj.parent_frame_);
virtual_joints_.push_back(vj);
}
}
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 srdf::Model::loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{
for (TiXmlElement* group_xml = robot_xml->FirstChildElement("group"); group_xml; group_xml = group_xml->NextSiblingElement("group"))
{
const char *gname = group_xml->Attribute("name");
if (!gname)
{
logError("Group name not specified");
continue;
}
Group g;
g.name_ = std::string(gname); boost::trim(g.name_);
// get the links in the groups
for (TiXmlElement* link_xml = group_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
{
const char *lname = link_xml->Attribute("name");
if (!lname)
{
logError("Link name not specified");
continue;
}
std::string lname_str = boost::trim_copy(std::string(lname));
if (!urdf_model.getLink(lname_str))
{
logError("Link '%s' declared as part of group '%s' is not known to the URDF", lname, gname);
continue;
}
g.links_.push_back(lname_str);
}
// get the joints in the groups
for (TiXmlElement* joint_xml = group_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
{
const char *jname = joint_xml->Attribute("name");
if (!jname)
{
logError("Joint name not specified");
continue;
}
std::string jname_str = boost::trim_copy(std::string(jname));
if (!urdf_model.getJoint(jname_str))
{
bool missing = true;
for (std::size_t k = 0 ; k < virtual_joints_.size() ; ++k)
if (virtual_joints_[k].name_ == jname_str)
{
missing = false;
break;
}
if (missing)
{
logError("Joint '%s' declared as part of group '%s' is not known to the URDF", jname, gname);
continue;
}
}
g.joints_.push_back(jname_str);
}
// get the chains in the groups
for (TiXmlElement* chain_xml = group_xml->FirstChildElement("chain"); chain_xml; chain_xml = chain_xml->NextSiblingElement("chain"))
{
const char *base = chain_xml->Attribute("base_link");
const char *tip = chain_xml->Attribute("tip_link");
if (!base)
{
logError("Base link name not specified for chain");
continue;
}
if (!tip)
{
logError("Tip link name not specified for chain");
continue;
}
std::string base_str = boost::trim_copy(std::string(base));
std::string tip_str = boost::trim_copy(std::string(tip));
if (!urdf_model.getLink(base_str))
{
logError("Link '%s' declared as part of a chain in group '%s' is not known to the URDF", base, gname);
continue;
}
if (!urdf_model.getLink(tip_str))
{
logError("Link '%s' declared as part of a chain in group '%s' is not known to the URDF", tip, gname);
continue;
}
bool found = false;
boost::shared_ptr<const urdf::Link> l = urdf_model.getLink(tip_str);
std::set<std::string> seen;
while (!found && l)
{
seen.insert(l->name);
if (l->name == base_str)
found = true;
else
l = l->getParent();
}
if (!found)
{
l = urdf_model.getLink(base_str);
while (!found && l)
{
if (seen.find(l->name) != seen.end())
found = true;
else
l = l->getParent();
}
}
if (found)
g.chains_.push_back(std::make_pair(base_str, tip_str));
else
logError("Links '%s' and '%s' do not form a chain. Not included in group '%s'", base, tip, gname);
}
// get the subgroups in the groups
for (TiXmlElement* subg_xml = group_xml->FirstChildElement("group"); subg_xml; subg_xml = subg_xml->NextSiblingElement("group"))
{
const char *sub = subg_xml->Attribute("name");
if (!sub)
{
logError("Group name not specified when included as subgroup");
continue;
}
g.subgroups_.push_back(boost::trim_copy(std::string(sub)));
}
if (g.links_.empty() && g.joints_.empty() && g.chains_.empty() && g.subgroups_.empty())
logWarn("Group '%s' is empty.", gname);
groups_.push_back(g);
}
// check the subgroups
std::set<std::string> known_groups;
bool update = true;
while (update)
{
update = false;
for (std::size_t i = 0 ; i < groups_.size() ; ++i)
{
if (known_groups.find(groups_[i].name_) != known_groups.end())
continue;
if (groups_[i].subgroups_.empty())
{
known_groups.insert(groups_[i].name_);
update = true;
}
else
{
bool ok = true;
for (std::size_t j = 0 ; ok && j < groups_[i].subgroups_.size() ; ++j)
if (known_groups.find(groups_[i].subgroups_[j]) == known_groups.end())
ok = false;
if (ok)
{
known_groups.insert(groups_[i].name_);
update = true;
}
}
}
}
// if there are erroneous groups, keep only the valid ones
if (known_groups.size() != groups_.size())
{
std::vector<Group> correct;
for (std::size_t i = 0 ; i < groups_.size() ; ++i)
if (known_groups.find(groups_[i].name_) != known_groups.end())
correct.push_back(groups_[i]);
else
logError("Group '%s' has unsatisfied subgroups", groups_[i].name_.c_str());
groups_.swap(correct);
}
}
void srdf::Model::loadLinkSphereApproximations(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{
for (TiXmlElement* cslink_xml = robot_xml->FirstChildElement("link_sphere_approximation"); cslink_xml; cslink_xml = cslink_xml->NextSiblingElement("link_sphere_approximation"))
{
int non_0_radius_sphere_cnt = 0;
const char *link_name = cslink_xml->Attribute("link");
if (!link_name)
{
logError("Name of link is not specified in link_collision_spheres");
continue;
}
LinkSpheres link_spheres;
link_spheres.link_ = boost::trim_copy(std::string(link_name));
if (!urdf_model.getLink(link_spheres.link_))
{
logError("Link '%s' is not known to URDF.", link_name);
continue;
}
// get the spheres for this link
int cnt = 0;
for (TiXmlElement* sphere_xml = cslink_xml->FirstChildElement("sphere"); sphere_xml; sphere_xml = sphere_xml->NextSiblingElement("sphere"), cnt++)
{
const char *s_center = sphere_xml->Attribute("center");
const char *s_r = sphere_xml->Attribute("radius");
if (!s_center || !s_r)
{
logError("Link collision sphere %d for link '%s' does not have both center and radius.", cnt, link_name);
continue;
}
Sphere sphere;
try
{
std::stringstream center(s_center);
center.exceptions(std::stringstream::failbit | std::stringstream::badbit);
center >> sphere.center_x_ >> sphere.center_y_ >> sphere.center_z_;
sphere.radius_ = boost::lexical_cast<double>(s_r);
}
catch (std::stringstream::failure &e)
{
logError("Link collision sphere %d for link '%s' has bad center attribute value.", cnt, link_name);
continue;
}
catch (boost::bad_lexical_cast &e)
{
logError("Link collision sphere %d for link '%s' has bad radius attribute value.", cnt, link_name);
continue;
}
// ignore radius==0 spheres unless there is only 1 of them
//
// NOTE:
// - If a link has no sphere_approximation then one will be generated
// automatically containing a single sphere which encloses the entire
// collision geometry. Internally this is represented by not having
// a link_sphere_approximations_ entry for this link.
// - If a link has only spheres with radius 0 then it will not be
// considered for collision detection. In this case the internal
// representation is a single radius=0 sphere.
// - If a link has at least one sphere with radius>0 then those spheres
// are used. Any radius=0 spheres are eliminated.
if (sphere.radius_ > std::numeric_limits<double>::epsilon())
{
if (non_0_radius_sphere_cnt == 0)
link_spheres.spheres_.clear();
link_spheres.spheres_.push_back(sphere);
non_0_radius_sphere_cnt++;
}
else if (non_0_radius_sphere_cnt == 0)
{
sphere.center_x_ = 0.0;
sphere.center_y_ = 0.0;
sphere.center_z_ = 0.0;
sphere.radius_ = 0.0;
link_spheres.spheres_.clear();
link_spheres.spheres_.push_back(sphere);
}
}
if (!link_spheres.spheres_.empty())
link_sphere_approximations_.push_back(link_spheres);
}
}