bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger* logger) { // Get Joint Name const char *name = config->Attribute("name"); if (!name) { logger->reportError("unnamed joint found"); return false; } joint.m_name = name; joint.m_parentLinkToJointTransform.setIdentity(); // Get transform from Parent Link to Joint Frame TiXmlElement *origin_xml = config->FirstChildElement("origin"); if (origin_xml) { if (!parseTransform(joint.m_parentLinkToJointTransform, origin_xml,logger)) { logger->reportError("Malformed parent origin element for joint:"); logger->reportError(joint.m_name.c_str()); return false; } } // Get Parent Link TiXmlElement *parent_xml = config->FirstChildElement("parent"); if (parent_xml) { if (m_parseSDF) { joint.m_parentLinkName = std::string(parent_xml->GetText()); } else { const char *pname = parent_xml->Attribute("link"); if (!pname) { logger->reportError("no parent link name specified for Joint link. this might be the root?"); logger->reportError(joint.m_name.c_str()); return false; } else { joint.m_parentLinkName = std::string(pname); } } } // Get Child Link TiXmlElement *child_xml = config->FirstChildElement("child"); if (child_xml) { if (m_parseSDF) { joint.m_childLinkName = std::string(child_xml->GetText()); } else { const char *pname = child_xml->Attribute("link"); if (!pname) { logger->reportError("no child link name specified for Joint link [%s]."); logger->reportError(joint.m_name.c_str()); return false; } else { joint.m_childLinkName = std::string(pname); } } } // Get Joint type const char* type_char = config->Attribute("type"); if (!type_char) { logger->reportError("joint [%s] has no type, check to see if it's a reference."); logger->reportError(joint.m_name.c_str()); return false; } std::string type_str = type_char; if (type_str == "planar") joint.m_type = URDFPlanarJoint; else if (type_str == "floating") joint.m_type = URDFFloatingJoint; else if (type_str == "revolute") joint.m_type = URDFRevoluteJoint; else if (type_str == "continuous") joint.m_type = URDFContinuousJoint; else if (type_str == "prismatic") joint.m_type = URDFPrismaticJoint; else if (type_str == "fixed") joint.m_type = URDFFixedJoint; else { logger->reportError("Joint "); logger->reportError(joint.m_name.c_str()); logger->reportError("has unknown type:"); logger->reportError(type_str.c_str()); return false; } if (m_parseSDF) { if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint) { // axis TiXmlElement *axis_xml = config->FirstChildElement("axis"); if (!axis_xml){ logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis"); logger->reportWarning(joint.m_name.c_str()); joint.m_localJointAxis.setValue(1,0,0); } else{ TiXmlElement *xyz_xml = axis_xml->FirstChildElement("xyz"); if (xyz_xml) { if (!parseVector3(joint.m_localJointAxis,std::string(xyz_xml->GetText()),logger)) { logger->reportError("Malformed axis element:"); logger->reportError(joint.m_name.c_str()); logger->reportError(" for joint:"); logger->reportError(xyz_xml->GetText()); return false; } } TiXmlElement *limit_xml = axis_xml->FirstChildElement("limit"); if (limit_xml) { if (!parseJointLimits(joint, limit_xml,logger)) { logger->reportError("Could not parse limit element for joint:"); logger->reportError(joint.m_name.c_str()); return false; } } else if (joint.m_type == URDFRevoluteJoint) { logger->reportError("Joint is of type REVOLUTE but it does not specify limits"); logger->reportError(joint.m_name.c_str()); return false; } else if (joint.m_type == URDFPrismaticJoint) { logger->reportError("Joint is of type PRISMATIC without limits"); logger->reportError( joint.m_name.c_str()); return false; } TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); if (prop_xml) { if (!parseJointDynamics(joint, prop_xml,logger)) { logger->reportError("Could not parse dynamics element for joint:"); logger->reportError(joint.m_name.c_str()); return false; } } } } } else { // Get Joint Axis if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint) { // axis TiXmlElement *axis_xml = config->FirstChildElement("axis"); if (!axis_xml){ logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis"); logger->reportWarning(joint.m_name.c_str()); joint.m_localJointAxis.setValue(1,0,0); } else{ if (axis_xml->Attribute("xyz")) { if (!parseVector3(joint.m_localJointAxis,axis_xml->Attribute("xyz"),logger)) { logger->reportError("Malformed axis element:"); logger->reportError(joint.m_name.c_str()); logger->reportError(" for joint:"); logger->reportError(axis_xml->Attribute("xyz")); return false; } } } } // Get limit TiXmlElement *limit_xml = config->FirstChildElement("limit"); if (limit_xml) { if (!parseJointLimits(joint, limit_xml,logger)) { logger->reportError("Could not parse limit element for joint:"); logger->reportError(joint.m_name.c_str()); return false; } } else if (joint.m_type == URDFRevoluteJoint) { logger->reportError("Joint is of type REVOLUTE but it does not specify limits"); logger->reportError(joint.m_name.c_str()); return false; } else if (joint.m_type == URDFPrismaticJoint) { logger->reportError("Joint is of type PRISMATIC without limits"); logger->reportError( joint.m_name.c_str()); return false; } joint.m_jointDamping = 0; joint.m_jointFriction = 0; // Get Dynamics TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); if (prop_xml) { // Get joint damping const char* damping_str = prop_xml->Attribute("damping"); if (damping_str) { joint.m_jointDamping = urdfLexicalCast<double>(damping_str); } // Get joint friction const char* friction_str = prop_xml->Attribute("friction"); if (friction_str) { joint.m_jointFriction = urdfLexicalCast<double>(friction_str); } if (damping_str == NULL && friction_str == NULL) { logger->reportError("joint dynamics element specified with no damping and no friction"); return false; } } } 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; }