in src/esp/io/URDFParser.cpp [382:567]
bool Parser::parseLink(const std::shared_ptr<Model>& model,
Link& link,
const XMLElement* config) {
const char* linkName = config->Attribute("name");
if (!linkName) {
ESP_VERY_VERBOSE() << "Link with no name";
return false;
}
ESP_VERY_VERBOSE() << "------------------------------------";
ESP_VERY_VERBOSE() << "Parser::parseLink:" << linkName;
link.m_name = linkName;
{
// optional 'contact' parameters
const XMLElement* ci = config->FirstChildElement("contact");
if (ci) {
const XMLElement* damping_xml = ci->FirstChildElement("inertia_scaling");
if (damping_xml) {
if (!damping_xml->Attribute("value")) {
ESP_VERY_VERBOSE()
<< "Link/contact: damping element must have value attribute";
return false;
}
link.m_contactInfo.m_inertiaScaling =
std::stod(damping_xml->Attribute("value"));
link.m_contactInfo.m_flags |= CONTACT_HAS_INERTIA_SCALING;
}
{
const XMLElement* friction_xml =
ci->FirstChildElement("lateral_friction");
if (friction_xml) {
if (!friction_xml->Attribute("value")) {
ESP_VERY_VERBOSE() << "Link/contact: lateral_friction "
"element must have value attribute";
return false;
}
link.m_contactInfo.m_lateralFriction =
std::stod(friction_xml->Attribute("value"));
}
}
{
const XMLElement* rolling_xml =
ci->FirstChildElement("rolling_friction");
if (rolling_xml) {
if (!rolling_xml->Attribute("value")) {
ESP_VERY_VERBOSE() << "Link/contact: rolling friction "
"element must have value attribute";
return false;
}
link.m_contactInfo.m_rollingFriction =
std::stod(rolling_xml->Attribute("value"));
link.m_contactInfo.m_flags |= CONTACT_HAS_ROLLING_FRICTION;
}
}
{
const XMLElement* restitution_xml =
ci->FirstChildElement("restitution");
if (restitution_xml) {
if (!restitution_xml->Attribute("value")) {
ESP_VERY_VERBOSE() << "Link/contact: restitution "
"element must have value attribute";
return false;
}
link.m_contactInfo.m_restitution =
std::stod(restitution_xml->Attribute("value"));
link.m_contactInfo.m_flags |= CONTACT_HAS_RESTITUTION;
}
}
{
const XMLElement* spinning_xml =
ci->FirstChildElement("spinning_friction");
if (spinning_xml) {
if (!spinning_xml->Attribute("value")) {
ESP_VERY_VERBOSE() << "Link/contact: spinning friction "
"element must have value attribute";
return false;
}
link.m_contactInfo.m_spinningFriction =
std::stod(spinning_xml->Attribute("value"));
link.m_contactInfo.m_flags |= CONTACT_HAS_SPINNING_FRICTION;
}
}
{
const XMLElement* friction_anchor =
ci->FirstChildElement("friction_anchor");
if (friction_anchor) {
link.m_contactInfo.m_flags |= CONTACT_HAS_FRICTION_ANCHOR;
}
}
{
const XMLElement* stiffness_xml = ci->FirstChildElement("stiffness");
if (stiffness_xml) {
if (!stiffness_xml->Attribute("value")) {
ESP_VERY_VERBOSE() << "Link/contact: stiffness element "
"must have value attribute";
return false;
}
link.m_contactInfo.m_contactStiffness =
std::stod(stiffness_xml->Attribute("value"));
link.m_contactInfo.m_flags |= CONTACT_HAS_STIFFNESS_DAMPING;
}
}
{
const XMLElement* damping_xml = ci->FirstChildElement("damping");
if (damping_xml) {
if (!damping_xml->Attribute("value")) {
ESP_VERY_VERBOSE() << "Link/contact: damping element "
"must have value attribute";
return false;
}
link.m_contactInfo.m_contactDamping =
std::stod(damping_xml->Attribute("value"));
link.m_contactInfo.m_flags |= CONTACT_HAS_STIFFNESS_DAMPING;
}
}
}
}
// Inertial (optional)
const XMLElement* i = config->FirstChildElement("inertial");
if (i) {
if (!parseInertia(link.m_inertia, i)) {
ESP_VERY_VERBOSE() << "Could not parse inertial element for Link:";
ESP_VERY_VERBOSE() << link.m_name;
return false;
}
link.m_inertia.m_mass *= model->getGlobalScaling();
} else {
if ((strlen(linkName) == 5) && (strncmp(linkName, "world", 5)) == 0) {
link.m_inertia.m_mass = 0.f;
link.m_inertia.m_linkLocalFrame = Mn::Matrix4(); // Identity
link.m_inertia.m_ixx = 0.f;
link.m_inertia.m_iyy = 0.f;
link.m_inertia.m_izz = 0.f;
} else {
ESP_VERY_VERBOSE()
<< "W - No inertial data for link:" << link.m_name
<< ", using mass=1, localinertiadiagonal = 1,1,1, identity local "
"inertial frame";
link.m_inertia.m_mass = 1.f * model->getMassScaling();
link.m_inertia.m_linkLocalFrame = Mn::Matrix4(); // Identity
link.m_inertia.m_ixx = 1.f;
link.m_inertia.m_iyy = 1.f;
link.m_inertia.m_izz = 1.f;
}
}
// Multiple Visuals (optional)
for (const XMLElement* vis_xml = config->FirstChildElement("visual"); vis_xml;
vis_xml = vis_xml->NextSiblingElement("visual")) {
VisualShape visual;
if (parseVisual(model, visual, vis_xml)) {
link.m_visualArray.push_back(visual);
} else {
ESP_VERY_VERBOSE() << "Could not parse visual element for Link:"
<< link.m_name;
return false;
}
}
// Multiple Collisions (optional)
for (const XMLElement* col_xml = config->FirstChildElement("collision");
col_xml; col_xml = col_xml->NextSiblingElement("collision")) {
CollisionShape col;
if (parseCollision(col, col_xml)) {
link.m_collisionArray.push_back(col);
} else {
ESP_VERY_VERBOSE() << "Could not parse collision element for Link:"
<< link.m_name.c_str();
return false;
}
}
return true;
}