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;
}
