bool Parser::parseJoint()

in src/esp/io/URDFParser.cpp [1025:1167]
120 lines of code
30 McCabe index (conditional complexity)

bool Parser::parseJoint(Joint& joint, const tinyxml2::XMLElement* config) {
  // Get Joint Name
  const char* name = config->Attribute("name");
  if (!name) {
    ESP_VERY_VERBOSE() << "Unnamed joint found";
    return false;
  }
  joint.m_name = name;
  joint.m_parentLinkToJointTransform = Mn::Matrix4();  // Identity

  // Get transform from Parent Link to Joint Frame
  const XMLElement* origin_xml = config->FirstChildElement("origin");
  if (origin_xml) {
    if (!parseTransform(joint.m_parentLinkToJointTransform, origin_xml)) {
      ESP_VERY_VERBOSE() << "Malformed parent origin element for joint:"
                         << joint.m_name;
      return false;
    }
  }

  // Get Parent Link
  const XMLElement* parent_xml = config->FirstChildElement("parent");
  if (parent_xml) {
    const char* pname = parent_xml->Attribute("link");
    if (!pname) {
      ESP_VERY_VERBOSE() << "No parent link name specified for "
                            "Joint link. this might be the root?"
                         << joint.m_name;
      return false;
    } else {
      joint.m_parentLinkName = std::string(pname);
    }
  }

  // Get Child Link
  const XMLElement* child_xml = config->FirstChildElement("child");
  if (child_xml) {
    const char* pname = child_xml->Attribute("link");
    if (!pname) {
      ESP_VERY_VERBOSE() << "No child link name specified for Joint link"
                         << joint.m_name;
      return false;
    } else {
      joint.m_childLinkName = std::string(pname);
    }
  }

  // Get Joint type
  const char* type_char = config->Attribute("type");
  if (!type_char) {
    ESP_VERY_VERBOSE() << "Joint" << joint.m_name
                       << "has no type, check to see if it's a reference.";
    return false;
  }

  std::string type_str = type_char;
  if (type_str == "spherical")
    joint.m_type = SphericalJoint;
  else if (type_str == "planar")
    joint.m_type = PlanarJoint;
  else if (type_str == "floating")
    joint.m_type = FloatingJoint;
  else if (type_str == "revolute")
    joint.m_type = RevoluteJoint;
  else if (type_str == "continuous")
    joint.m_type = ContinuousJoint;
  else if (type_str == "prismatic")
    joint.m_type = PrismaticJoint;
  else if (type_str == "fixed")
    joint.m_type = FixedJoint;
  else {
    ESP_VERY_VERBOSE() << "Joint" << joint.m_name
                       << "has unknown type:" << type_str;
    return false;
  }

  // Get Joint Axis
  if (joint.m_type != FloatingJoint && joint.m_type != FixedJoint) {
    // axis
    const XMLElement* axis_xml = config->FirstChildElement("axis");
    if (!axis_xml) {
      ESP_VERY_VERBOSE() << "W - urdfdom: no axis elemement for Joint, "
                            "defaulting to (1,0,0) axis"
                         << joint.m_name;
      joint.m_localJointAxis = Mn::Vector3(1, 0, 0);
    } else {
      if (axis_xml->Attribute("xyz")) {
        if (!parseVector3(joint.m_localJointAxis, axis_xml->Attribute("xyz"))) {
          ESP_VERY_VERBOSE()
              << "Malformed axis element:" << axis_xml->Attribute("xyz")
              << "for joint:" << joint.m_name;
          return false;
        }
      }
    }
  }

  // Get limit
  const XMLElement* limit_xml = config->FirstChildElement("limit");
  if (limit_xml) {
    if (!parseJointLimits(joint, limit_xml)) {
      ESP_VERY_VERBOSE() << "Could not parse limit element for joint:"
                         << joint.m_name;
      return false;
    }
  } else if (joint.m_type == RevoluteJoint) {
    ESP_VERY_VERBOSE()
        << "Joint is of type REVOLUTE but it does not specify limits:"
        << joint.m_name;
    return false;
  } else if (joint.m_type == PrismaticJoint) {
    ESP_VERY_VERBOSE() << "Joint is of type PRISMATIC without limits:"
                       << joint.m_name;
    return false;
  }

  joint.m_jointDamping = 0;
  joint.m_jointFriction = 0;

  // Get Dynamics
  const XMLElement* 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 = std::stod(damping_str);
    }

    // Get joint friction
    const char* friction_str = prop_xml->Attribute("friction");
    if (friction_str) {
      joint.m_jointFriction = std::stod(friction_str);
    }

    if (damping_str == nullptr && friction_str == nullptr) {
      ESP_VERY_VERBOSE() << "Joint dynamics element specified with "
                            "no damping and no friction";
      return false;
    }
  }

  return true;
}