ContactsGenerator.cpp

Go to the documentation of this file.

#include <urdf_traverser/Functions.h>
#include <urdf_traverser/DependencyOrderedJoints.h>

#include <urdf2inventor/IVHelpers.h>
#include <urdf2graspit/ContactFunctions.h>
#include <urdf2graspit/ContactsGenerator.h>
#include <urdf2inventor/Helpers.h>

#include <urdf2graspit/Types.h>
#include <urdf2graspit/MarkerSelector.h>

#include <string>
#include <ros/ros.h>
#include <ros/package.h>

#include <map>
#include <vector>
#include <set>

#define RAD_TO_DEG 180/M_PI

using urdf2graspit::ContactsGenerator;
using urdf2graspit::markerselector::MarkerSelector;
using urdf2inventor::EigenTransform;

bool ContactsGenerator::transformToDHReferenceFrames(const std::vector<DHParam>& dh)
{
    UrdfTraverserPtr trav = getTraverser();
    if (!trav)
    {
        ROS_ERROR("Traverser not set.");
        return false;
    }
    std::map<std::string,EigenTransform> transforms;
    if (!DHParam::getTransforms(dh, true, transforms))
    {
        ROS_ERROR("Could not get transforms from DH to URDF");
        return false; 
    }

    std::map<std::string,EigenTransform>::iterator it;
    for (it=transforms.begin(); it!=transforms.end(); ++it)
    {
        LinkPtr link=trav->getLink(it->first);
        if (!link.get())
        {
            ROS_ERROR("Link %s does not exist", it->first.c_str());
            return false;
        }
        bool preApply = true;
        applyTransformToContacts(link, it->second, preApply);
    }
    return true;
}


void ContactsGenerator::applyTransformToContacts(LinkPtr& link, const EigenTransform& trans, bool preMult)
{
    std::map<std::string, std::vector<ContactPtr> >::iterator lCnt = linkContacts.find(link->name);
    if (lCnt != linkContacts.end())
    {
        for (std::vector<ContactPtr>::iterator it = lCnt->second.begin(); it != lCnt->second.end(); ++it)
        {
            ContactPtr c = *it;
            EigenTransform t = EigenTransform::Identity();
            t.translate(c->loc);
            t.rotate(c->ori);
            // ROS_INFO_STREAM("////// Applying transform "<<trans<<" to "<<c->loc);
            if (preMult) t = trans * t;
            else         t = t * trans;
            c->loc = t.translation();
            c->ori = Eigen::Quaterniond(t.rotation());
            c->norm = trans.rotation() * c->norm;
        }
    }
}

void ContactsGenerator::scaleContacts(double scale_factor)
{
    if (isContactsScaled) return;
    std::map<std::string, std::vector<ContactPtr> >::iterator linkItr;
    for (linkItr=linkContacts.begin(); linkItr!=linkContacts.end(); ++linkItr)
    {
        for (std::vector<ContactPtr>::iterator it = linkItr->second.begin(); it != linkItr->second.end(); ++it)
        {
            ContactPtr c = *it;
            c->loc *= scale_factor;
        }
    }
    isContactsScaled = true;
}

