bool parseVisual(Visual &vis, TiXmlElement *config) { vis.clear(); // Origin TiXmlElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(vis.origin, o)) return false; } // Geometry TiXmlElement *geom = config->FirstChildElement("geometry"); vis.geometry = parseGeometry(geom); if (!vis.geometry) return false; const char *name_char = config->Attribute("name"); if (name_char) vis.name = name_char; // Material TiXmlElement *mat = config->FirstChildElement("material"); if (mat) { // get material name if (!mat->Attribute("name")) { CONSOLE_BRIDGE_logError("Visual material must contain a name attribute"); return false; } vis.material_name = mat->Attribute("name"); // try to parse material element in place vis.material.reset(new Material()); if (!parseMaterial(*vis.material, mat, true)) { CONSOLE_BRIDGE_logDebug("urdfdom: material has only name, actual material definition may be in the model"); } } return true; }
bool parseModelState(ModelState &ms, TiXmlElement* config) { ms.clear(); const char *name_char = config->Attribute("name"); if (!name_char) { CONSOLE_BRIDGE_logError("No name given for the model_state."); return false; } ms.name = std::string(name_char); const char *time_stamp_char = config->Attribute("time_stamp"); if (time_stamp_char) { try { double sec = boost::lexical_cast<double>(time_stamp_char); ms.time_stamp.set(sec); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("Parsing time stamp [%s] failed: %s", time_stamp_char, e.what()); return false; } } TiXmlElement *joint_state_elem = config->FirstChildElement("joint_state"); if (joint_state_elem) { JointStateSharedPtr joint_state; joint_state.reset(new JointState()); const char *joint_char = joint_state_elem->Attribute("joint"); if (joint_char) joint_state->joint = std::string(joint_char); else { CONSOLE_BRIDGE_logError("No joint name given for the model_state."); return false; } // parse position const char *position_char = joint_state_elem->Attribute("position"); if (position_char) { std::vector<std::string> pieces; boost::split( pieces, position_char, boost::is_any_of(" ")); for (unsigned int i = 0; i < pieces.size(); ++i){ if (pieces[i] != ""){ try { joint_state->position.push_back(boost::lexical_cast<double>(pieces[i].c_str())); } catch (boost::bad_lexical_cast &/*e*/) { throw ParseError("position element ("+ pieces[i] +") is not a valid float"); } } } } // parse velocity const char *velocity_char = joint_state_elem->Attribute("velocity"); if (velocity_char) { std::vector<std::string> pieces; boost::split( pieces, velocity_char, boost::is_any_of(" ")); for (unsigned int i = 0; i < pieces.size(); ++i){ if (pieces[i] != ""){ try { joint_state->velocity.push_back(boost::lexical_cast<double>(pieces[i].c_str())); } catch (boost::bad_lexical_cast &/*e*/) { throw ParseError("velocity element ("+ pieces[i] +") is not a valid float"); } } } } // parse effort const char *effort_char = joint_state_elem->Attribute("effort"); if (effort_char) { std::vector<std::string> pieces; boost::split( pieces, effort_char, boost::is_any_of(" ")); for (unsigned int i = 0; i < pieces.size(); ++i){ if (pieces[i] != ""){ try { joint_state->effort.push_back(boost::lexical_cast<double>(pieces[i].c_str())); } catch (boost::bad_lexical_cast &/*e*/) { throw ParseError("effort element ("+ pieces[i] +") is not a valid float"); } } } } // add to vector ms.joint_states.push_back(joint_state); } return false; }
bool parseJoint(Joint &joint, TiXmlElement* config) { joint.clear(); // Get Joint Name const char *name = config->Attribute("name"); if (!name) { CONSOLE_BRIDGE_logError("unnamed joint found"); return false; } joint.name = name; // Get transform from Parent Link to Joint Frame TiXmlElement *origin_xml = config->FirstChildElement("origin"); if (!origin_xml) { CONSOLE_BRIDGE_logDebug("urdfdom: Joint [%s] missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", joint.name.c_str()); joint.parent_to_joint_origin_transform.clear(); } else { if (!parsePose(joint.parent_to_joint_origin_transform, origin_xml)) { joint.parent_to_joint_origin_transform.clear(); CONSOLE_BRIDGE_logError("Malformed parent origin element for joint [%s]", joint.name.c_str()); return false; } } // Get Parent Link TiXmlElement *parent_xml = config->FirstChildElement("parent"); if (parent_xml) { const char *pname = parent_xml->Attribute("link"); if (!pname) { CONSOLE_BRIDGE_logInform("no parent link name specified for Joint link [%s]. this might be the root?", joint.name.c_str()); } else { joint.parent_link_name = std::string(pname); } } // Get Child Link TiXmlElement *child_xml = config->FirstChildElement("child"); if (child_xml) { const char *pname = child_xml->Attribute("link"); if (!pname) { CONSOLE_BRIDGE_logInform("no child link name specified for Joint link [%s].", joint.name.c_str()); } else { joint.child_link_name = std::string(pname); } } // Get Joint type const char* type_char = config->Attribute("type"); if (!type_char) { CONSOLE_BRIDGE_logError("joint [%s] has no type, check to see if it's a reference.", joint.name.c_str()); return false; } std::string type_str = type_char; if (type_str == "planar") joint.type = Joint::PLANAR; else if (type_str == "floating") joint.type = Joint::FLOATING; else if (type_str == "revolute") joint.type = Joint::REVOLUTE; else if (type_str == "continuous") joint.type = Joint::CONTINUOUS; else if (type_str == "prismatic") joint.type = Joint::PRISMATIC; else if (type_str == "fixed") joint.type = Joint::FIXED; else { CONSOLE_BRIDGE_logError("Joint [%s] has no known type [%s]", joint.name.c_str(), type_str.c_str()); return false; } // Get Joint Axis if (joint.type != Joint::FLOATING && joint.type != Joint::FIXED) { // axis TiXmlElement *axis_xml = config->FirstChildElement("axis"); if (!axis_xml){ CONSOLE_BRIDGE_logDebug("urdfdom: no axis elemement for Joint link [%s], defaulting to (1,0,0) axis", joint.name.c_str()); joint.axis = Vector3(1.0, 0.0, 0.0); } else{ if (axis_xml->Attribute("xyz")){ try { joint.axis.init(axis_xml->Attribute("xyz")); } catch (ParseError &e) { joint.axis.clear(); CONSOLE_BRIDGE_logError("Malformed axis element for joint [%s]: %s", joint.name.c_str(), e.what()); return false; } } } } // Get limit TiXmlElement *limit_xml = config->FirstChildElement("limit"); if (limit_xml) { joint.limits.reset(new JointLimits()); if (!parseJointLimits(*joint.limits, limit_xml)) { CONSOLE_BRIDGE_logError("Could not parse limit element for joint [%s]", joint.name.c_str()); joint.limits.reset(); return false; } } else if (joint.type == Joint::REVOLUTE) { CONSOLE_BRIDGE_logError("Joint [%s] is of type REVOLUTE but it does not specify limits", joint.name.c_str()); return false; } else if (joint.type == Joint::PRISMATIC) { CONSOLE_BRIDGE_logError("Joint [%s] is of type PRISMATIC without limits", joint.name.c_str()); return false; } // Get safety TiXmlElement *safety_xml = config->FirstChildElement("safety_controller"); if (safety_xml) { joint.safety.reset(new JointSafety()); if (!parseJointSafety(*joint.safety, safety_xml)) { CONSOLE_BRIDGE_logError("Could not parse safety element for joint [%s]", joint.name.c_str()); joint.safety.reset(); return false; } } // Get calibration TiXmlElement *calibration_xml = config->FirstChildElement("calibration"); if (calibration_xml) { joint.calibration.reset(new JointCalibration()); if (!parseJointCalibration(*joint.calibration, calibration_xml)) { CONSOLE_BRIDGE_logError("Could not parse calibration element for joint [%s]", joint.name.c_str()); joint.calibration.reset(); return false; } } // Get Joint Mimic TiXmlElement *mimic_xml = config->FirstChildElement("mimic"); if (mimic_xml) { joint.mimic.reset(new JointMimic()); if (!parseJointMimic(*joint.mimic, mimic_xml)) { CONSOLE_BRIDGE_logError("Could not parse mimic element for joint [%s]", joint.name.c_str()); joint.mimic.reset(); return false; } } // Get Dynamics TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); if (prop_xml) { joint.dynamics.reset(new JointDynamics()); if (!parseJointDynamics(*joint.dynamics, prop_xml)) { CONSOLE_BRIDGE_logError("Could not parse joint_dynamics element for joint [%s]", joint.name.c_str()); joint.dynamics.reset(); return false; } } return true; }
bool parseJointSafety(JointSafety &js, TiXmlElement* config) { js.clear(); // Get soft_lower_limit joint limit const char* soft_lower_limit_str = config->Attribute("soft_lower_limit"); if (soft_lower_limit_str == NULL) { CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no soft_lower_limit, using default value"); js.soft_lower_limit = 0; } else { try { js.soft_lower_limit = boost::lexical_cast<double>(soft_lower_limit_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("soft_lower_limit value (%s) is not a float: %s",soft_lower_limit_str, e.what()); return false; } } // Get soft_upper_limit joint limit const char* soft_upper_limit_str = config->Attribute("soft_upper_limit"); if (soft_upper_limit_str == NULL) { CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no soft_upper_limit, using default value"); js.soft_upper_limit = 0; } else { try { js.soft_upper_limit = boost::lexical_cast<double>(soft_upper_limit_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("soft_upper_limit value (%s) is not a float: %s",soft_upper_limit_str, e.what()); return false; } } // Get k_position_ safety "position" gain - not exactly position gain const char* k_position_str = config->Attribute("k_position"); if (k_position_str == NULL) { CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no k_position, using default value"); js.k_position = 0; } else { try { js.k_position = boost::lexical_cast<double>(k_position_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("k_position value (%s) is not a float: %s",k_position_str, e.what()); return false; } } // Get k_velocity_ safety velocity gain const char* k_velocity_str = config->Attribute("k_velocity"); if (k_velocity_str == NULL) { CONSOLE_BRIDGE_logError("joint safety: no k_velocity"); return false; } else { try { js.k_velocity = boost::lexical_cast<double>(k_velocity_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("k_velocity value (%s) is not a float: %s",k_velocity_str, e.what()); return false; } } return true; }
bool parseJointLimits(JointLimits &jl, TiXmlElement* config) { jl.clear(); // Get lower joint limit const char* lower_str = config->Attribute("lower"); if (lower_str == NULL){ CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: no lower, defaults to 0"); jl.lower = 0; } else { try { jl.lower = boost::lexical_cast<double>(lower_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("lower value (%s) is not a float: %s", lower_str, e.what()); return false; } } // Get upper joint limit const char* upper_str = config->Attribute("upper"); if (upper_str == NULL){ CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: no upper, , defaults to 0"); jl.upper = 0; } else { try { jl.upper = boost::lexical_cast<double>(upper_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("upper value (%s) is not a float: %s",upper_str, e.what()); return false; } } // Get joint effort limit const char* effort_str = config->Attribute("effort"); if (effort_str == NULL){ CONSOLE_BRIDGE_logError("joint limit: no effort"); return false; } else { try { jl.effort = boost::lexical_cast<double>(effort_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("effort value (%s) is not a float: %s",effort_str, e.what()); return false; } } // Get joint velocity limit const char* velocity_str = config->Attribute("velocity"); if (velocity_str == NULL){ CONSOLE_BRIDGE_logError("joint limit: no velocity"); return false; } else { try { jl.velocity = boost::lexical_cast<double>(velocity_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("velocity value (%s) is not a float: %s",velocity_str, e.what()); return false; } } return true; }
ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) { ModelInterfaceSharedPtr model(new ModelInterface); model->clear(); TiXmlDocument xml_doc; xml_doc.Parse(xml_string.c_str()); if (xml_doc.Error()) { CONSOLE_BRIDGE_logError(xml_doc.ErrorDesc()); xml_doc.ClearError(); model.reset(); return model; } TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot"); if (!robot_xml) { CONSOLE_BRIDGE_logError("Could not find the 'robot' element in the xml file"); model.reset(); return model; } // Get robot name const char *name = robot_xml->Attribute("name"); if (!name) { CONSOLE_BRIDGE_logError("No name given for the robot."); model.reset(); return model; } model->name_ = std::string(name); // Get all Material elements for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) { MaterialSharedPtr material; material.reset(new Material); try { parseMaterial(*material, material_xml, false); // material needs to be fully defined here if (model->getMaterial(material->name)) { CONSOLE_BRIDGE_logError("material '%s' is not unique.", material->name.c_str()); material.reset(); model.reset(); return model; } else { model->materials_.insert(make_pair(material->name,material)); CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new material '%s'", material->name.c_str()); } } catch (ParseError &/*e*/) { CONSOLE_BRIDGE_logError("material xml is not initialized correctly"); material.reset(); model.reset(); return model; } } // Get all Link elements for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) { LinkSharedPtr link; link.reset(new Link); try { parseLink(*link, link_xml); if (model->getLink(link->name)) { CONSOLE_BRIDGE_logError("link '%s' is not unique.", link->name.c_str()); model.reset(); return model; } else { // set link visual(s) material CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material", link->name.c_str()); if (link->visual) { assignMaterial(link->visual, model, link->name.c_str()); } for (const auto& visual : link->visual_array) { assignMaterial(visual, model, link->name.c_str()); } model->links_.insert(make_pair(link->name,link)); CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new link '%s'", link->name.c_str()); } } catch (ParseError &/*e*/) { CONSOLE_BRIDGE_logError("link xml is not initialized correctly"); model.reset(); return model; } } if (model->links_.empty()){ CONSOLE_BRIDGE_logError("No link elements found in urdf file"); model.reset(); return model; } // Get all Joint elements for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) { JointSharedPtr joint; joint.reset(new Joint); if (parseJoint(*joint, joint_xml)) { if (model->getJoint(joint->name)) { CONSOLE_BRIDGE_logError("joint '%s' is not unique.", joint->name.c_str()); model.reset(); return model; } else { model->joints_.insert(make_pair(joint->name,joint)); CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new joint '%s'", joint->name.c_str()); } } else { CONSOLE_BRIDGE_logError("joint xml is not initialized correctly"); model.reset(); return model; } } // 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 { model->initTree(parent_link_tree); } catch(ParseError &e) { CONSOLE_BRIDGE_logError("Failed to build tree: %s", e.what()); model.reset(); return model; } // find the root link try { model->initRoot(parent_link_tree); } catch(ParseError &e) { CONSOLE_BRIDGE_logError("Failed to find root link: %s", e.what()); model.reset(); return model; } return model; }
bool parseLink(Link &link, TiXmlElement* config) { link.clear(); const char *name_char = config->Attribute("name"); if (!name_char) { CONSOLE_BRIDGE_logError("No name given for the link."); return false; } link.name = std::string(name_char); // Inertial (optional) TiXmlElement *i = config->FirstChildElement("inertial"); if (i) { link.inertial.reset(new Inertial()); if (!parseInertial(*link.inertial, i)) { CONSOLE_BRIDGE_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")) { VisualSharedPtr vis; vis.reset(new Visual()); if (parseVisual(*vis, vis_xml)) { link.visual_array.push_back(vis); } else { vis.reset(); CONSOLE_BRIDGE_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")) { CollisionSharedPtr col; col.reset(new Collision()); if (parseCollision(*col, col_xml)) { link.collision_array.push_back(col); } else { col.reset(); CONSOLE_BRIDGE_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]; return true; }
bool parseInertial(Inertial &i, TiXmlElement *config) { i.clear(); // Origin TiXmlElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(i.origin, o)) return false; } TiXmlElement *mass_xml = config->FirstChildElement("mass"); if (!mass_xml) { CONSOLE_BRIDGE_logError("Inertial element must have a mass element"); return false; } if (!mass_xml->Attribute("value")) { CONSOLE_BRIDGE_logError("Inertial: mass element must have value attribute"); return false; } try { i.mass = boost::lexical_cast<double>(mass_xml->Attribute("value")); } catch (boost::bad_lexical_cast &/*e*/) { std::stringstream stm; stm << "Inertial: mass [" << mass_xml->Attribute("value") << "] is not a float"; CONSOLE_BRIDGE_logError(stm.str().c_str()); return false; } TiXmlElement *inertia_xml = config->FirstChildElement("inertia"); if (!inertia_xml) { CONSOLE_BRIDGE_logError("Inertial element must have inertia element"); return false; } if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") && inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") && inertia_xml->Attribute("izz"))) { CONSOLE_BRIDGE_logError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes"); return false; } try { i.ixx = boost::lexical_cast<double>(inertia_xml->Attribute("ixx")); i.ixy = boost::lexical_cast<double>(inertia_xml->Attribute("ixy")); i.ixz = boost::lexical_cast<double>(inertia_xml->Attribute("ixz")); i.iyy = boost::lexical_cast<double>(inertia_xml->Attribute("iyy")); i.iyz = boost::lexical_cast<double>(inertia_xml->Attribute("iyz")); i.izz = boost::lexical_cast<double>(inertia_xml->Attribute("izz")); } catch (boost::bad_lexical_cast &/*e*/) { std::stringstream stm; stm << "Inertial: one of the inertia elements is not a valid double:" << " ixx [" << inertia_xml->Attribute("ixx") << "]" << " ixy [" << inertia_xml->Attribute("ixy") << "]" << " ixz [" << inertia_xml->Attribute("ixz") << "]" << " iyy [" << inertia_xml->Attribute("iyy") << "]" << " iyz [" << inertia_xml->Attribute("iyz") << "]" << " izz [" << inertia_xml->Attribute("izz") << "]"; CONSOLE_BRIDGE_logError(stm.str().c_str()); return false; } return true; }