bool assignMaterial(const VisualSharedPtr& visual, ModelInterfaceSharedPtr& model, const char* link_name) { if (visual->material_name.empty()) return true; const MaterialSharedPtr& material = model->getMaterial(visual->material_name); if (material) { CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material to '%s'", link_name, visual->material_name.c_str()); visual->material = material; } else { if (visual->material) { CONSOLE_BRIDGE_logDebug("urdfdom: link '%s' material '%s' defined in Visual.", link_name, visual->material_name.c_str()); model->materials_.insert(make_pair(visual->material->name, visual->material)); } else { CONSOLE_BRIDGE_logWarn("link '%s' material '%s' undefined.", link_name,visual->material_name.c_str()); return false; } } return true; }
TiXmlDocument* exportURDF(const ModelInterface &model) { TiXmlDocument *doc = new TiXmlDocument(); TiXmlElement *robot = new TiXmlElement("robot"); robot->SetAttribute("name", model.name_); doc->LinkEndChild(robot); for (std::map<std::string, MaterialSharedPtr>::const_iterator m=model.materials_.begin(); m!=model.materials_.end(); m++) { CONSOLE_BRIDGE_logDebug("urdfdom: exporting material [%s]\n",m->second->name.c_str()); exportMaterial(*(m->second), robot); } for (std::map<std::string, LinkSharedPtr>::const_iterator l=model.links_.begin(); l!=model.links_.end(); l++) { CONSOLE_BRIDGE_logDebug("urdfdom: exporting link [%s]\n",l->second->name.c_str()); exportLink(*(l->second), robot); } for (std::map<std::string, JointSharedPtr>::const_iterator j=model.joints_.begin(); j!=model.joints_.end(); j++) { CONSOLE_BRIDGE_logDebug("urdfdom: exporting joint [%s]\n",j->second->name.c_str()); exportJoint(*(j->second), robot); } return doc; }
bool parseJointMimic(JointMimic &jm, TiXmlElement* config) { jm.clear(); // Get name of joint to mimic const char* joint_name_str = config->Attribute("joint"); if (joint_name_str == NULL) { CONSOLE_BRIDGE_logError("joint mimic: no mimic joint specified"); return false; } else jm.joint_name = joint_name_str; // Get mimic multiplier const char* multiplier_str = config->Attribute("multiplier"); if (multiplier_str == NULL) { CONSOLE_BRIDGE_logDebug("urdfdom.joint_mimic: no multiplier, using default value of 1"); jm.multiplier = 1; } else { try { jm.multiplier = boost::lexical_cast<double>(multiplier_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("multiplier value (%s) is not a float: %s",multiplier_str, e.what()); return false; } } // Get mimic offset const char* offset_str = config->Attribute("offset"); if (offset_str == NULL) { CONSOLE_BRIDGE_logDebug("urdfdom.joint_mimic: no offset, using default value of 0"); jm.offset = 0; } else { try { jm.offset = boost::lexical_cast<double>(offset_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("offset value (%s) is not a float: %s",offset_str, e.what()); return false; } } return true; }
bool parseJointDynamics(JointDynamics &jd, TiXmlElement* config) { jd.clear(); // Get joint damping const char* damping_str = config->Attribute("damping"); if (damping_str == NULL){ CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: no damping, defaults to 0"); jd.damping = 0; } else { try { jd.damping = boost::lexical_cast<double>(damping_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("damping value (%s) is not a float: %s",damping_str, e.what()); return false; } } // Get joint friction const char* friction_str = config->Attribute("friction"); if (friction_str == NULL){ CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: no friction, defaults to 0"); jd.friction = 0; } else { try { jd.friction = boost::lexical_cast<double>(friction_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("friction value (%s) is not a float: %s",friction_str, e.what()); return false; } } if (damping_str == NULL && friction_str == NULL) { CONSOLE_BRIDGE_logError("joint dynamics element specified with no damping and no friction"); return false; } else{ CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: damping %f and friction %f", jd.damping, jd.friction); return true; } }
bool parseJointCalibration(JointCalibration &jc, TiXmlElement* config) { jc.clear(); // Get rising edge position const char* rising_position_str = config->Attribute("rising"); if (rising_position_str == NULL) { CONSOLE_BRIDGE_logDebug("urdfdom.joint_calibration: no rising, using default value"); jc.rising.reset(); } else { try { jc.rising.reset(new double(boost::lexical_cast<double>(rising_position_str))); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("risingvalue (%s) is not a float: %s",rising_position_str, e.what()); return false; } } // Get falling edge position const char* falling_position_str = config->Attribute("falling"); if (falling_position_str == NULL) { CONSOLE_BRIDGE_logDebug("urdfdom.joint_calibration: no falling, using default value"); jc.falling.reset(); } else { try { jc.falling.reset(new double(boost::lexical_cast<double>(falling_position_str))); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("fallingvalue (%s) is not a float: %s",falling_position_str, e.what()); return false; } } return true; }
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 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; }