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 <Eigen/Core>
 
#include <VirtualRobot/CollisionDetection/CDManager.h>
#include <VirtualRobot/SceneObjectSet.h>
#include <VirtualRobot/Robot.h>
#include <MotionPlanning/CSpace/CSpace.h>
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
#include <RobotAPI/libraries/core/Trajectory.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <MemoryX/core/GridFileManager.h>
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <RobotComponents/interface/components/MotionPlanning/CSpace/SimoxCSpace.h>
#include "GlobalCache.h"
#include <VirtualRobot/XML/RobotIO.h>
 
#include <atomic>
 
#include "CSpace.h"
 
#include <RobotAPI/libraries/core/RobotPool.h>
 
namespace armarx
{
    class SimoxCSpace;
    using SimoxCSpacePtr = IceInternal::Handle<SimoxCSpace>;
 
 
 
    class SimoxCSpace :
        virtual public SimoxCSpaceBase,
        virtual public CSpace
    {
    public:
        using CSpace::isCollisionFree;
 
        SimoxCSpace(memoryx::CommonStorageInterfacePrx commonStoragePrx, bool loadVisualizationModel = false, float stationaryObjectMargin = 0.0f);
 
        //from setting the cspace up
        void addStationaryObject(const StationaryObject& obj, const Ice::Current& = Ice::emptyCurrent)override;
        virtual void setStationaryObjects(const StationaryObjectList& objList);
        void setAgent(const AgentPlanningInformation& agentInfo, const Ice::Current& = Ice::emptyCurrent)override;
 
        virtual void addObjectsFromWorkingMemory(memoryx::WorkingMemoryInterfacePrx workingMemoryPrx, const std::vector<std::string>& excludedInstancesIds = std::vector<std::string>());
 
        static SimoxCSpacePtr PrefetchWorkingMemoryObjects(memoryx::WorkingMemoryInterfacePrx workingMemoryPrx, memoryx::CommonStorageInterfacePrx commonStoragePrx, RobotStateComponentInterfacePrx rsc);
 
        virtual void addPlanarObject(const std::vector<Eigen::Vector3f>& convexHull);
 
        //using it
        void initCollisionTest(const Ice::Current& = Ice::emptyCurrent)override;
 
        CSpaceBasePtr clone(const Ice::Current& = Ice::emptyCurrent) override
        {
            return clone(false);
        }
 
        virtual CSpaceBasePtr clone(bool loadVisualizationModel);
 
        virtual Saba::CSpaceSampledPtr createSimoxCSpace() const;
 
        bool isCollisionFree(const::std::pair<const Ice::Float*, const Ice::Float*>& cfg, const Ice::Current& = Ice::emptyCurrent)override;
 
        FloatRangeSeq getDimensionsBounds(const Ice::Current& = Ice::emptyCurrent) const override;
        Ice::Long getDimensionality(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return agentJoints.size();
        }
 
        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
        const StationaryObjectList& getStationaryObjects() const
        {
            return stationaryObjects;
        }
 
        const AgentPlanningInformation& getAgent() const
        {
            return agentInfo;
        }
 
        const VirtualRobot::SceneObjectSetPtr& getStationaryObjectSet() const
        {
            return stationaryObjectSet;
        }
 
        const VirtualRobot::RobotPtr& getAgentSceneObj() const
        {
            return agentSceneObj;
        }
 
        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
        void ice_postUnmarshal() override;
 
    protected:
        template <class IceBaseClass, class DerivedClass> friend class armarx::GenericFactory;
 
        const std::vector<VirtualRobot::RobotNodePtr>& getAgentJoints() const
        {
            return agentJoints;
        }
 
        SimoxCSpace() = default;
 
        //data
        bool isInitialized {false};
        bool loadVisualizationModel {false};
 
 
        memoryx::GridFileManagerPtr fileManager;
 
        //cd data
        VirtualRobot::CDManager cd;
 
        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;
 
 
        void initStationaryObjects();
 
        struct AgentData
        {
            VirtualRobot::RobotPtr agent;
            std::vector<VirtualRobot::RobotNodePtr> joints;
            std::vector<VirtualRobot::SceneObjectSetPtr> collisionSets;
        };
 
        //static functions to load data
        void initAgent(AgentData agentData);
 
        void initAgent()
        {
            initAgent(getAgentDataAndLoadRobot());
        }
 
        void initAgent(VirtualRobot::RobotPtr robotPtr)
        {
            initAgent(getAgentDataFromRobot(std::move(robotPtr)));
        }
 
        VirtualRobot::ManipulationObjectPtr getSimoxManipulatorObject(const memoryx::ObjectClassBasePtr& object, const memoryx::GridFileManagerPtr& fileManager, bool inflate = true, VirtualRobot::CollisionCheckerPtr const& colChecker = VirtualRobot::CollisionCheckerPtr {}) const;
 
        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>;
    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;
        }
 
        CSpaceBasePtr clone(bool loadVisualizationModel) override;
 
        FloatRangeSeq getDimensionsBounds(const Ice::Current& = Ice::emptyCurrent) const override;
        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>;
    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;
        }
 
        CSpaceBasePtr clone(bool loadVisualizationModel) override;
 
        FloatRangeSeq getDimensionsBounds(const Ice::Current& = Ice::emptyCurrent) const override;
        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();
    };
 
}