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; }
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 parseLink(Link &link, TiXmlElement* config) { link.clear(); const char *name_char = config->Attribute("name"); if (!name_char) { logError("No name given for the link."); return false; } link.name = std::string(name_char); // Inertial (optional) TiXmlElement *i = config->FirstChildElement("inertial"); if (i) { resetPtr(link.inertial,new Inertial()); if (!parseInertial(*link.inertial, i)) { logError("Could not parse inertial element for Link [%s]", link.name.c_str()); return false; } } // Multiple Visuals (optional) for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual")) { VisualPtr vis; resetPtr(vis,new Visual()); if (parseVisual(*vis, vis_xml)) { link.visual_array.push_back(vis); } else { resetPtr(vis); logError("Could not parse visual element for Link [%s]", link.name.c_str()); return false; } } // Visual (optional) // Assign the first visual to the .visual ptr, if it exists if (!link.visual_array.empty()) link.visual = link.visual_array[0]; // Multiple Collisions (optional) for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision")) { CollisionPtr col; resetPtr(col,new Collision()); if (parseCollision(*col, col_xml)) { link.collision_array.push_back(col); } else { resetPtr(col); logError("Could not parse collision element for Link [%s]", link.name.c_str()); return false; } } // Collision (optional) // Assign the first collision to the .collision ptr, if it exists if (!link.collision_array.empty()) link.collision = link.collision_array[0]; }