bool ContactsGenerator::generateContactsForallVisuals(const std::string& linkName, const int linkNum, const int fingerNum,
        const float coefficient, const std::vector<MarkerSelector::Marker>& markers)
{
    UrdfTraverserPtr trav = getTraverser();
    if (!trav)
    {
        ROS_ERROR("Traverser not set.");
        return false;
    }

    LinkPtr link = trav->getLink(linkName);

    std::map<std::string, std::vector<ContactPtr> >::iterator lCnt = linkContacts.find(linkName);
    if (lCnt == linkContacts.end())
        lCnt = linkContacts.insert(std::make_pair(linkName, std::vector<ContactPtr>())).first;

    std::vector<MarkerSelector::Marker>::const_iterator m = markers.begin();
    int visualNum = -1;
    for (std::vector<VisualPtr >::iterator vit = link->visual_array.begin();
            vit != link->visual_array.end(); ++vit)
    {
        ++visualNum;

        if ((m != markers.end()) && (m->visualNum > visualNum)) continue;

        VisualPtr visual = *vit;

        EigenTransform vTrans = urdf_traverser::getTransform(visual->origin);   // transform from link reference frame to the visual

        // move forward in markers vector until we get the correct visual number
        while (m != markers.end())
        {
            // ROS_INFO("linkname %s vn %i",m->linkName.c_str(),m->visualNum);
            if (m->visualNum == visualNum)
            {
                break;
            }
            ++m;
        }

        // ROS_INFO("Checking Marker for link %s, visual num %i (m visual num %i)",
        //      linkName.c_str(),visualNum, m->visualNum);
        // if (m==markers.end()) ROS_INFO("No marker in map.");

        // loop over all marker entries we have for this visual number
        while ((m != markers.end()) && (m->visualNum == visualNum))
        {
            ContactPtr contact(new Contact());
            contact->linkNum = linkNum;
            contact->fingerNum = fingerNum;

            setUpSoftFrictionEdges(contact->numFrictionEdges, contact->frictionEdges);

            // ROS_INFO("Num friction edges: %i",contact.numFrictionEdges);

            // have to transform from the visual reference frame back to the link reference frame
            Eigen::Vector3d pos(m->coords);
            Eigen::Vector3d norm(m->normal);
            norm.normalize();

            // ROS_INFO_STREAM("Normal: "<<norm);

            // ROS_INFO_STREAM("Visual trans: "<<vTrans);

            EigenTransform toPos = EigenTransform::Identity();
            Eigen::Vector3d dNorm(0, 0, 1);
            // dNorm=vTrans.rotation()*dNorm;
            Eigen::Quaterniond rotNorm = Eigen::Quaterniond::FromTwoVectors(dNorm, norm);
            toPos.translate(pos);
            toPos.rotate(rotNorm);

            EigenTransform jtToPos = vTrans * toPos;
            pos = jtToPos.translation();
            contact->loc = pos;

            Eigen::Quaterniond ori(jtToPos.rotation());
            contact->ori = ori;

            // Contact normal
            norm = dNorm;
            norm.normalize();
            contact->norm = norm;
            contact->cof = coefficient;

            contacts.push_back(contact);
            lCnt->second.push_back(contact);

            // ROS_INFO_STREAM("Adding contact for link "<<linkName<<", visual "<<visualNum<<
            //    ": "<<contact<<" (mark: "<<*m<<") vis trans: "<<vTrans.translation());

            ++m;
        }
    }

    return true;
}


bool ContactsGenerator::generateContacts(const std::vector<std::string>& rootFingerJoints,
        const std::string& palmLinkName,
        const float coefficient, const MarkerSelector::MarkerMap& markers,
        const std::vector<DHParam>& dh)
{
    UrdfTraverserPtr trav = getTraverser();
    if (!trav)
    {
        ROS_ERROR("Traverser not set.");
        return false;
    }

    LinkPtr palm_link = trav->getLink(palmLinkName);
    if (!palm_link.get())
    {
        ROS_ERROR_STREAM("Palm link "<<palmLinkName<<" not found.");
        return false;
    }

    initOutStructure(trav->getModelName());

    // first, do the palm:
    std::string palmLinkNameClean = MarkerSelector::Marker::toSoBaseName(palmLinkName);
    MarkerSelector::MarkerMap::const_iterator palmM = markers.find(palmLinkNameClean);
    if (palmM != markers.end())
    {
        int linkNum = 0;
        int fingerNum = -1;
        if (!generateContactsForallVisuals(palmLinkName, linkNum, fingerNum, coefficient, palmM->second))
        {
            ROS_ERROR("Failed to generate contact for link %s", palmLinkName.c_str());
            return false;
        }
    }

    // now, do all the fingers:
    int fingerNum = -1;
    for (std::vector<std::string>::const_iterator it = rootFingerJoints.begin();
         it != rootFingerJoints.end(); ++it)
    {
        ++fingerNum;
        // ROS_INFO("Handling root finger %s",it->c_str());

        JointPtr root_joint = trav->getJoint(*it);
        if (!root_joint)
        {
            ROS_ERROR("Could not find joint %s for generating contact", it->c_str());
            trav->printJointNames("");
            return false;
        }
        std::vector<JointPtr> chain;
        bool onlyActive = true;
        if (!urdf_traverser::getDependencyOrderedJoints(*trav, chain, root_joint, false,
                                                        onlyActive) || chain.empty())
        {
            ROS_ERROR("Could not get joint chain, joint %s", root_joint->name.c_str());
            return false;
        }

        int linkNum = -1;
        for (std::vector<JointPtr>::iterator cit = chain.begin(); cit != chain.end(); ++cit)
        {
            ++linkNum;
            JointPtr chainJoint = *cit;
            std::string linkName = chainJoint->child_link_name;

            std::string linkNameClean = MarkerSelector::Marker::toSoBaseName(linkName);
            MarkerSelector::MarkerMap::const_iterator markerIt = markers.find(linkNameClean);
            if (markerIt == markers.end())
            {
                // no markers defined for this link
                // ROS_INFO("No markers defined for this link: %s",linkName.c_str());
                continue;
            }

            // ROS_INFO("Marker defined for link %s",linkName.c_str());

            if (!generateContactsForallVisuals(linkName, linkNum, fingerNum, coefficient, markerIt->second))
            {
                ROS_ERROR("Failed to generate contact for link %s", linkName.c_str());
                return false;
            }
        }
    }

    if (!transformToDHReferenceFrames(dh))
    {
        ROS_ERROR("Could not transform the contacts to DH parameter space");
        return false;
    }
    scaleContacts(getScaleFactor());
    return true;
}


