Urdf2Inventor.h

Go to the documentation of this file.

#ifndef URDF2INVENTOR_URDF2INVENTOR_H
#define URDF2INVENTOR_URDF2INVENTOR_H
// Copyright Jennifer Buehler

//-----------------------------------------------------
#include <urdf2inventor/ConversionResult.h>
#include <urdf2inventor/MeshConvertRecursionParams.h>

#include <urdf_traverser/UrdfTraverser.h>

#include <baselib_binding/SharedPtr.h>

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

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

#include <Inventor/nodes/SoSeparator.h>


namespace urdf2inventor
{

class Urdf2Inventor
{
public:
    // the .iv files are represented as strings
    typedef std::string MeshFormat;
    typedef ConversionResult<MeshFormat> ConversionResultT;
    typedef baselib_binding::shared_ptr<ConversionResultT>::type ConversionResultPtr;
    typedef baselib_binding::shared_ptr<ConversionParameters>::type ConversionParametersPtr;

    typedef MeshConvertRecursionParams<MeshFormat> MeshConvertRecursionParamsT;
    typedef baselib_binding::shared_ptr<MeshConvertRecursionParamsT>::type MeshConvertRecursionParamsPtr;

    typedef baselib_binding::shared_ptr<urdf_traverser::UrdfTraverser>::type UrdfTraverserPtr;
    typedef baselib_binding::shared_ptr<const urdf_traverser::UrdfTraverser>::type UrdfTraverserConstPtr;

    typedef urdf_traverser::EigenTransform EigenTransform;
    typedef urdf_traverser::LinkPtr LinkPtr;
    typedef urdf_traverser::LinkConstPtr LinkConstPtr;
    typedef urdf_traverser::JointPtr JointPtr;
    typedef urdf_traverser::JointConstPtr JointConstPtr;
    //typedef urdf_traverser::;

    // output file format to convert the meshes to
    static std::string OUTPUT_EXTENSION;

    // within the output directory specified in the node, another directory is going to be created
    // to contain the mesh files. The name of this directory can be specified here.
    static std::string MESH_OUTPUT_DIRECTORY_NAME;
    // within the output directory specified in the node, another directory is going to be created
    // to contain the texture files. The name of this directory can be specified here.
    static std::string TEX_OUTPUT_DIRECTORY_NAME;

    explicit Urdf2Inventor(const UrdfTraverserPtr& traverser,
                           float _scaleFactor = 1):
        urdf_traverser(traverser),
        scaleFactor(_scaleFactor),
        isScaled(false)
    {
        assert(urdf_traverser.get());
    }

    ~Urdf2Inventor()
    {
    }

    bool joinFixedLinks(const std::string& from_link);

    bool allRotationsToAxis(const std::string& fromLinkName, const Eigen::Vector3d& axis);


    // SoNode * loadAndGetAsInventor(const std::string& filename, const std::string fromLink="", bool useScaleFactor = true);

    SoNode * getAsInventor(const std::string& fromLink, bool useScaleFactor,
                           bool addAxes, float axesRadius, float axesLength,
                           const EigenTransform& addVisualTransform,
                           std::set<std::string> * textureFiles);

    bool writeAsInventor(const std::string& outputFilename,  const std::string& fromLink /*= ""*/,
                         bool useScaleFactor /*= true*/, const EigenTransform& addVisualTransform,
                         bool _addAxes = false, float _axesRadius = 0.003, float _axesLength = 0.015);

    ConversionResultPtr loadAndConvert(const std::string& urdfFilename,
                                       bool joinFixed,
                                       const ConversionParametersPtr& params);


    bool loadModelFromFile(const std::string& urdfFilename);

    ConversionParametersPtr getBasicConversionParams(const std::string& rootLink,
            const std::string& material,
            const EigenTransform& addVisualTransform)
    {
        return ConversionParametersPtr(new ConversionParameters(rootLink, material, addVisualTransform));
    }

    virtual ConversionResultPtr convert(const ConversionParametersPtr& params,
                                        const MeshConvertRecursionParamsPtr& meshParams=MeshConvertRecursionParamsPtr());

    bool printModel();

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

    void printJointNames(const std::string& fromLink);

    bool getJointNames(const std::string& fromLink, const bool skipFixed, std::vector<std::string>& result);

    void cleanup();

protected:

    UrdfTraverserPtr getTraverser()
    {
        return urdf_traverser;
    }

    UrdfTraverserConstPtr readTraverser() const
    {
        return urdf_traverser;
    }


    virtual ConversionResultPtr preConvert(const ConversionParametersPtr& params);

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

    bool scale();

    inline float getScaleFactor() const
    {
        return scaleFactor;
    }

    void addLocalAxes(const LinkConstPtr& link, SoSeparator * addToNode, bool useScaleFactor,
                      float _axesRadius, float _axesLength) const;

    EigenTransform getTransform(const LinkPtr& from_link,  const JointPtr& to_joint);

private:

    SoNode * getAsInventor(const LinkPtr& from_link, bool useScaleFactor,
                           bool _addAxes, float _axesRadius, float _axesLength,
                           const EigenTransform& addTransform,
                           std::set<std::string> * textureFiles);

    bool writeInventorFile(SoNode * node, const std::string& filename);

    bool writeAsInventor(const std::string& outFilename, const LinkPtr& from_link,
                         bool useScaleFactor /*= true*/, const EigenTransform& addTransform,
                         bool _addAxes = false, float _axesRadius = 0.003, float _axesLength = 0.015);


    UrdfTraverserPtr urdf_traverser;

    // The graspit model might ahve to be scaled compared to the urdf model, this is the scale factor which does that.
    float scaleFactor;
    bool isScaled;
};

}  //  namespace urdf2inventor
#endif   // URDF2INVENTOR_URDF2INVENTOR_H