bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger)
{
const char* linkName = config->Attribute("name");
if (!linkName)
{
logger->reportError("Link with no name");
return false;
}
link.m_name = linkName;
if (m_parseSDF) {
TiXmlElement* pose = config->FirstChildElement("pose");
if (0==pose)
{
link.m_linkTransformInWorld.setIdentity();
}
else
{
parseTransform(link.m_linkTransformInWorld, pose,logger,m_parseSDF);
}
}
// Inertial (optional)
TiXmlElement *i = config->FirstChildElement("inertial");
if (i)
{
if (!parseInertia(link.m_inertia, i,logger))
{
logger->reportError("Could not parse inertial element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
} else
{
if ((strlen(linkName)==5) && (strncmp(linkName, "world", 5))==0)
{
link.m_inertia.m_mass = 0.f;
link.m_inertia.m_linkLocalFrame.setIdentity();
link.m_inertia.m_ixx = 0.f;
link.m_inertia.m_iyy = 0.f;
link.m_inertia.m_izz= 0.f;
} else
{
logger->reportWarning("No inertial data for link, using mass=1, localinertiadiagonal = 1,1,1, identity local inertial frame");
link.m_inertia.m_mass = 1.f;
link.m_inertia.m_linkLocalFrame.setIdentity();
link.m_inertia.m_ixx = 1.f;
link.m_inertia.m_iyy = 1.f;
link.m_inertia.m_izz= 1.f;
logger->reportWarning(link.m_name.c_str());
}
}
// Multiple Visuals (optional)
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
{
UrdfVisual visual;
if (parseVisual(model, visual, vis_xml,logger))
{
link.m_visualArray.push_back(visual);
}
else
{
logger->reportError("Could not parse visual element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
}
// Multiple Collisions (optional)
for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
{
UrdfCollision col;
if (parseCollision(col, col_xml,logger))
{
link.m_collisionArray.push_back(col);
}
else
{
logger->reportError("Could not parse collision element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
}
return true;
}
bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger)
{
const char* linkName = config->Attribute("name");
if (!linkName)
{
logger->reportError("Link with no name");
return false;
}
link.m_name = linkName;
if (m_parseSDF) {
TiXmlElement* pose = config->FirstChildElement("pose");
if (0==pose)
{
link.m_linkTransformInWorld.setIdentity();
}
else
{
parseTransform(link.m_linkTransformInWorld, pose,logger,m_parseSDF);
}
}
{
//optional 'contact' parameters
TiXmlElement* ci = config->FirstChildElement("contact");
if (ci)
{
TiXmlElement *damping_xml = ci->FirstChildElement("inertia_scaling");
if (damping_xml)
{
if (m_parseSDF)
{
link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->GetText());
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;
} else
{
if (!damping_xml->Attribute("value"))
{
logger->reportError("Link/contact: damping element must have value attribute");
return false;
}
link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->Attribute("value"));
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;
}
}
{
TiXmlElement *friction_xml = ci->FirstChildElement("lateral_friction");
if (friction_xml)
{
if (m_parseSDF)
{
link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->GetText());
} else
{
if (!friction_xml->Attribute("value"))
{
logger->reportError("Link/contact: lateral_friction element must have value attribute");
return false;
}
link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
}
}
}
}
}
// Inertial (optional)
TiXmlElement *i = config->FirstChildElement("inertial");
if (i)
{
if (!parseInertia(link.m_inertia, i,logger))
{
logger->reportError("Could not parse inertial element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
} else
{
if ((strlen(linkName)==5) && (strncmp(linkName, "world", 5))==0)
{
link.m_inertia.m_mass = 0.f;
link.m_inertia.m_linkLocalFrame.setIdentity();
link.m_inertia.m_ixx = 0.f;
link.m_inertia.m_iyy = 0.f;
link.m_inertia.m_izz= 0.f;
} else
{
logger->reportWarning("No inertial data for link, using mass=1, localinertiadiagonal = 1,1,1, identity local inertial frame");
link.m_inertia.m_mass = 1.f;
link.m_inertia.m_linkLocalFrame.setIdentity();
link.m_inertia.m_ixx = 1.f;
link.m_inertia.m_iyy = 1.f;
link.m_inertia.m_izz= 1.f;
logger->reportWarning(link.m_name.c_str());
}
}
// Multiple Visuals (optional)
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
{
UrdfVisual visual;
if (parseVisual(model, visual, vis_xml,logger))
{
link.m_visualArray.push_back(visual);
}
else
{
logger->reportError("Could not parse visual element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
}
// Multiple Collisions (optional)
for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
{
UrdfCollision col;
if (parseCollision(col, col_xml,logger))
{
link.m_collisionArray.push_back(col);
}
else
{
logger->reportError("Could not parse collision element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
}
return true;
}