SimoxCSpace.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    RobotComponents
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl.txt
 *             GNU General Public License
 */
#pragma once
 
#include <array>
#include <atomic>
 
#include <Eigen/Core>
 
#include <VirtualRobot/CollisionDetection/CDManager.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/SceneObjectSet.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
 
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/RobotPool.h>
#include <RobotAPI/libraries/core/Trajectory.h>
 
#include <RobotComponents/interface/components/MotionPlanning/CSpace/SimoxCSpace.h>
 
#include "CSpace.h"
#include "GlobalCache.h"
#include <MemoryX/core/GridFileManager.h>
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MotionPlanning/CSpace/CSpace.h>
 
namespace armarx
{
    class SimoxCSpace;
    /**
     * @brief An ice handle for a SimoxCSpace.
     */
    using SimoxCSpacePtr = IceInternal::Handle<SimoxCSpace>;
 
    /**
     * @brief The SimoxCSpace contains a set of stationary obstacles and an agent.
     * The agent can have attached objects.
     * The objects are loaded from a \ref memoryx::CommonStorageInterfacePrx.
     * The number of dimensions is determined by the agent's kinematic chains.
     * @see armarx::SimoxCSpaceBase
     * @see RobotComponents-Tutorial-SimoxCSpace
     */
    class SimoxCSpace : virtual public SimoxCSpaceBase, virtual public CSpace
    {
    public:
        using CSpace::isCollisionFree;
 
        /**
         * @brief ctor
         * @param commonStoragePrx The common storage containing the objects.
         * @param loadVisualizationModel Whether to load the visualization model. (true, if you want to display the object. false if you want to plan)
         */
        SimoxCSpace(memoryx::CommonStorageInterfacePrx commonStoragePrx,
                    bool loadVisualizationModel = false,
                    float stationaryObjectMargin = 0.0f);
 
