urdfreader.cc

Go to the documentation of this file.

#include <fstream>
#include <stack>

#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>
#include "ros/ros.h"

#include "rdl_dynamics/Model.h"
#include "rdl_dynamics/SpatialAlgebraOperators.h"
#include "rdl_urdfreader/urdfreader.h"

using namespace std;

namespace RobotDynamics
{
namespace Urdf
{
using namespace Math;

typedef vector<urdf::LinkSharedPtr> URDFLinkVector;
typedef vector<urdf::JointSharedPtr> URDFJointVector;
typedef map<string, urdf::LinkSharedPtr> URDFLinkMap;
typedef map<string, urdf::JointSharedPtr> URDFJointMap;

bool construct_model(ModelPtr rdl_model, urdf::ModelInterfaceSharedPtr urdf_model, bool floating_base, bool verbose)
{
    urdf::LinkSharedPtr urdf_root_link;

    URDFLinkMap link_map;
    link_map = urdf_model->links_;

    URDFJointMap joint_map;
    joint_map = urdf_model->joints_;

    vector<string> joint_names;

    stack<urdf::LinkSharedPtr> link_stack;
    stack<int> joint_index_stack;

    // add the bodies in a depth-first order of the model tree
    link_stack.push(link_map[(urdf_model->getRoot()->name)]);

    // add the root body
    urdf::LinkConstSharedPtr root = urdf_model->getRoot();
    Vector3d root_inertial_rpy;
    Vector3d root_inertial_position;
    Matrix3d root_inertial_inertia;
    // cppcheck-suppress variableScope

    if (root->inertial)
    {
        double root_inertial_mass = root->inertial->mass;

        root_inertial_position.set(root->inertial->origin.position.x, root->inertial->origin.position.y, root->inertial->origin.position.z);

        root->inertial->origin.rotation.getRPY(root_inertial_rpy[0], root_inertial_rpy[1], root_inertial_rpy[2]);

        RobotDynamics::Math::Matrix3d root_inertia_in_com_frame = RobotDynamics::Math::Matrix3dZero;
        root_inertia_in_com_frame(0, 0) = root->inertial->ixx;
        root_inertia_in_com_frame(0, 1) = root->inertial->ixy;
        root_inertia_in_com_frame(0, 2) = root->inertial->ixz;
        root_inertia_in_com_frame(1, 0) = root->inertial->ixy;
        root_inertia_in_com_frame(1, 1) = root->inertial->iyy;
        root_inertia_in_com_frame(1, 2) = root->inertial->iyz;
        root_inertia_in_com_frame(2, 0) = root->inertial->ixz;
        root_inertia_in_com_frame(2, 1) = root->inertial->iyz;
        root_inertia_in_com_frame(2, 2) = root->inertial->izz;

        RobotDynamics::Math::SpatialTransform X_body_to_inertial = RobotDynamics::Math::XeulerXYZ(root_inertial_rpy);
        root_inertial_inertia =
            root_inertial_rpy == Vector3d(0., 0., 0.) ? root_inertia_in_com_frame : X_body_to_inertial.E.transpose() * root_inertia_in_com_frame * X_body_to_inertial.E;
        Body root_link = Body(root_inertial_mass, root_inertial_position, root_inertial_inertia);

        Joint root_joint(JointTypeFixed);
        if (floating_base)
        {
            root_joint = Joint(JointTypeFloatingBase);
        }

        SpatialTransform root_joint_frame = SpatialTransform();

        if (verbose)
        {
            cout << "+ Adding Root Body " << endl;
            cout << "  joint frame: " << root_joint_frame << endl;
            if (floating_base)
            {
                cout << "  joint type : floating" << endl;
            }
            else
            {
                cout << "  joint type : fixed" << endl;
            }
            cout << "  body inertia: " << endl << root_link.mInertia << endl;
            cout << "  body mass   : " << root_link.mMass << endl;
            cout << "  body name   : " << root->name << endl;
        }

        rdl_model->appendBody(root_joint_frame, root_joint, root_link, root->name);
    }

    if (link_stack.top()->child_joints.size() > 0)
    {
        joint_index_stack.push(0);
    }

    //            while(link_stack.size() > 0)
    while (!link_stack.empty())
    {
        urdf::LinkSharedPtr cur_link = link_stack.top();
        unsigned int joint_idx;
        if (joint_index_stack.size() > 0)
        {
            joint_idx = joint_index_stack.top();
        }
        else
        {
            joint_idx = 0;
        }

        if (joint_idx < cur_link->child_joints.size())
        {
            urdf::JointSharedPtr cur_joint = cur_link->child_joints[joint_idx];

            // increment joint index
            joint_index_stack.pop();
            joint_index_stack.push(joint_idx + 1);

            link_stack.push(link_map[cur_joint->child_link_name]);
            joint_index_stack.push(0);

            if (verbose)
            {
                for (unsigned int i = 1; i < joint_index_stack.size() - 1; i++)
                {
                    cout << "  ";
                }
                cout << "joint '" << cur_joint->name << "' child link '" << link_stack.top()->name << "' type = " << cur_joint->type << endl;
            }

            joint_names.push_back(cur_joint->name);
        }
        else
        {
            link_stack.pop();
            joint_index_stack.pop();
        }
    }

    unsigned int j;
    for (j = 0; j < joint_names.size(); j++)
    {
        urdf::JointSharedPtr urdf_joint = joint_map[joint_names[j]];
        urdf::LinkSharedPtr urdf_parent = link_map[urdf_joint->parent_link_name];
        urdf::LinkSharedPtr urdf_child = link_map[urdf_joint->child_link_name];

        // determine where to add the current joint and child body
        unsigned int rdl_parent_id = 0;
        if (rdl_model->mBodies.size() != 1 || rdl_model->mFixedBodies.size() != 0)
        {
            rdl_parent_id = rdl_model->GetBodyId(urdf_parent->name.c_str());
        }

        if (rdl_parent_id == std::numeric_limits<unsigned int>::max())
        {
            cerr << "Error while processing joint '" << urdf_joint->name << "': parent link '" << urdf_parent->name << "' could not be found." << endl;
        }

        // create the joint
        Joint rdl_joint;
        if (urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::CONTINUOUS)
        {
            rdl_joint = Joint(SpatialVector(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z, 0., 0., 0.));
        }
        else if (urdf_joint->type == urdf::Joint::PRISMATIC)
        {
            rdl_joint = Joint(SpatialVector(0., 0., 0., urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
        }
        else if (urdf_joint->type == urdf::Joint::FIXED)
        {
            rdl_joint = Joint(JointTypeFixed);
        }
        else if (urdf_joint->type == urdf::Joint::FLOATING)
        {
            // todo: what order of DoF should be used?
            rdl_joint = Joint(SpatialVector(0., 0., 0., 1., 0., 0.), SpatialVector(0., 0., 0., 0., 1., 0.), SpatialVector(0., 0., 0., 0., 0., 1.),
                              SpatialVector(1., 0., 0., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(0., 0., 1., 0., 0., 0.));
        }
        else if (urdf_joint->type == urdf::Joint::PLANAR)
        {
            // todo: which two directions should be used that are perpendicular
            // to the specified axis?
            cerr << "Error while processing joint '" << urdf_joint->name << "': planar joints not yet supported!" << endl;
            return false;
        }

        // compute the joint transformation
        Vector3d joint_rpy;
        Vector3d joint_translation;
        urdf_joint->parent_to_joint_origin_transform.rotation.getRPY(joint_rpy[0], joint_rpy[1], joint_rpy[2]);
        joint_translation.set(urdf_joint->parent_to_joint_origin_transform.position.x, urdf_joint->parent_to_joint_origin_transform.position.y,
                              urdf_joint->parent_to_joint_origin_transform.position.z);
        SpatialTransform rdl_joint_frame = Xrot(joint_rpy[0], Vector3d(1., 0., 0.)) * Xrot(joint_rpy[1], Vector3d(0., 1., 0.)) *
                                           Xrot(joint_rpy[2], Vector3d(0., 0., 1.)) * Xtrans(Vector3d(joint_translation));

        // assemble the body
        Vector3d link_inertial_position;
        Vector3d link_inertial_rpy;
        Matrix3d link_inertial_inertia = Matrix3d::Zero();
        double link_inertial_mass = 0.;

        // but only if we actually have inertial data
        if (urdf_child->inertial)
        {
            link_inertial_mass = urdf_child->inertial->mass;

            link_inertial_position.set(urdf_child->inertial->origin.position.x, urdf_child->inertial->origin.position.y, urdf_child->inertial->origin.position.z);
            urdf_child->inertial->origin.rotation.getRPY(link_inertial_rpy[0], link_inertial_rpy[1], link_inertial_rpy[2]);

            RobotDynamics::Math::Matrix3d link_inertia_in_com_frame = RobotDynamics::Math::Matrix3dZero;
            link_inertia_in_com_frame(0, 0) = urdf_child->inertial->ixx;
            link_inertia_in_com_frame(0, 1) = urdf_child->inertial->ixy;
            link_inertia_in_com_frame(0, 2) = urdf_child->inertial->ixz;
            link_inertia_in_com_frame(1, 0) = urdf_child->inertial->ixy;
            link_inertia_in_com_frame(1, 1) = urdf_child->inertial->iyy;
            link_inertia_in_com_frame(1, 2) = urdf_child->inertial->iyz;
            link_inertia_in_com_frame(2, 0) = urdf_child->inertial->ixz;
            link_inertia_in_com_frame(2, 1) = urdf_child->inertial->iyz;
            link_inertia_in_com_frame(2, 2) = urdf_child->inertial->izz;

            RobotDynamics::Math::SpatialTransform X_body_to_inertial = RobotDynamics::Math::XeulerXYZ(link_inertial_rpy);
            link_inertial_inertia = link_inertial_rpy == Vector3d(0., 0., 0.) ? link_inertia_in_com_frame :
                                                                                X_body_to_inertial.E.transpose() * link_inertia_in_com_frame * X_body_to_inertial.E;
        }

        Body rdl_body = Body(link_inertial_mass, link_inertial_position, link_inertial_inertia);

        if (verbose)
        {
            cout << "+ Adding Body " << endl;
            cout << "  parent_id  : " << rdl_parent_id << endl;
            cout << "  joint frame: " << rdl_joint_frame << endl;
            cout << "  joint dofs : " << rdl_joint.mDoFCount << endl;
            for (unsigned int j = 0; j < rdl_joint.mDoFCount; j++)
            {
                cout << "    " << j << ": " << rdl_joint.mJointAxes[j].transpose() << endl;
            }
            cout << "  body inertia: " << endl << rdl_body.mInertia << endl;
            cout << "  body mass   : " << rdl_body.mMass << endl;
            cout << "  body name   : " << urdf_child->name << endl;
        }

        if (urdf_joint->type == urdf::Joint::FLOATING)
        {
            Matrix3d zero_matrix = Matrix3d::Zero();
            Body null_body(0., Vector3d::Zero(3), zero_matrix);
            Joint joint_txtytz(JointTypeTranslationXYZ);
            string trans_body_name = urdf_child->name + "_Translate";
            rdl_model->addBody(rdl_parent_id, rdl_joint_frame, joint_txtytz, null_body, trans_body_name);

            Joint joint_euler_zyx(JointTypeEulerXYZ);
            rdl_model->appendBody(SpatialTransform(), joint_euler_zyx, rdl_body, urdf_child->name);
        }
        else
        {
            rdl_model->addBody(rdl_parent_id, rdl_joint_frame, rdl_joint, rdl_body, urdf_child->name);
        }
    }

    return true;
}

bool parseJointBodyNameMapFromFile(const char* filename, std::map<std::string, std::string>& jointBodyMap)
{
    ifstream model_file(filename);
    if (!model_file)
    {
        cerr << "Error opening file '" << filename << "'." << endl;
        return false;
    }

    // reserve memory for the contents of the file
    std::string model_xml_string;
    model_file.seekg(0, std::ios::end);
    model_xml_string.reserve(model_file.tellg());
    model_file.seekg(0, std::ios::beg);
    model_xml_string.assign((std::istreambuf_iterator<char>(model_file)), std::istreambuf_iterator<char>());

    model_file.close();

    return parseJointBodyNameMapFromString(model_xml_string.c_str(), jointBodyMap);
}

bool parseJointAndBodyNamesFromString(const char* model_xml_string, std::vector<std::string>& joint_names, std::vector<std::string>& body_names)
{
    TiXmlDocument doc;

    // Check if contents are valid, if not, abort
    if (!doc.Parse(model_xml_string) && doc.Error())
    {
        std::cerr << "Can't parse urdf. Xml is invalid" << std::endl;
        return false;
    }

    // Find joints in transmission tags
    TiXmlElement* root = doc.RootElement();

    for (TiXmlElement* joint = root->FirstChildElement("joint"); joint; joint = joint->NextSiblingElement("joint"))
    {
        if (!std::strcmp(joint->Attribute("type"), "fixed") || !std::strcmp(joint->Attribute("type"), "floating"))
        {
            continue;
        }

        joint_names.push_back(joint->Attribute("name"));
        body_names.push_back(joint->FirstChildElement("child")->Attribute("link"));
    }

    return true;
}

bool parseJointAndBodyNamesFromString(const std::string& model_xml_string, std::vector<std::string>& joint_names, std::vector<std::string>& body_names)
{
    return parseJointAndBodyNamesFromString(model_xml_string.c_str(), joint_names, body_names);
}

bool parseJointBodyNameMapFromString(const std::string& model_xml_string, std::map<std::string, std::string>& jointBodyMap)
{
    std::vector<std::string> joint_names, body_names;
    if (!parseJointAndBodyNamesFromString(model_xml_string, joint_names, body_names))
    {
        return false;
    }

    jointBodyMap.clear();
    for (unsigned int i = 0; i < joint_names.size(); i++)
    {
        jointBodyMap.insert(std::pair<std::string, std::string>(joint_names[i], body_names[i]));
    }

    return true;
}

bool parseJointBodyNameMapFromString(const char* model_xml_string, std::map<std::string, std::string>& jointBodyMap)
{
    std::vector<std::string> joint_names, body_names;
    if (!parseJointAndBodyNamesFromString(model_xml_string, joint_names, body_names))
    {
        return false;
    }

    jointBodyMap.clear();
    for (unsigned int i = 0; i < joint_names.size(); i++)
    {
        jointBodyMap.insert(std::pair<std::string, std::string>(joint_names[i], body_names[i]));
    }

    return true;
}

bool parseJointAndQIndex(const RobotDynamics::Model& model, const std::vector<std::string>& body_names, std::vector<unsigned int>& q_indices)
{
    q_indices.clear();
    for (unsigned int i = 0; i < body_names.size(); i++)
    {
        q_indices.push_back(model.mJoints[model.GetBodyId(body_names[i].c_str())].q_index);
    }

    return true;
}

bool parseJointAndQIndex(const std::string& model_xml_string, std::vector<unsigned int>& q_indices)
{
    RobotDynamics::ModelPtr model(new RobotDynamics::Model());
    if (!urdfReadFromString(model_xml_string, model))
    {
        return false;
    }

    std::vector<std::string> joint_names, body_names;
    if (!parseJointAndBodyNamesFromString(model_xml_string, joint_names, body_names))
    {
        return false;
    }

    return parseJointAndQIndex(*model, body_names, q_indices);
}

bool urdfReadFromFile(const char* filename, ModelPtr model, bool floating_base, bool verbose)
{
    ifstream model_file(filename);
    if (!model_file)
    {
        cerr << "Error opening file '" << filename << "'." << endl;
        return false;
    }

    // reserve memory for the contents of the file
    string model_xml_string;
    model_file.seekg(0, std::ios::end);
    model_xml_string.reserve(model_file.tellg());
    model_file.seekg(0, std::ios::beg);
    model_xml_string.assign((std::istreambuf_iterator<char>(model_file)), std::istreambuf_iterator<char>());

    model_file.close();

    return urdfReadFromString(model_xml_string.c_str(), model, floating_base, verbose);
}

bool urdfReadFromString(const char* model_xml_string, ModelPtr model, bool floating_base, bool verbose)
{
    assert(model);

    urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(model_xml_string);

    if (!construct_model(model, urdf_model, floating_base, verbose))
    {
        cerr << "Error constructing model from urdf file." << endl;
        return false;
    }

    model->gravity.SpatialVector::set(0., 0., 0., 0., 0., -9.81);

    return true;
}

bool urdfReadFromFile(const std::string& filename, ModelPtr model)
{
    ifstream model_file(filename.c_str());
    if (!model_file)
    {
        cerr << "Error opening file '" << filename << "'." << endl;
        return false;
    }

    // reserve memory for the contents of the file
    std::string model_xml_string;
    model_file.seekg(0, std::ios::end);
    model_xml_string.reserve(model_file.tellg());
    model_file.seekg(0, std::ios::beg);
    model_xml_string.assign((std::istreambuf_iterator<char>(model_file)), std::istreambuf_iterator<char>());

    model_file.close();

    return urdfReadFromString(model_xml_string, model);
}

bool urdfReadFromString(const std::string& model_xml_string, ModelPtr model)
{
    assert(model);

    urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(model_xml_string.c_str());

    bool floating_base = std::strcmp(urdf_model->getRoot()->name.c_str(), "world") ? true : false;

    if (!construct_model(model, urdf_model, floating_base, false))
    {
        cerr << "Error constructing model from urdf file." << endl;
        return false;
    }

    model->gravity.SpatialVector::set(0., 0., 0., 0., 0., -9.81);

    return true;
}
}  // namespace Urdf
}  // namespace RobotDynamics