RobotURDF.cpp

Go to the documentation of this file.

/***************************************************************************/
#include "robodyn_controllers/RobotURDF.h"

/***************************************************************************/
RobotURDF::RobotURDF(const std::string& filename, const std::string& rootLink) : Robot()
{
    parseFile(filename, rootLink);
}

/***************************************************************************/
RobotURDF::~RobotURDF()
{
//    linkMap.clear();
//    jointMap.clear();
}

/***************************************************************************/
void RobotURDF::parseFile(const std::string& filename, const std::string& rootLink)
{
    TiXmlDocument file(filename.c_str());

    if (!file.LoadFile())
    {
        std::stringstream err;
        err << "RobotURDF::parseFile could not load file " << filename;
        NasaCommonLogging::Logger::log("gov.nasa.controllers.RobotURDF", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
        return;
    }

    TiXmlHandle  doc(&file);
    TiXmlElement* robotElement;
    TiXmlElement* curElement;

    robotElement = doc.FirstChildElement("robot").Element();

    if (robotElement == NULL)
    {
        std::stringstream err;
        err << "RobotURDF::parseFile provided file does not contain a robot definition, file " << filename;
        NasaCommonLogging::Logger::log("gov.nasa.controllers.RobotURDF", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
        return;
    }

//    PVDEBUG("RobotURDF::parseFile loading configuration for robot %s\n", robotElement->Attribute("name"));

    // load the links
//    curElement = robotElement->FirstChildElement("link");
//    while (curElement != NULL)
//    {
//       parseLink(curElement);
//       curElement = curElement->NextSiblingElement("link");
//    }

    // load the joints
    curElement = robotElement->FirstChildElement("joint");

    while (curElement != NULL)
    {
        parseJoint(curElement);
        curElement = curElement->NextSiblingElement("joint");
    }

    if (!kdl_parser::treeFromFile(filename, robotTree))
    {
        std::stringstream err;
        err << "RobotURDF::parseFile provided file does not contain a robot definition, file " << filename;
        NasaCommonLogging::Logger::log("gov.nasa.controllers.RobotURDF", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

//    PVDEBUG("RobotURDF::parseFile file loaded\n");
//
//    // build the tree
//
//     JointMap joints;
//     getChildJoints(rootLink, &joints);
//
//     PVDEBUG("RobotURDF::parseFile building KDL Tree\n");
//     JointMap::iterator ittr;
//     for(ittr = joints.begin(); ittr != joints.end(); ittr++)
//     {
//        KDL::Chain chain;
//        buildTree(chain, ittr->first, "root",
//            ittr->first, ittr->second);
//     }
//     PVDEBUG("RobotURDF::parseFile KDL Tree complete\n");
}

/***************************************************************************/
//void RobotURDF::buildTree(KDL::Chain& chain, const std::string chainName,
//    const std::string chainHook, const std::string jointName,
//    const Joint& baseJoint)
//{
//    if (linkMap.find(baseJoint.child) == linkMap.end())
//    {
//        PDEBUG("RobotURDF::buildTree missing child link (%s) of joint %s, assuming NULL link\n",
//            baseJoint.child.c_str(), jointName.c_str());
//
//        KDL::Segment halfSeg(KDL::Segment("NullLink", KDL::Joint(jointName, baseJoint.type),
//            KDL::Frame(KDL::Rotation::RPY(0.0, 0.0, 0.0), KDL::Vector(0.0, 0.0, 0.0)),
//            KDL::RigidBodyInertia() ));
//
//        chain.addSegment(halfSeg);
//
//        PVDEBUG("RobotURDF::buildTree  -------- addChain: %s  [null]\n", chainName.c_str());
//        addChain(chainName, chain, chainHook);
//    }
//    else
//    {
//        KDL::RigidBodyInertia childLink = linkMap.find(baseJoint.child)->second;
//
//        JointMap joints;
//        getChildJoints(baseJoint.child, &joints);
//
//        if (joints.size() == 0)
//        {
//            KDL::Segment lastSeg(KDL::Segment(baseJoint.child, KDL::Joint(jointName, baseJoint.type),
//                KDL::Frame(KDL::Rotation::RPY(0.0, 0.0, 0.0), KDL::Vector(0.0, 0.0, 0.0)), childLink));
//
//            chain.addSegment(lastSeg);
//
//            PVDEBUG("RobotURDF::buildTree  -------- addChain: %s [end]\n", chainName.c_str());
//            addChain(chainName, chain, chainHook);
//        }
//        else if (joints.size() == 1)
//        {
//            KDL::Segment midSeg(KDL::Segment(baseJoint.child, KDL::Joint(jointName, baseJoint.type),
//                joints.begin()->second.frame, childLink));
//
//            chain.addSegment(midSeg);
//
//            buildTree(chain, chainName, chainHook,
//                joints.begin()->first, joints.begin()->second);
//        }
//        else
//        {
//            KDL::Segment tailSeg(baseJoint.child, KDL::Joint(jointName, baseJoint.type),
//                KDL::Frame(KDL::Rotation::RPY(0.0, 0.0, 0.0), KDL::Vector(0.0, 0.0, 0.0)), childLink);
//            PVDEBUG("RobotURDF::buildTree  -------- addChain: %s [split]\n", chainName.c_str());
//            chain.addSegment(tailSeg);
//
//            addChain(chainName, chain, chainHook);
//            JointMap::iterator ittr;
//            for(ittr = joints.begin(); ittr != joints.end(); ittr++)
//            {
//               KDL::Chain schain;
//               buildTree(schain, ittr->second.child, chain.segments[chain.getNrOfSegments() -1].getName(),
//                   ittr->first, ittr->second);
//            }
//        }
//    }
//}

/***************************************************************************/
//void RobotURDF::getChildJoints(std::string parentName, JointMap *jnts)
//{
//    jnts->clear();
//
//    JointMap::iterator ittr;
//    for (ittr = jointMap.begin(); ittr != jointMap.end(); ittr++)
//    {
//        if (ittr->second.parent.compare(parentName) == 0)
//        {
//            jnts->insert(std::pair<std::string, Joint>(ittr->first, ittr->second));
//        }
//    }
//}

/***************************************************************************/
//void RobotURDF::parseLink(TiXmlElement *element)
//{
//    PVDEBUG("RobotURDF::parseLink %s\n", element->Attribute("name") );
//
//    double mass = 0.0;
//    double ixx, ixy, ixz, iyy, iyz, izz = 0.0;
//
//    TiXmlElement *inertial = element->FirstChildElement("inertial");
//
//    if (inertial == NULL)
//    {
//        PDEBUG("RobotURDF::parseLink URDF format incomplete, link %s does not contain an inertial child, using default inertial",
//            element->Attribute("name"));
//    }
//    else
//    {
//        TiXmlElement *child = inertial->FirstChildElement("mass");
//        if (child == NULL)
//        {
//            PDEBUG("RobotURDF::parseLink URDF format incomplete, link %s does not contain a mass element, using default mass",
//                element->Attribute("name"));
//        }
//        else
//        {
//            child->QueryDoubleAttribute("value", &mass);
//        }
//
//        child = inertial->FirstChildElement("inertia");
//        if (child == NULL)
//        {
//            PDEBUG("RobotURDF::parseLink URDF format incomplete, link %s does not contain an inertia element, using default inertia",
//                element->Attribute("name"));
//        }
//        else
//        {
//            child->QueryDoubleAttribute("ixx", &ixx);
//            child->QueryDoubleAttribute("ixy", &ixy);
//            child->QueryDoubleAttribute("ixz", &ixz);
//            child->QueryDoubleAttribute("iyy", &iyy);
//            child->QueryDoubleAttribute("iyz", &iyz);
//            child->QueryDoubleAttribute("izz", &izz);
//        }
//
//        child = inertial->FirstChildElement("origin");
//        if (child != NULL)
//        {
//            // Currently, the origin element is not used by us, so print a warning
//            // if someone is using it.  Code could be added to support this, but I
//            // don't know how to turn the optional xyz and optional rpy elements
//            // of the origin into a single KDL::Vector.
//            PDEBUG("RobotURDF::parseLink URDF format unsupported, link %s contains an unsupported origin element that will be ignored\n",
//                element->Attribute("name"));
//            PDEBUG("    rpy=%s\n", child->Attribute("rpy"));
//            PDEBUG("    xyz=%s\n", child->Attribute("xyz"));
//        }
//    }
//
//    linkMap[element->Attribute("name")] = KDL::RigidBodyInertia(mass, KDL::Vector(0,0,0),
//        KDL::RotationalInertia(ixx, ixy, ixz, iyy, iyz, izz));
//}

/***************************************************************************/
void RobotURDF::parseJoint(TiXmlElement* element)
{
//    PVDEBUG("RobotURDF::parseJoint %s\n", element->Attribute("name") );

//    Joint joint;
    TiXmlElement* elmnt;

    // Get the joint axis, if not specified, z will be used
//    int axis_x, axis_y, axis_z = 0;
//    elmnt = element->FirstChildElement("axis");
//    if (elmnt != NULL)
//    {
//        const char *aval = elmnt->Attribute("xyz");
//        if (aval != NULL)
//        {
//            std::stringstream a;
//            a << aval;
//            a >> axis_x;
//            a >> axis_y;
//            a >> axis_z;
//        }
//    }

    // get the joint type
    const char* jtype = element->Attribute("type");
//    if (strcmp(jtype, "revolute"  ) == 0)
//    {
//        if      (axis_x != 0) joint.type = KDL::Joint::RotX;
//        else if (axis_y != 0) joint.type = KDL::Joint::RotY;
//        else                  joint.type = KDL::Joint::RotZ;
//    }
//    else if (strcmp(jtype, "prismatic" ) == 0)
//    {
//        if      (axis_x != 0) joint.type = KDL::Joint::TransX;
//        else if (axis_y != 0) joint.type = KDL::Joint::TransY;
//        else                  joint.type = KDL::Joint::TransZ;
//    }
//    else if (strcmp(jtype, "fixed"     ) == 0)
//    {
//        joint.type = KDL::Joint::None;
//    }
//    else
//    {
//        // KDL::Joint::TransAxis and  KDL::Joint::RotAxis throw exceptions in KDL::Joint so don't
//        // bother with them, just throw an exception now.  So URDF continuous and planar have no
//        // useful mapping.
//        THROW_RUNTIME("RobotURDF::parseJoint found a joint type %s, that does not translate to known KDL joint",
//            jtype);
//    }
//
    // get the links that this joint connects
//    elmnt = element->FirstChildElement("parent");
//    if (elmnt == NULL)
//    {
//        THROW_RUNTIME("RobotURDF::parseJoint URDF format error, joint %s does not contain a parent element",
//            element->Attribute("name"));
//    }
//    joint.parent = std::string(elmnt->Attribute("link"));
//
//    elmnt = element->FirstChildElement("child");
//    if (elmnt == NULL)
//    {
//        THROW_RUNTIME("RobotURDF::parseJoint URDF format error, joint %s does not contain a child element",
//            element->Attribute("name"));
//    }
//    joint.child = std::string(elmnt->Attribute("link"));

//    double roll, pitch, yaw, x, y, z = 0.0;
//    elmnt = element->FirstChildElement("origin");
//    if (elmnt == NULL)
//    {
//        PDEBUG("RobotURDF::parseJoint URDF format incomplete, joint %s does not contain an origin element, using default origin",
//            element->Attribute("name"));
//    }
//    else
//    {
//
//        const char *vval = elmnt->Attribute("xyz");
//        if (vval != NULL)
//        {
//            std::stringstream a;
//            a << vval;
//            a >> x;
//            a >> y;
//            a >> z;
//        }
//
//        const char *rval = elmnt->Attribute("rpy");
//        if (rval != NULL)
//        {
//            std::stringstream a;
//            a << rval;
//            a >> roll;
//            a >> pitch;
//            a >> yaw;
//        }
//
//        const char *rotm = elmnt->Attribute("rotm");
//        if (rotm != NULL)
//        {
//            double xx, xy, xz, yx, yy, yz, zx, zy, zz = 0.0;
//            std::stringstream a;
//             a << rotm;
//             a >> xx;
//             a >> xy;
//             a >> xz;
//             a >> yx;
//             a >> yy;
//             a >> yz;
//             a >> zx;
//             a >> zy;
//             a >> zz;
//
//            KDL::Rotation r(xx, xy, xz, yx, yy, yz, zx, zy, zz);
//
//            double mr, mp, my;
//            r.GetRPY(mr, mp, my);
//
//            printf("  %s rotm -> rpy=\"%10.6f %10.6f %10.6f\"\n", element->Attribute("name"), mr, mp, my);
//        }
//    }
//    joint.frame = KDL::Frame(KDL::Rotation::RPY(roll, pitch, yaw), KDL::Vector(x, y, z));

    if ((strcmp(jtype, "revolute"  ) == 0) ||  (strcmp(jtype, "prismatic" ) == 0))
    {
        JointTraits* data = addJoint(element->Attribute("name"));

        elmnt = element->FirstChildElement("limit");

        if (elmnt == NULL)
        {
//            PDEBUG("RobotURDF::parseJoint URDF format incomplete, joint %s does not contain a limit element, using default limits",
//                element->Attribute("name"));
        }
        else
        {
            elmnt->QueryDoubleAttribute("lower", &data->minPosition);
            elmnt->QueryDoubleAttribute("upper", &data->maxPosition);
            elmnt->QueryDoubleAttribute("velocity", &data->maxVelocity);
            elmnt->QueryDoubleAttribute("effort", &data->maxTorque);
        }

        elmnt = element->FirstChildElement("traits");

        if (elmnt == NULL)
        {
            data->springConstant = 10.0;
            data->type = "SuperDriver";
        }
        else
        {
            elmnt->QueryDoubleAttribute("spring_k", &data->springConstant);

            if (elmnt->Attribute("type"))
            {
                data->type = std::string(elmnt->Attribute("type"));
            }

            if (data->type == "")
            {
                data->type = "SuperDriver";
            }
        }
    }

    // ad the joint to the map
//    jointMap[element->Attribute("name")] = joint;
}