        //from setting the cspace up
        /**
         * @brief Adds a stationary object to the cspace.
         * @param obj The object to add.
         */
        void addStationaryObject(const StationaryObject& obj,
                                 const Ice::Current& = Ice::emptyCurrent) override;
        virtual void setStationaryObjects(const StationaryObjectList& objList);
        /**
         * @brief Adds a stationary agent to the cspace.
         * @param obj The object to add.
         */
        void setAgent(const AgentPlanningInformation& agentInfo,
                      const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief Adds all objects except the ones with ids specified in the parameter excludedInstancesIds from the working memory to the cspace.
         * @param workingMemoryPrx The working memory.
         * @param excludedInstancesIds the ids of the instances in teh working memory that should not be added
         */
        virtual void addObjectsFromWorkingMemory(
            memoryx::WorkingMemoryInterfacePrx workingMemoryPrx,
            const std::vector<std::string>& excludedInstancesIds = std::vector<std::string>());
 
        /**
         * @brief Load objects from WorkingMemory and puts them into the mesh cache.
         * @param workingMemoryPrx
         */
        static SimoxCSpacePtr
        PrefetchWorkingMemoryObjects(memoryx::WorkingMemoryInterfacePrx workingMemoryPrx,
                                     memoryx::CommonStorageInterfacePrx commonStoragePrx,
                                     RobotStateComponentInterfacePrx rsc);
 
        virtual void addPlanarObject(const std::vector<Eigen::Vector3f>& convexHull);
 
        //using it
        /**
         * @brief Initializes the collision test.
         */
        void initCollisionTest(const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @return A clone of this cspace.
         */
        CSpaceBasePtr
        clone(const Ice::Current& = Ice::emptyCurrent) override
        {
            return clone(false);
        }
 
        /**
         * @param loadVisualizationModel Whether the clone should load the visualization model.
         * @return A clone of this cspace.
         */
        virtual CSpaceBasePtr clone(bool loadVisualizationModel);
 
        virtual Saba::CSpaceSampledPtr createSimoxCSpace() const;
 
        /**
         * @param cfg The configuration to check.
         * @return Whether the given configuration is collision free.
         */
        bool isCollisionFree(const ::std::pair<const Ice::Float*, const Ice::Float*>& cfg,
                             const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @return The cspace's dimension bounds.
         */
        FloatRangeSeq getDimensionsBounds(const Ice::Current& = Ice::emptyCurrent) const override;
 
        /**
         * @return The cspace's dimensionality.
         */
        Ice::Long
        getDimensionality(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return agentJoints.size();
        }
 
        /**
         * @brief Sets a configuration to check for. (can be used to display a configuration)
         * @param it A pointer to the configuration.
         */
        virtual void
        setConfig(const std::vector<float> cfg)
        {
            assert(cfg.size() == static_cast<std::size_t>(getDimensionality()));
            setConfig(cfg.data());
        }
 
        virtual void setConfig(const float*& it);
 
        virtual void
        setConfig(const float*&& it) //deals with vector<float>::data()
        {
            setConfig(it); //calls the ref version
        }
 
        //getter
        /**
         * @return The stationary objects.
         */
        const StationaryObjectList&
        getStationaryObjects() const
        {
            return stationaryObjects;
        }
 
        const AgentPlanningInformation&
        getAgent() const
        {
            return agentInfo;
        }
 
        /**
         * @return The set of stationary objects' scene objects.
         */
        const VirtualRobot::SceneObjectSetPtr&
        getStationaryObjectSet() const
        {
            return stationaryObjectSet;
        }
 
        const VirtualRobot::RobotPtr&
        getAgentSceneObj() const
        {
            return agentSceneObj;
        }
 
        /**
         * @return The collision checker.
         */
        VirtualRobot::CDManager&
        getCD()
        {
            return cd;
        }
 
        virtual Ice::StringSeq getAgentJointNames() const;
 
        //utility
        VectorXf jointMapToVector(const std::map<std::string, float>& jointMap,
                                  bool checkForNonexistenJointsInCspace = false) const;
 
        virtual TrajectoryPtr
        pathToTrajectory(const Path& p) const
        {
            return pathToTrajectory(p.nodes);
        }
 
        virtual TrajectoryPtr
        pathToTrajectory(const PathWithCost& p) const
        {
            return pathToTrajectory(p.nodes);
        }
 
        virtual TrajectoryPtr pathToTrajectory(const VectorXfSeq& p) const;
 
        std::vector<armarx::DebugDrawerLineSet>
        getTraceVisu(const Path& p,
                     const std::vector<std::string>& nodeNames,
                     float traceStepSize = std::numeric_limits<float>::infinity())
        {
            return getTraceVisu(p.nodes, nodeNames, traceStepSize);
        }
 
        std::vector<armarx::DebugDrawerLineSet>
        getTraceVisu(const PathWithCost& p,
                     const std::vector<std::string>& nodeNames,
                     float traceStepSize = std::numeric_limits<float>::infinity())
        {
            return getTraceVisu(p.nodes, nodeNames, traceStepSize);
        }
 
        std::vector<armarx::DebugDrawerLineSet>
        getTraceVisu(const VectorXfSeq& vecs,
                     const std::vector<std::string>& nodeNames,
                     float traceStepSize = std::numeric_limits<float>::infinity());
 
        float
        getStationaryObjectMargin() const
        {
            return stationaryObjectMargin;
        }
 
        void
        setStationaryObjectMargin(float stationaryObjectMargin)
        {
            if (!isInitialized)
            {
                this->stationaryObjectMargin = stationaryObjectMargin;
            }
            else
            {
                ARMARX_ERROR << "Could not set stationary object margin, because the cSpace is "
                                "already initialized.";
            }
        }
 
        //from ice
        /**
         * @brief Initializes basic structures after transmission through ice.
         */
        void ice_postUnmarshal() override;
 
    protected:
        template <class IceBaseClass, class DerivedClass>
        friend class armarx::GenericFactory;
 
        const std::vector<VirtualRobot::RobotNodePtr>&
        getAgentJoints() const
        {
            return agentJoints;
        }
 
        /**
         * @brief Default ctor. Used for ice factories.
         */
        SimoxCSpace() = default;
 
        //data
        /**
         * @brief Whether the collision check is initialized
         */
        bool isInitialized{false};
        /**
         * @brief Whether the visualization model of objects/agents sould be loaded.
         */
        bool loadVisualizationModel{false};
 
 
        /**
         * @brief The file manager used to load objects.
         */
        memoryx::GridFileManagerPtr fileManager;
 
        //cd data
        /**
         * @brief The collision checker.
         */
        VirtualRobot::CDManager cd;
 
        /**
         * @brief The scene objects for stationary objects.
         */
        VirtualRobot::SceneObjectSetPtr stationaryObjectSet{
            new VirtualRobot::SceneObjectSet{"StationaryObjectSet"}};
 
        std::vector<VirtualRobot::RobotNodePtr> agentJoints;
 
        VirtualRobot::RobotPtr agentSceneObj;
 
        std::vector<std::vector<Eigen::Vector3f>> stationaryPlanes;
 
        mutable GlobalCache<VirtualRobot::ManipulationObjectPtr, std::pair<int, std::string>>
            meshCache;
        mutable GlobalCache<RobotPoolPtr,
                            std::pair<VirtualRobot::RobotIO::RobotDescription, std::string>>
            robotCache;
 
 
        /**
         * @brief Initializes stationary objects for collision checking.
         */
        void initStationaryObjects();
 
        /**
         * @brief Contains information about an agent.
         */
        struct AgentData
        {
            /**
            * @brief The agent's collision model. (contains more data but mainly used for the model)
            */
            VirtualRobot::RobotPtr agent;
            /**
             * @brief The agent's joints.
             */
            std::vector<VirtualRobot::RobotNodePtr> joints;
            /**
             * @brief The collision stes.
             */
            std::vector<VirtualRobot::SceneObjectSetPtr> collisionSets;
        };
 
        //static functions to load data
        /**
         * @brief Initializes an agent.
         * (Does not need to read from the file system if loadAnyModel is false)
         * @param agentData Data about the agent.
         * @param loadAnyModel if unset only the robot structure is loaded.
         * If set the robot's and objects' collision model are loaded. (depending on
         * loadVisualizationModel the robot's visualisation model is loaded)
         */
        void initAgent(AgentData agentData);
 
        void
        initAgent()
        {
            initAgent(getAgentDataAndLoadRobot());
        }
 
        void
        initAgent(VirtualRobot::RobotPtr robotPtr)
        {
            initAgent(getAgentDataFromRobot(std::move(robotPtr)));
        }
 
        /**
         * @param object The object.
         * @return The given objects manipulation object.
         */
        VirtualRobot::ManipulationObjectPtr
        getSimoxManipulatorObject(const memoryx::ObjectClassBasePtr& object,
                                  const memoryx::GridFileManagerPtr& fileManager,
                                  bool inflate = true,
                                  VirtualRobot::CollisionCheckerPtr const& colChecker =
                                      VirtualRobot::CollisionCheckerPtr{}) const;
 
        /**
         * @param object The object.
         * @param pose The pose to use.
         * @return The given objects manipulation object moved to the given pose.
         */
        VirtualRobot::ManipulationObjectPtr
        getMovedSimoxManipulatorObject(const memoryx::ObjectClassBasePtr& object,
                                       const armarx::PoseBasePtr& pose,
                                       memoryx::GridFileManagerPtr& fileManager) const;
 
        AgentData getAgentDataAndLoadRobot() const;
        AgentData getAgentDataFromRobot(VirtualRobot::RobotPtr robotPtr) const;
    };
 
 
    class SimoxCSpaceWith2DPose;
    using SimoxCSpaceWith2DPosePtr = IceInternal::Handle<SimoxCSpaceWith2DPose>;
 
    /**
     * @brief Similar to \ref armarx::SimoxCSpace, but prepends dimensions for translation in x and y and a rotation around z. (use this for path planning)
     */
    class SimoxCSpaceWith2DPose :
        virtual public SimoxCSpace,
        virtual public SimoxCSpaceWith2DPoseBase
    {
    public:
        SimoxCSpaceWith2DPose(memoryx::CommonStorageInterfacePrx commonStoragePrx,
                              bool loadVisualizationModel = false,
                              float stationaryObjectMargin = 0.0f);
 
        void
        setPoseBounds(const Vector3fRange& newBounds)
        {
            poseBounds = newBounds;
        }
 
        /**
         * @param loadVisualizationModel Whether the clone should load the visualization model.
         * @return A clone of this cspace.
         */
        CSpaceBasePtr clone(bool loadVisualizationModel) override;
 
        /**
         * @return The cspace's dimension bounds.
         */
        FloatRangeSeq getDimensionsBounds(const Ice::Current& = Ice::emptyCurrent) const override;
 
        /**
         * @return The cspace's dimensionality.
         */
        Ice::Long
        getDimensionality(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return agentJoints.size() + 3;
        }
 
        Ice::StringSeq getAgentJointNames() const override;
 
        void setConfig(const float*& it) override;
 
    protected:
        template <class IceBaseClass, class DerivedClass>
        friend class armarx::GenericFactory;
        SimoxCSpaceWith2DPose() = default;
 
        Eigen::Matrix4f globalPoseBuffer = Eigen::Matrix4f::Identity();
    };
 
    class SimoxCSpaceWith3DPose;
    using SimoxCSpaceWith3DPosePtr = IceInternal::Handle<SimoxCSpaceWith3DPose>;
 
    /**
     * @brief Similar to \ref armarx::SimoxCSpace, but prepends dimensions for translation in x, y and z and rotations around x, y and z.
     * (use this for path planning of flying robots).
     */
    class SimoxCSpaceWith3DPose :
        virtual public SimoxCSpace,
        virtual public SimoxCSpaceWith3DPoseBase
    {
    public:
        SimoxCSpaceWith3DPose(memoryx::CommonStorageInterfacePrx commonStoragePrx,
                              bool loadVisualizationModel,
                              float stationaryObjectMargin = 0.0f);
 
        void
        setPoseBounds(const Vector6fRange& newBounds)
        {
            poseBounds = newBounds;
        }
 
        /**
         * @param loadVisualizationModel Whether the clone should load the visualization model.
         * @return A clone of this cspace.
         */
        CSpaceBasePtr clone(bool loadVisualizationModel) override;
 
        /**
         * @return The cspace's dimension bounds.
         */
        FloatRangeSeq getDimensionsBounds(const Ice::Current& = Ice::emptyCurrent) const override;
 
        /**
         * @return The cspace's dimensionality.
         */
        Ice::Long
        getDimensionality(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return agentJoints.size() + 6;
        }
 
        void setConfig(const float*& it) override;
 
        Ice::StringSeq getAgentJointNames() const override;
 
    protected:
        template <class IceBaseClass, class DerivedClass>
        friend class armarx::GenericFactory;
        SimoxCSpaceWith3DPose() = default;
 
        Eigen::Matrix4f globalPoseBuffer = Eigen::Matrix4f::Identity();
    };
 
} // namespace armarx