Urdf2Graspit.h

Go to the documentation of this file.

#ifndef URDF2GRASPIT_URDF2GRASPIT_H
#define URDF2GRASPIT_URDF2GRASPIT_H
// Copyright Jennifer Buehler

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

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

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

#include <baselib_binding/SharedPtr.h>
#include <urdf2graspit/Urdf2GraspItBase.h>

#include <urdf2graspit/DHParam.h>
#include <urdf2graspit/ConversionResult.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 Urdf2GraspIt: public Urdf2GraspItBase
{
public:
    typedef urdf2graspit::ConversionResult GraspItConversionResult;
    typedef baselib_binding::shared_ptr<GraspItConversionResult>::type GraspItConversionResultPtr;

    typedef urdf2graspit::ConversionParameters GraspItConversionParameters;
    typedef baselib_binding::shared_ptr<GraspItConversionParameters>::type GraspItConversionParametersPtr;


    explicit Urdf2GraspIt(UrdfTraverserPtr& traverser, float _scaleFactor = 1000, bool _negateJointMoves = false):
        Urdf2GraspItBase(traverser, _scaleFactor),
        negateJointMoves(_negateJointMoves),
        isDHScaled(false),
        dhTransformed(false)
    {
        urdf2inventor::Urdf2Inventor::MESH_OUTPUT_DIRECTORY_NAME = "iv";
    }

    ~Urdf2GraspIt()
    {
    }

    ConversionResultPtr processAll(const std::string& urdfFilename,
                                 const std::string& rootLinkName,
                                 const std::vector<std::string>& fingerRootNames,
                                 const std::string& material/* = "plastic"*/,
                                 const EigenTransform& addVisualTransform);


    bool getDHParams(std::vector<DHParam>& dhparams, const std::string& fromLinkName) const;


    bool toDenavitHartenberg(const std::string& fromLink);

private:

    bool checkConversionPrerequisites(const GraspItConversionParametersPtr& param) const;

    virtual ConversionResultPtr preConvert(const ConversionParametersPtr& params);
    virtual ConversionResultPtr postConvert(const ConversionParametersPtr& rootLink, ConversionResultPtr& result);

    bool getDHParams(std::vector<DHParam>& dhparameters, const JointConstPtr& joint,
                     const EigenTransform& parentWorldTransform,
                     const Eigen::Vector3d& parentX, const Eigen::Vector3d& parentZ,
                     const Eigen::Vector3d& parentPos, bool asRootJoint,
                     EigenTransform& parentWorldTransformDH) const;

    bool getDHParams(std::vector<DHParam>& dhparameters, const LinkConstPtr& from_link) const;


    void scaleParams(std::vector<DHParam>& dh, double scale_factor) const;

    void printParams(const std::vector<DHParam>& dh) const;

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


    void getLimits(const urdf::Joint& j, float& min, float& max);

    void getJointMoves(const urdf::Joint& j, float& velocity, float& effort);


    bool getXML(const std::vector<DHParam>& dhparams, const std::vector<std::string>& rootFingerJoints,
                const std::string& palmLinkName, const std::string * eigenXML,
                const std::string * contactsVGR, const std::string& mesh_pathprepend, std::string& result);

    // Returns if this is a revoluting joint in the URDF description
    inline bool isRevoluting(const JointPtr& joint) const
    {
        return (joint->type == urdf::Joint::REVOLUTE) || (joint->type == urdf::Joint::CONTINUOUS);
    }

    // Returns if this is a continuous joint in the URDF description
    inline bool isContinuous(const JointPtr& joint) const
    {
        return (joint->type == urdf::Joint::CONTINUOUS);
    }

    // Returns if this is a prismatic joint in the URDF description
    inline bool isPrismatic(const JointPtr& joint) const
    {
        return (joint->type == urdf::Joint::PRISMATIC);
    }

/*    int getChildJoint(const JointPtr& joint, JointPtr& child);
    LinkPtr getChildLink(const JointPtr& joint);
    JointPtr getParentJoint(const JointPtr& joint);
*/


    void toGlobalCoordinates(const EigenTransform& transform,
                             const Eigen::Vector3d& input, Eigen::Vector3d& output);

    void getGlobalCoordinates(const JointConstPtr& joint,
                              const EigenTransform& parentWorldTransform,
                              Eigen::Vector3d& rotationAxis, Eigen::Vector3d& position) const;


    bool getDHTransform(const JointPtr& joint, const std::vector<DHParam>& dh, EigenTransform& result) const;

    std::string getWorldFileTemplate(const std::string& robotName,
                                     const std::vector<DHParam>& dhparams,
                                     const std::string& prependpath);

    // bool coordsConvert(const JointPtr& joint, const JointPtr& root_joint, const std::vector<DHParam>& dh,
    //                   bool dhToKin, EigenTransform& result);

    bool negateJointMoves;
    bool isDHScaled;
    bool dhTransformed;

    std::vector<DHParam> dh_parameters;
};

}  //  namespace urdf2graspit
#endif   // URDF2GRASPIT_URDF2GRASPIT_H