SimoxObjectWrapper.h

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package    MemoryX::Helpers
* @author     ALexey Kozlov ( kozlov at kit dot edu)
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
// boost
#include <filesystem>
#include <memory>
 
// Simox-VirtualRobot
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/XML/RobotIO.h>
 
// MemoryX
#include <MemoryX/core/GridFileManager.h>
#include <MemoryX/core/entity/AbstractEntityWrapper.h>
 
namespace memoryx::EntityWrappers
{
 
    /**
     * SimoxObjectWrapper offers a simplified access to the Simox ManipulationObject
     * (i.e visualization, collision model, grasps) stored in the attributes of MemoryX entity.
     *
     */
    class SimoxObjectWrapper : public AbstractFileEntityWrapper
    {
    public:
        enum UncertaintyVisuType
        {
            eEllipse,
            eHeatMap,
            eHeatSurface
        };
 
        /**
         * Constructs new SimoxObjectWrapper.
         *
         * @param gfm        GridFileManager to store/load files
         * @param loadMode   eFull or eCollisionModel. If eFull, then both, visualization and collision model is loaded.
         * If eCollisionModel then only the collision model will be loaded.
         *
         */
        SimoxObjectWrapper(
            const GridFileManagerPtr& gfm,
            VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
        ~SimoxObjectWrapper() override;
 
        /**
         * Retrieve visualization model.
         *
         * Corresponding files will be fetched and cached locally, if needed.
         */
        VirtualRobot::VisualizationNodePtr getVisualization() const;
 
        /**
         * Retrieve collision model
         *
         * Corresponding files will be fetched and cached locally, if needed.
         */
        VirtualRobot::CollisionModelPtr getCollisionModel() const;
 
        /**
         * Retrieve the complete Simox ManipulationObject
         *
         * Corresponding files will be fetched and cached locally, if needed.
         */
        VirtualRobot::ManipulationObjectPtr getManipulationObject() const;
 
        /**
         * Retrieve the Simox MainpulationObjects file name in local cache. File is cached
         * when this method is called.
         *
         */
        std::string getManipulationObjectFileName() const;
 
        /**
         * Store Simox ManipulationObject in the wrapped entity.
         *
         * Namely, this function does the following:
         * \li 1. Clears previous manipulation object, if any. Corresponding files are deleted from GridFS as well.
         * \li 2. Saves all files in the GridFS
         * \li 2.1. Manipulation object XML file
         * \li 2.2. Visualization model file (normally, *.iv)
         * \li 2.3. Collision model file (normally, *.iv)
         * \li 2.4. Any textures used by 2.2. and 2.3.
         * \li 3. Stores references to the files from 2. in corresponding attributes of the wrapped enity
         *
         *  @param  mo    manipulation object
         *  @param  filesDBName Mongo database name to store additional files into
         *
         * @return MongoDB Id of the stored object
         */
        std::string setAndStoreManipulationObject(const VirtualRobot::ManipulationObjectPtr mo,
                                                  const std::string& filesDBName);
 
        /**
         * Set Simox ManipulationObject in the wrapped entity. The object is not stored to GridFS.
         *
         *  @param  mo    manipulation object
         */
        void setManipulationObject(const VirtualRobot::ManipulationObjectPtr mo);
 
        /**
         *  Create Simox ManipulationObject from an XML file and store it in the wrapped entity.
         *  See setManipulationObject for details.
         *
         *  @param  xmlFName    name of XML file with manipulation object definition
         *  @param  filesDBName Mongo database name to store additional files into
         */
        void setAndStoreManipulationFile(const std::string& xmlFName,
                                         const std::string& filesDBName);
 
        /**
         *  Create Simox ManipulationObject from an XML file, but do not store it in the wrapped entity.
         *  See setManipulationObject for details.
         *
         *  @param  xmlFName    name of XML file with manipulation object definition
          */
        void setManipulationFile(const std::string& xmlFName);
 
        /**
         *  Create Simox ManipulationObject from separate .iv models and store it in the wrapped entity.
         *  See setManipulationObject for details.
         *
         *  @param  ivFNameVis          name of .iv file for visualization model
         *  @param  ivFNameCollision    name of .iv file for collision model
         *  @param  filesDBName Mongo   database name to store additional files into
         */
        void setAndStoreModelIVFiles(const std::string& ivFNameVis,
                                     const std::string& ivFNameCollision,
                                     const std::string& filesDBName);
 
        /**
         *  Create Simox ManipulationObject from separate .iv models, but do not store it in the wrapped entity.
         *  See setManipulationObject for details.
         *
         *  @param  ivFNameVis          name of .iv file for visualization model
         *  @param  ivFNameCollision    name of .iv file for collision model
          */
        void setModelIVFiles(const std::string& ivFNameVis, const std::string& ivFNameCollision);
 
        /**
         * Update the Simox object using attribute values (position, orientation etc) from the entity,
         * but reusing the existing visualization. This method meant to be used with subsequent observations of
         * the same entity. For new entity instances, create new SimoxObjectWrapper object instead.
         *
         * @param  entity          entity to get attribute values from
         */
        void updateFromEntity(const EntityBasePtr& entity);
 
 
        /**
         *  Update object visualization using the current values of pose and position uncertainty from the wrapped entity
         */
        void refreshVisuPose();
 
        /**
         *  Set visualization type for position uncertainty.
         *
         *  @param visuType one of: eEllipse - ellipsoids, full 3d uncertainty shown
         *                          eHeatMap - flat colored map, XY uncertainty only
         *                          eHeatSurface - 3d surface,
         */
        void setUncertaintyVisuType(UncertaintyVisuType visuType);
 
        /**
         *  Set parameters for uncertainty visualization
         *
         *  @param ellipseRadiusInSigmas
         *  @param heatmapMinVariance
         *  @param heatmapDensityThreshold
         *
         */
        void setUncertaintyVisuParams(float ellipseRadiusInSigmas,
                                      float heatmapMinVariance,
                                      float heatmapDensityThreshold,
                                      int heatmapGridSize);
 
        /**
         * Clears the Simox ManipulationObject and optionally removes the corresponding files from GridFS.
         *
         * @param removeFiles   whether GridFS files (.iv models, textures, MO XML) should be deleted as well
         */
        void clear(bool removeFiles = true);
 
        /**
         * @brief GetAllFilenames walks \p node and appends absolute paths of all found SoFile, SoImage, and SoTexture2 node filenames to \p storeFilenames.
         * @param node SoNode which is analyzed
         * @param storeFilenames vector to which the found filenames are appended
         * @param origFile name of the original file used to load \p node (can be used to retrieve absolute paths like texture filenames which are stored as relative paths in Coin3D).
         */
        static void GetAllFilenames(SoNode* node,
                                    std::vector<std::string>& storeFilenames,
                                    const std::string& origFile = "");
 
        /**
         * @brief GetAbsolutePath prepend \p filename with \p origFile and append it to \p storeFilenames (does nothing if \p filename is NULL)
         * @param filename SbString of the filename to check
         * @param origFile string containing the original filename
         * @param storeFilenames vector filename strings to which the result should be appended
         */
        static std::string GetAbsolutePath(SbString filename, const std::string& origFile);
 
        /**
         * @brief FindIvTextures scans the given \p ivFName for texture files and returns them in the \p textures string list
         * @param ivFName path to Inventor file to scan for textures
         * @param textures vector of strings containing the paths to texture files
         */
        static void FindIvTextures(const std::string& ivFName, NameList& textures);
 
        /**
         * Returns the orientation of the object that it should have when it is being put down e.g. onto the table by the robot.
         */
        Eigen::Vector3f getPutdownOrientationRPY();
        /**
         * Sets the orientation of the object that it should have when it is being put down e.g. onto the table by the robot.
         */
        void setPutdownOrientationRPY(Eigen::Vector3f& rpy);
        Ice::ObjectPtr ice_clone() const override;
 
    private:
        VirtualRobot::CoinVisualizationFactoryPtr coinVisFactory;
        mutable VirtualRobot::ManipulationObjectPtr simoxObject;
        VirtualRobot::RobotIO::RobotDescription loadMode;
        std::filesystem::path cachePath;
        std::string moTempFile;
 
        UncertaintyVisuType uncertaintyVisuType;
        float ellipseRadiusInSigmas;
        float heatmapMinVariance;
        float heatmapDensityThreshold;
        int heatmapGridSize;
 
        void loadSimoxObject() const;
 
        std::string cacheAttributeFile(const std::string& attrName,
                                       bool preserveOriginalFName = false) const;
        bool cacheAttributeFiles(const std::string& attrName,
                                 bool preserveOriginalFName = false) const;
 
        VirtualRobot::ManipulationObjectPtr
        loadManipulationObjectFile(const std::string& xmlFName) const;
        VirtualRobot::ManipulationObjectPtr
        createManipulationObjectFromIvFiles(const std::string& objName,
                                            const std::string& ivFNameVis,
                                            const std::string& ivFNameCollision) const;
 
        std::string storeManipulationObject(const VirtualRobot::ManipulationObjectPtr mo,
                                            const std::string& filesDBName);
        void storeEntityIVFiles(const std::string& visuFName,
                                const std::string& collisionFName,
                                const std::string& filesDBName,
                                bool processTextures = true);
 
        VirtualRobot::VisualizationNodePtr
        getUncertaintyEllipsesVisu(GaussianMixtureDistributionBasePtr posGM,
                                   const Eigen::Vector3f& objPos,
                                   const VirtualRobot::ColorMap& cmap,
                                   float transparency);
        VirtualRobot::VisualizationNodePtr
        getUncertaintyHeatVisu(GaussianMixtureDistributionBasePtr posGM,
                               const Eigen::Vector3f& objPos,
                               bool heatSurface,
                               const VirtualRobot::ColorMap& cmap);
 
        float evaluate2DGaussian(const Eigen::Vector2f& point,
                                 const Eigen::Vector2f& mean,
                                 const Eigen::Matrix2f& covInv,
                                 float covDetSqrtInv);
        float evaluate3DGaussian(const Eigen::Vector3f& point,
                                 const Eigen::Vector3f& mean,
                                 const Eigen::Matrix3f& covInv,
                                 float covDet);
 
        VirtualRobot::ColorMap getUncertaintyColorMap();
        float getUncertaintyEllipseTransparency();
    };
 
    using SimoxObjectWrapperPtr = IceInternal::Handle<SimoxObjectWrapper>;
 
} // namespace memoryx::EntityWrappers