ContactsGenerator.h

Go to the documentation of this file.

#ifndef URDF2GRASPIT_CONTACTSGENERATOR_H
#define URDF2GRASPIT_CONTACTSGENERATOR_H
// Copyright Jennifer Buehler

//-----------------------------------------------------
#include <urdf/model.h>

#include <iostream>
#include <string>
#include <map>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <urdf2graspit/Urdf2GraspItBase.h>
#include <baselib_binding/SharedPtr.h>
#include <urdf2graspit/DHParam.h>
#include <urdf2graspit/MarkerSelector.h>
#include <urdf2graspit/OutputStructure.h>

// epsilon value to use when comparing whether two axes are the same
// (difference of angle between axes)
#define U2G_EPSILON 1e-07

namespace urdf2graspit
{

class ContactsGenerator: public Urdf2GraspItBase
{
public:
    explicit ContactsGenerator(UrdfTraverserPtr& traverser, float _scaleFactor = 1000):
        Urdf2GraspItBase(traverser, _scaleFactor),
        isContactsScaled(false)
    {
    }

    ~ContactsGenerator()
    {
    }

    bool generateContacts(const std::vector<std::string>& rootFingerJoints, const std::string& palmLinkName,
                          const float coefficient, const markerselector::MarkerSelector::MarkerMap& markers,
                          const std::vector<DHParam>& dh);

    bool 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);

    std::string getContactsFileContent(const std::string& robotName);


protected:
    class Contact
    {
    public:
        Contact(): numFrictionEdges(-1), fingerNum(-1), linkNum(-1), cof(0) {}
        Contact(const Contact& o):
            numFrictionEdges(o.numFrictionEdges), frictionEdges(o.frictionEdges),
            fingerNum(o.fingerNum), linkNum(o.linkNum),
            loc(o.loc), ori(o.ori), norm(o.norm), cof(o.cof) {}

        friend std::ostream& operator<<(std::ostream& o, const Contact& c)
        {
            o << c.linkNum << ", " << c.fingerNum << ": " << c.loc;
            return o;
        }

        // Number of friction edges
        unsigned int numFrictionEdges;
        // Friction edges (6*numFrictionEdges doubles)
        // The 6 values specify friction cone boundary wrenches.
        // See more documentation in function ContactsGenerator::setUpFrictionEllipsoid().
        std::vector<double> frictionEdges;
        // Finger number
        int fingerNum;
        // Link number
        int linkNum;
        // Contact (frame) location
        Eigen::Vector3d loc;
        // Contact (frame) orientation
        Eigen::Quaterniond ori;
        // Contact normal (facing away from link)
        Eigen::Vector3d norm;
        // Contact friciton coefficient
        float cof;
    };
    typedef baselib_binding::shared_ptr<Contact>::type ContactPtr;

    bool transformToDHReferenceFrames(const std::vector<DHParam>& dh);

    void applyTransformToContacts(LinkPtr& link, const EigenTransform& trans, bool preMult);


    static bool getDHParam(const std::string jointName, const std::vector<DHParam>& dh, DHParam& jointDH);

    SoNode * getAxesAsInventor(const LinkPtr& from_link,
            const std::vector<DHParam>& dh,
            float _axesRadius, float _axesLength,
            bool linkIsRoot);
private:

    void scaleContacts(double scale_factor);

    // Helper function for generateContacts()
    bool generateContactsForallVisuals(const std::string& linkName,
                                       const int linkNum, const int fingerNum,
                                       const float coefficient,
                                       const std::vector<markerselector::MarkerSelector::Marker>& markers);
    bool isContactsScaled;

    std::vector<ContactPtr> contacts;
    std::map<std::string, std::vector<ContactPtr> > linkContacts;
};

}  //  namespace urdf2graspit
#endif   // URDF2GRASPIT_CONTACTSGENERATOR_H