std::string ContactsGenerator::getContactsFileContent(const std::string& robotName)
{
    std::stringstream str;

    str << "<?xml version=\"1.0\" ?> " << std::endl; 
    str << "<virtual_contacts>" << std::endl;
    str << "<robot_name>";
    str << robotName;
    str << "</robot_name>" << std::endl;

    // Number of contacts
    int contSize = contacts.size();
    //ROS_INFO_STREAM("Number of contacts: "<<contSize);

    for (std::vector<ContactPtr>::const_iterator cit = contacts.begin(); cit != contacts.end(); ++cit)
    {
        ContactPtr c = *cit;

        //ROS_INFO_STREAM("Contact: "<<*c);
        str << "<virtual_contact>" << std::endl;
        str << "<finger_number>";
        str << c->fingerNum;
        str << "</finger_number>" << std::endl;

        str << "<link_number>";
        str << c->linkNum;
        str << "</link_number>" << std::endl;

        str << "<num_friction_edges>";
        str << c->numFrictionEdges;
        str << "</num_friction_edges>" << std::endl;

        str << "<friction_edges>" << std::endl;

        // The friction edges. One line for each friction edge (total number defined in 2) ).
        //  6 * <number-friction-edges> values.
        for (unsigned int i = 0; i < c->numFrictionEdges; ++i)
        {
            str << "<friction_edge>";
            for (unsigned int j = 0; j < 6; ++j)
            {
                str << c->frictionEdges[i * 6 + j] << " ";
            }
            str << "</friction_edge>" << std::endl;
        }
        
        str << "</friction_edges>" << std::endl;

        str << "<location>" << std::endl;
        str << static_cast<float>(c->loc.x()) << " "
            << static_cast<float>(c->loc.y()) << " "
            << static_cast<float>(c->loc.z());
        str << "</location>" << std::endl;

        str << "<rotation>" << std::endl;
        str << static_cast<float>(c->ori.w()) << " "
            << static_cast<float>(c->ori.x()) << " "
            << static_cast<float>(c->ori.y()) << " "
            << static_cast<float>(c->ori.z());
        str << "</rotation>" << std::endl;

        // Frame location (mostly()) equals virtual contact location)
        str << "<translation>";
        str << static_cast<float>(c->loc.x()) << " "
            << static_cast<float>(c->loc.y()) << " "
            << static_cast<float>(c->loc.z());
        str << "</translation>" << std::endl;

        str << "<normal>";
        str << static_cast<float>(c->norm.x()) << " "
            << static_cast<float>(c->norm.y()) << " "
            << static_cast<float>(c->norm.z());
        str << "</normal>" << std::endl;

        str << "<sCof>";
        str << static_cast<float>(c->cof);
        str << "</sCof>" << std::endl;;
        str << "</virtual_contact>" << std::endl;
    }
    str << "</virtual_contacts>" << std::endl;

    return str.str();
}


SoNode * ContactsGenerator::getAxesAsInventor(
        const LinkPtr& from_link, 
        const std::vector<DHParam>& dh,
        float _axesRadius, float _axesLength,
        bool linkIsRoot)
{
    // ROS_INFO_STREAM("Get axes of "<<from_link->name<<" (parent joint "<<from_link->parent_joint->name<<")");
        
    EigenTransform transform;
    if (!linkIsRoot)
    {
        JointPtr parentJoint = from_link->parent_joint;
        DHParam jointDH;
        if (!getDHParam(parentJoint->name, dh, jointDH))
        {
            ROS_ERROR_STREAM("Could not get DH parameter for "<<parentJoint->name);
            return NULL;
        }

        ROS_INFO_STREAM("* Using DH params "<<jointDH);

        transform.setIdentity();
        Eigen::Vector3d x(1,0,0); 
        Eigen::Vector3d y(0,1,0); 
        Eigen::Vector3d z(0,0,1);
        transform.rotate(Eigen::AngleAxisd(jointDH.theta,z));
        transform.translate(z*jointDH.d);
        transform.rotate(Eigen::AngleAxisd(jointDH.alpha,x));
        transform.translate(x*jointDH.r);
    }
    else
    {
         ROS_INFO_STREAM("* Transforming as root node ");
    }

    SoSeparator * allVisuals = new SoSeparator();
    allVisuals->ref(); 
    // Eigen::Vector3d pos(0,0,0);
    // urdf2inventor::addSphere(allVisuals, pos, _axesRadius, 1,0,0);            
    addLocalAxes(from_link, allVisuals, false, _axesRadius, _axesLength);

    UrdfTraverserPtr trav = getTraverser();
    if (!trav)
    {
        ROS_ERROR("Traverser not set.");
        return NULL;
    }

    // recurse to children
    for (std::vector<JointPtr>::const_iterator pj = from_link->child_joints.begin();
            pj != from_link->child_joints.end(); pj++)
    {
        JointPtr joint = *pj;
        LinkPtr childLink = trav->getLink(joint->child_link_name);
        SoNode * childNode = getAxesAsInventor(childLink, dh, _axesRadius, _axesLength, false);
        if (!childNode)
        {
            ROS_ERROR_STREAM("Could not get child node for "<<childLink->name);
            return NULL;
        }
        if (linkIsRoot)
        {
            transform = urdf_traverser::getTransform(joint);
        }
        urdf2inventor::addSubNode(childNode, allVisuals, transform);
    }

    return allVisuals;
}

bool ContactsGenerator::getDHParam(const std::string jointName, const std::vector<DHParam>& dh, DHParam& jointDH)
{
    for (std::vector<DHParam>::const_iterator it=dh.begin(); it!=dh.end(); ++it)
    {
        if (it->joint->name == jointName)
        {
            jointDH=*it;
            return true;
        }
    }
    return false;
}

bool ContactsGenerator::generateContactsWithViewer(const std::vector<std::string>& fingerRoots,
        const std::string& palmLinkName, float standard_coefficient,
        const std::vector<DHParam>& dh,
        bool _displayAxes, bool _axesFromDH,
        float _axesRadius, float _axesLength,
        const EigenTransform& addVisualTransform,
        bool facesCCW)
{
    UrdfTraverserPtr trav = getTraverser();
    if (!trav)
    {
        ROS_ERROR("Traverser not set.");
        return false;
    }

    LinkPtr palm = trav->getLink(palmLinkName);
    if (!palm.get())
    {
        ROS_ERROR_STREAM("Could not find palm link "<<palmLinkName);
        return false;
    }

    if (!prepareModelForDenavitHartenberg(palmLinkName))
    {
        ROS_ERROR("Could not prepare for DH parameter compatible URDF model.");
        return false;
    }
    
    SoNode * node = getAsInventor(palmLinkName, false, 
        _displayAxes && !_axesFromDH, _axesRadius, _axesLength, addVisualTransform, NULL);

    Eigen::Vector3d minBB, maxBB;
    urdf2inventor::getBoundingBox(node,minBB,maxBB);
    Eigen::Vector3d diagonal = maxBB-minBB;
    float markerFactor = 0.002; //factor of diagonal length to define the markers radius

    bool success = true;
    MarkerSelector markerSelector(diagonal.norm()*markerFactor, facesCCW);
    //markerSelector.init("Marker selector");
    if (!node)
    {
        ROS_ERROR("Could not get inventor node");
        return false;
    }

    if (_displayAxes && _axesFromDH)
    {
        SoNode * dhAxes = getAxesAsInventor (palm, dh, _axesRadius, _axesLength, true);
        if (!dhAxes)
        {
            ROS_ERROR("Could not the DH axes, so they won't be displayed");
        }
        else
        {
            SoSeparator * sep = dynamic_cast<SoSeparator*>(node);
            if (!sep)
            {
                ROS_ERROR("Inventor node parent is not a separator");
            }
            else
            {
                sep->addChild(dhAxes);
            }
        }
    }

    ROS_INFO_STREAM("Model inventor files loaded, now loading into viewer...");
    markerSelector.init("Marker selector");
    markerSelector.loadModel(node);
    markerSelector.runViewer();

    MarkerSelector::MarkerMap markers = markerSelector.getMarkers();
    ROS_INFO("Number of contacts: %lu",markers.size());
    ROS_INFO("Markers: %s",markerSelector.toString().c_str());

    // if there was a visual transform (addVisualTransform) we also need
    // to correct the normals which are now in visual coordinate space
    for (MarkerSelector::MarkerMap::iterator lit=markers.begin(); lit!=markers.end(); ++lit)
    {
        for (std::vector<MarkerSelector::Marker>::iterator mit=lit->second.begin(); mit!=lit->second.end(); ++mit)
        {
            MarkerSelector::Marker& m = *mit;
            m.coords = addVisualTransform*m.coords;
            m.normal = addVisualTransform*m.normal;
        }
    }

    if (!generateContacts(fingerRoots, palmLinkName, standard_coefficient, markers, dh))
    {
        ROS_ERROR("could not generate contacts");
        return false;
    }
    return true;
}