SimoxCSpace.cpp

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
 */
 
#include "SimoxCSpace.h"
 
#include <algorithm>
#include <cmath>
#include <exception>
#include <unordered_map>
#include <unordered_set>
 
#include <VirtualRobot/CollisionDetection/CDManager.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/RobotFactory.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/util/algorithm.h>
 
#include <RobotAPI/libraries/core/Pose.h>
 
#include "../util/Metrics.h"
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MotionPlanning/CSpace/CSpace.h>
#include <MotionPlanning/CSpace/CSpaceSampled.h>
 
namespace armarx
{
    bool
    ensureCoinIsInitialized()
    {
        struct Initializer
        {
            //this is not threadsave
            Initializer()
            {
                //this is checked by SoDB::init()
                //if (!SoDB::isInitialized())
                //{
                SoDB::init();
                //}
            }
 
            bool
            doStuff()
            {
                return true;
            }
        };
 
        static Initializer init;
        return init.doStuff(); //suppresses unused variable
    }
 
    SimoxCSpace::SimoxCSpace(memoryx::CommonStorageInterfacePrx commonStoragePrx,
                             bool loadVisualizationModel,
                             float stationaryObjectMargin) :
        loadVisualizationModel{loadVisualizationModel}
    {
        this->stationaryObjectMargin = stationaryObjectMargin;
        //check eigen layout for mapped vectors
        Eigen::Matrix4f a;
        ARMARX_CHECK_EXPRESSION(a.data() + 0 == &a(0, 0));
        ARMARX_CHECK_EXPRESSION(a.data() + 1 == &a(1, 0));
        ARMARX_CHECK_EXPRESSION(a.data() + 2 == &a(2, 0));
        ARMARX_CHECK_EXPRESSION(a.data() + 3 == &a(3, 0));
        ARMARX_CHECK_EXPRESSION(a.data() + 4 == &a(0, 1));
        ARMARX_CHECK_EXPRESSION(a.data() + 5 == &a(1, 1));
        ARMARX_CHECK_EXPRESSION(a.data() + 6 == &a(2, 1));
        ARMARX_CHECK_EXPRESSION(a.data() + 7 == &a(3, 1));
        ARMARX_CHECK_EXPRESSION(a.data() + 8 == &a(0, 2));
        ARMARX_CHECK_EXPRESSION(a.data() + 9 == &a(1, 2));
        ARMARX_CHECK_EXPRESSION(a.data() + 10 == &a(2, 2));
        ARMARX_CHECK_EXPRESSION(a.data() + 11 == &a(3, 2));
        ARMARX_CHECK_EXPRESSION(a.data() + 12 == &a(0, 3));
        ARMARX_CHECK_EXPRESSION(a.data() + 13 == &a(1, 3));
        ARMARX_CHECK_EXPRESSION(a.data() + 14 == &a(2, 3));
        ARMARX_CHECK_EXPRESSION(a.data() + 15 == &a(3, 3));
 
        commonStorage = commonStoragePrx;
 
        if (!commonStorage)
        {
            ARMARX_ERROR_S << "SimoxCSpace ctor: commonStorage == null";
            throw std::invalid_argument{"SimoxCSpace ctor: commonStorage == null"};
        }
    }
 
    CSpaceBasePtr
    SimoxCSpace::clone(bool loadVisualizationModel)
    {
        TIMING_START(SimoxCSpaceClone);
        SimoxCSpacePtr cloned =
            new SimoxCSpace{commonStorage, loadVisualizationModel, stationaryObjectMargin};
 
        for (const auto& obj : stationaryObjects)
        {
            cloned->addStationaryObject(obj);
        }
        cloned->agentInfo = agentInfo;
        ARMARX_CHECK_EXPRESSION(agentSceneObj);
        cloned->initAgent();
        TIMING_END(SimoxCSpaceClone);
        return cloned;
    }
 
    Saba::CSpaceSampledPtr
    SimoxCSpace::createSimoxCSpace() const
    {
 
        //        ARMARX_INFO << "using kinematic chain set: " << agentInfo.kinemaicChainNames.at(0);
        VirtualRobot::CDManagerPtr tmpCd =
            VirtualRobot::CDManagerPtr(new VirtualRobot::CDManager(cd));
 
        //        for (VirtualRobot::SceneObjectSetPtr& set : tmpCd->getSceneObjectSets())
        //        {
        //            ARMARX_INFO << "set size: " << set->getSize();
        //        }
        Saba::CSpaceSampledPtr result(new Saba::CSpaceSampled(
            agentSceneObj,
            tmpCd,
            agentInfo.kinemaicChainNames.size() > 0
                ? agentSceneObj->getRobotNodeSet(agentInfo.kinemaicChainNames.at(0))
                : VirtualRobot::RobotNodeSetPtr()));
        return result;
    }
 
    //from SimoxCSpace
    void
    SimoxCSpace::addStationaryObject(const StationaryObject& obj, const Ice::Current&)
    {
        ARMARX_CHECK_EXPRESSION(obj.objectPose);
        ARMARX_CHECK_EXPRESSION(obj.objectClassBase);
        stationaryObjects.emplace_back(obj);
    }
 
    void
    SimoxCSpace::setStationaryObjects(const StationaryObjectList& objList)
    {
        stationaryObjects = objList;
    }
 
    void
    SimoxCSpace::setAgent(const AgentPlanningInformation& agentInfo, const Ice::Current&)
    {
        this->agentInfo = agentInfo;
        initAgent();
    }
 
    void
    SimoxCSpace::setConfig(const float*& it)
    {
        //stationary agents joints
        NameValueMap jointValues;
        for (auto agentJoint : agentJoints)
        {
            jointValues[agentJoint->getName()] = *it;
            agentJoint->setJointValueNoUpdate(*(it++));
        }
        //        ARMARX_VERBOSE << agentSceneObj.get() << " " << agentSceneObj->getUpdateVisualizationStatus() << " Setting joint values to new values: " << jointValues;
        agentSceneObj->applyJointValues();
    }
 
    bool
    SimoxCSpace::isCollisionFree(const ::std::pair<const Ice::Float*, const Ice::Float*>& cfg,
                                 const Ice::Current&)
    {
        ARMARX_CHECK_EXPRESSION(std::distance(cfg.first, cfg.second) == getDimensionality());
        const Ice::Float* it;
        it = cfg.first;
 
        setConfig(it);
 
        ARMARX_CHECK_EXPRESSION(cfg.second == it);
        //check
        return !cd.isInCollision();
    }
 
    void
    SimoxCSpace::initCollisionTest(const Ice::Current&)
    {
        if (isInitialized)
        {
            ARMARX_DEBUG_S << "was already initialized SimoxCSpace " << this;
            return;
        }
        TIMING_START(CSpaceInit);
        //init file manager
        ARMARX_CHECK_EXPRESSION(!fileManager);
        fileManager.reset(new memoryx::GridFileManager(commonStorage));
 
        //just to be save (may crash otherwise)
        TIMING_START(CSpaceInit_ensureCoinIsInitialized)
        ensureCoinIsInitialized();
        TIMING_END(CSpaceInit_ensureCoinIsInitialized)
 
        isInitialized = true;
        TIMING_START(CSpaceInit_initAgent)
        initAgent();
        TIMING_END(CSpaceInit_initAgent)
        TIMING_START(CSpaceInit_initStationaryObjects)
        initStationaryObjects();
        TIMING_END(CSpaceInit_initStationaryObjects)
        agentSceneObj->setJointValues(agentInfo.initialJointValues);
        ARMARX_CHECK_EXPRESSION(stationaryObjectSet->getSize() == stationaryObjects.size());
        TIMING_END(CSpaceInit);
    }
 
    void
    SimoxCSpace::initStationaryObjects()
    {
        stationaryObjectSet.reset(
            new VirtualRobot::SceneObjectSet{"StationaryObjectSet", cd.getCollisionChecker()});
        if (stationaryObjects.size() || stationaryPlanes.size())
        {
            ARMARX_CHECK_EXPRESSION(fileManager);
            for (std::size_t i = 0; i < stationaryObjects.size(); ++i)
            {
                ARMARX_CHECK_EXPRESSION(i == stationaryObjectSet->getSize());
                const auto& obj = stationaryObjects.at(i);
                auto&& sceneObj = getMovedSimoxManipulatorObject(
                    obj.objectClassBase, obj.objectPose, fileManager);
                //deactivate visu since it is not needed and moving visu is as expensive as moving the col mod
                sceneObj->setUpdateVisualization(false);
                stationaryObjectSet->addSceneObject(std::move(sceneObj));
            }
 
            for (auto& plane : stationaryPlanes)
            {
                VirtualRobot::CoinVisualizationNodePtr visu(new VirtualRobot::CoinVisualizationNode(
                    (SoNode*)VirtualRobot::CoinVisualizationFactory::CreatePolygonVisualization(
                        plane)));
                VirtualRobot::SceneObjectPtr s(new VirtualRobot::SceneObject(
                    "Plane",
                    visu,
                    VirtualRobot::CollisionModelPtr(new VirtualRobot::CollisionModel(visu)),
                    VirtualRobot::SceneObject::Physics{},
                    cd.getCollisionChecker()));
 
                stationaryObjectSet->addSceneObject(s);
            }
            if (stationaryObjectSet->getSize())
            {
                ARMARX_INFO << "SceneObjects: "
                            << armarx::transform(
                                   stationaryObjectSet->getSceneObjects(),
                                   +[](VirtualRobot::SceneObjectPtr const& obj)
                                   { return obj->getName(); });
                ARMARX_CHECK_EXPRESSION(!cd.hasSceneObjectSet(stationaryObjectSet));
                cd.addCollisionModel(stationaryObjectSet);
            }
        }
    }
 
    void
    SimoxCSpace::initAgent(AgentData agentData)
    {
        //move agent
        if (agentInfo.agentPose) //if none is set the default pose is used
        {
            agentData.agent->setGlobalPose(
                armarx::PosePtr::dynamicCast(agentInfo.agentPose)->toEigen());
        }
 
        agentSceneObj = std::move(agentData.agent);
        //deactivate visu since it is not needed and moving visu is as expensive as moving the col mod
        agentSceneObj->setUpdateVisualization(false);
 
        const std::size_t numberOfJoints = agentData.joints.size();
        agentJoints.clear();
        agentJoints.reserve(numberOfJoints);
        std::move(
            agentData.joints.begin(), agentData.joints.end(), std::back_inserter(agentJoints));
 
        if (isInitialized)
        {
            cd = VirtualRobot::CDManager{agentSceneObj->getCollisionChecker()};
            for (auto& colMod : agentData.collisionSets)
            {
                cd.addCollisionModel(std::move(colMod));
            }
            //            cd.addCollisionModel(stationaryObjectSet);
        }
    }
 
    SimoxCSpace::AgentData
    SimoxCSpace::getAgentDataAndLoadRobot() const
    {
        //load agent
        VirtualRobot::RobotPtr agent;
        {
            auto packageNames = agentInfo.agentProjectNames;
            if (!agentInfo.agentProjectName.empty())
            {
                packageNames.emplace_back(agentInfo.agentProjectName);
            }
 
            std::string absoluteFilePath;
            std::vector<std::string> paths;
            for (const auto& package : packageNames)
            {
                armarx::CMakePackageFinder packageFinder(package);
                auto pathStr = packageFinder.getDataDir();
                std::vector<std::string> packagePaths = Split(pathStr, ";,", false, true);
 
                paths.reserve(paths.size() + packagePaths.size());
                std::move(packagePaths.begin(), packagePaths.end(), std::back_inserter(paths));
            }
 
            if (!armarx::ArmarXDataPath::getAbsolutePath(
                    agentInfo.agentRelativeFilePath, absoluteFilePath, paths))
            {
                std::stringstream s;
                s << "could not find file " << agentInfo.agentRelativeFilePath << " in project "
                  << agentInfo.agentProjectName;
                ARMARX_ERROR_S << s.str();
                throw std::invalid_argument{s.str()};
            }
            //            static std::map<std::pair<VirtualRobot::RobotIO::RobotDescription, std::string>, VirtualRobot::RobotPtr> agentCache;
            //            static armarx::Mutex agentCacheMutex;
            //            ScopedLock lock(agentCacheMutex);
            auto loadType = isInitialized ? //the cspace is initializing => load models
                                (loadVisualizationModel ? VirtualRobot::RobotIO::eFull
                                                        : VirtualRobot::RobotIO::eCollisionModel)
                                          : VirtualRobot::RobotIO::eStructure;
            auto pair = std::make_pair(loadType, absoluteFilePath);
            //            auto it = robotCache.find(pair);
            if (!robotCache.hasObject(pair))
            {
                agent = VirtualRobot::RobotIO::loadRobot(absoluteFilePath, loadType);
 
                ARMARX_DEBUG << "Robot Cache MISS! Path: " << absoluteFilePath
                             << " load type: " << (int)loadType;
                RobotPoolPtr pool(new RobotPool(agent, 2));
                agent = pool->getRobot();
                robotCache.insertObject(pair, pool);
                //                agentCache.insert(std::make_pair(pair, agent)).first;
            }
            else
            {
                ARMARX_DEBUG << "Robot Cache hit! load type: " << (int)loadType;
                agent = robotCache.getCacheObject(pair)->getRobot();
            }
 
            //            auto loadType = isInitialized ?//the cspace is initializing => load models
            //                            (
            //                                loadVisualizationModel ?
            //                                VirtualRobot::RobotIO::eFull :
            //                                VirtualRobot::RobotIO::eCollisionModel
            //                            ) : VirtualRobot::RobotIO::eStructure;
            //            agent = VirtualRobot::RobotIO::loadRobot(
            //                        absoluteFilePath,
            //                        loadType
            //            );
            //            ARMARX_VERBOSE << "Loading robot! Path: " << absoluteFilePath << " load type: " << (int)loadType << " addr: " << agent.get();
            if (!agent)
            {
                std::stringstream s;
                s << "Can't load agent from: " << absoluteFilePath;
                ARMARX_ERROR_S << s.str();
                throw std::invalid_argument{s.str()};
            }
        }
 
        return getAgentDataFromRobot(agent);
    }
 
    SimoxCSpace::AgentData
    SimoxCSpace::getAgentDataFromRobot(VirtualRobot::RobotPtr robotPtr) const
    {
        AgentData data;
 
        ARMARX_CHECK_EXPRESSION(robotPtr);
        data.agent = robotPtr;
 
        if (isInitialized)
        {
            ARMARX_CHECK_EXPRESSION(fileManager);
            //attach objects
            //use the map to track this association
            std::unordered_map<std::string, std::vector<VirtualRobot::RobotNodePtr>>
                toCollSetAssociatedObjects;
            toCollSetAssociatedObjects.reserve(agentInfo.collisionSetNames.size());
 
            for (std::size_t i = 0; i < agentInfo.attachedObjects.size(); ++i)
            {
                const auto& attached = agentInfo.attachedObjects.at(i);
                auto&& sceneObj = getSimoxManipulatorObject(
                    attached.objectClassBase, fileManager, false, robotPtr->getCollisionChecker());
                std::stringstream sceneObjName;
                sceneObjName << "attached_obj_" << i << "_" << sceneObj->getName();
                sceneObj->setName(sceneObjName.str());
 
                //find node to attach to and attach
                if (!data.agent->hasRobotNode(attached.attachedToRobotNodeName))
                {
                    std::stringstream s;
                    s << "Agent " << data.agent->getName() << " has no node "
                      << attached.attachedToRobotNodeName << "to attach object "
                      << attached.objectClassBase->getName();
                    ARMARX_ERROR_S << s.str();
                    throw std::invalid_argument{s.str()};
                }
 
                VirtualRobot::RobotNodePtr nodeToAttachTo =
                    data.agent->getRobotNode(attached.attachedToRobotNodeName);
                VirtualRobot::RobotFactory::attach(
                    data.agent,
                    sceneObj,
                    nodeToAttachTo,
                    armarx::PosePtr::dynamicCast(attached.relativeObjectPose)->toEigen());
 
                //adding to cd
                if (attached.associatedCollisionSetName.empty())
                {
                    //add alone
                    data.collisionSets.emplace_back(new VirtualRobot::SceneObjectSet{
                        sceneObjName.str(), data.agent->getCollisionChecker()});
                    data.collisionSets.back()->addSceneObject(
                        data.agent->getRobotNode(sceneObjName.str()));
                }
                else
                {
                    //does the set exist?
                    if (std::find(agentInfo.collisionSetNames.begin(),
                                  agentInfo.collisionSetNames.end(),
                                  attached.associatedCollisionSetName) ==
                        agentInfo.collisionSetNames.end())
                    {
                        std::stringstream s;
                        s << "Agent " << data.agent->getName() << " has no set "
                          << attached.associatedCollisionSetName
                          << " to use as associated collision set for attached object "
                          << attached.objectClassBase->getName();
                        ARMARX_ERROR_S << s.str();
                        throw std::invalid_argument{s.str()};
                    }
                    //add to set
                    toCollSetAssociatedObjects[attached.associatedCollisionSetName].emplace_back(
                        data.agent->getRobotNode(sceneObjName.str()));
                }
            }
            //get collision models
            for (const auto& name : agentInfo.collisionSetNames)
            {
                if (!data.agent->hasRobotNodeSet(name))
                {
                    std::stringstream s;
                    s << "Agent " << data.agent->getName() << " has no collision model " << name;
                    ARMARX_ERROR_S << s.str();
                    throw std::invalid_argument{s.str()};
                }
 
                //add set + associated objects for cd
                data.collisionSets.emplace_back(
                    new VirtualRobot::SceneObjectSet{name, data.agent->getCollisionChecker()});
                data.collisionSets.back()->addSceneObjects(data.agent->getRobotNodeSet(name));
 
                for (auto& obj : toCollSetAssociatedObjects[name])
                {
                    data.collisionSets.back()->addSceneObject(std::move(obj));
                }
            }
            if (!agentInfo.collisionObjectNames.empty())
            {
                data.collisionSets.emplace_back(new VirtualRobot::SceneObjectSet{
                    "collisionObjects", data.agent->getCollisionChecker()});
                for (const auto& name : agentInfo.collisionObjectNames)
                {
                    if (!data.agent->hasRobotNode(name))
                    {
                        std::stringstream s;
                        s << "Agent " << data.agent->getName() << " has no collision model "
                          << name;
                        ARMARX_ERROR_S << s.str();
                        throw std::invalid_argument{s.str()};
                    }
 
                    //add set + associated objects for cd
                    data.collisionSets.back()->addSceneObject(data.agent->getRobotNode(name));
 
                    for (auto& obj : toCollSetAssociatedObjects[name])
                    {
                        data.collisionSets.back()->addSceneObject(std::move(obj));
                    }
                }
            }
        }
 
        //get joints
        //some joints may be blacklisted or already added
        std::set<std::string> blacklist(agentInfo.jointsExcludedFromPlanning.begin(),
                                        agentInfo.jointsExcludedFromPlanning.end());
        //add kinematic chains
        for (const auto& name : agentInfo.kinemaicChainNames)
        {
            if (!data.agent->hasRobotNodeSet(name))
            {
                std::stringstream s;
                s << "Agent " << data.agent->getName() << " has no kinematic chain " << name;
                ARMARX_ERROR_S << s.str();
                throw std::invalid_argument{s.str()};
            }
 
            //add chain
            for (auto& node : *(data.agent->getRobotNodeSet(name)))
            {
                auto nodeIt = blacklist.find(node->getName());
                if (nodeIt != blacklist.end())
                {
                    continue;
                }
 
                if (!(node->isTranslationalJoint() || node->isRotationalJoint()))
                {
                    std::stringstream s;
                    s << "The node " << node->getName() << " in the kinematic chain " << name
                      << " of agent " << data.agent->getName()
                      << " is neither rotational nor tranlational";
                    ARMARX_ERROR_S << s.str();
                    throw std::invalid_argument{s.str()};
                }
 
                data.joints.emplace_back(node);
                blacklist.emplace_hint(nodeIt, node->getName());
            }
        }
        //add single nodes
        for (const auto& name : agentInfo.additionalJointsForPlanning)
        {
            if (!data.agent->hasRobotNode(name))
            {
                std::stringstream s;
                s << "Agent " << data.agent->getName() << " has no node " << name;
                ARMARX_ERROR_S << s.str();
                throw std::invalid_argument{s.str()};
            }
            auto node = data.agent->getRobotNode(name);
 
            auto nodeIt = blacklist.find(node->getName());
            if (nodeIt != blacklist.end())
            {
                continue;
            }
 
            if (!(node->isTranslationalJoint() || node->isRotationalJoint()))
            {
                std::stringstream s;
                s << "The node " << node->getName() << " of agent " << data.agent->getName()
                  << " is neither rotational nor tranlational";
                ARMARX_ERROR_S << s.str();
                throw std::invalid_argument{s.str()};
            }
 
            data.joints.emplace_back(node);
            blacklist.emplace_hint(nodeIt, node->getName());
        }
        return data;
    }
 
    VirtualRobot::ManipulationObjectPtr
    SimoxCSpace::getSimoxManipulatorObject(
        const memoryx::ObjectClassBasePtr& object,
        const memoryx::GridFileManagerPtr& fileManager,
        bool inflate,
        VirtualRobot::CollisionCheckerPtr const& colChecker) const
    {
        memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(object);
        //        static boost::mutex meshCacheMutex;
        //        static std::map<std::pair<int, std::string>, VirtualRobot::ManipulationObjectPtr> meshCache;
        memoryx::EntityWrappers::SimoxObjectWrapperPtr sw;
        VirtualRobot::ManipulationObjectPtr mo;
        if (!objectClass)
        {
            std::stringstream s;
            s << "Can't use object class with ice id " << object->ice_id();
            ARMARX_ERROR_S << s.str();
            throw armarx::InvalidArgumentException{s.str()};
        }
        {
            //            boost::mutex::scoped_lock lock(meshCacheMutex);
            auto pair = std::make_pair((int)(stationaryObjectMargin * 1000), object->getId());
            auto pairZeroMargin = std::make_pair(0, object->getId());
 
            if (meshCache.hasObject(pair))
            {
                auto tmpMo = meshCache.getCacheObject(pair);
                IceUtil::Time start = IceUtil::Time::now();
                mo = tmpMo->clone(tmpMo->getName(), colChecker, true);
                IceUtil::Time end = IceUtil::Time::now();
                ARMARX_DEBUG << "mesh Cache hit for " << tmpMo->getName() << " - Cloning took "
                             << (end - start).toMilliSeconds();
            }
            else
            {
                if (meshCache.hasObject(pairZeroMargin))
                {
                    auto tmpMo = meshCache.getCacheObject(pairZeroMargin);
                    ARMARX_DEBUG << "mesh Cache HALFMISS - mesh inflation needed for "
                                 << tmpMo->getName();
                    mo = tmpMo->clone(tmpMo->getName(), colChecker, true);
                }
                else
                {
                    sw = objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(
                        fileManager, VirtualRobot::RobotIO::eCollisionModel));
                    VirtualRobot::ManipulationObjectPtr orgMo = sw->getManipulationObject();
                    ARMARX_DEBUG << "mesh Cache MISS for " << orgMo->getName();
                    ARMARX_CHECK_EXPRESSION(orgMo);
                    std::string moName = orgMo->getName();
                    mo = orgMo->clone(moName, colChecker, true);
                }
                if (this->stationaryObjectMargin != 0.0f && inflate)
                {
                    ARMARX_DEBUG << deactivateSpam(3, to_string(stationaryObjectMargin)) << "Using "
                                 << stationaryObjectMargin << " as margin";
 
                    mo->getCollisionModel()->inflateModel(this->stationaryObjectMargin);
                }
                meshCache.insertObject(pair, mo);
            }
        }
 
 
        return mo;
    }
 
    VirtualRobot::ManipulationObjectPtr
    SimoxCSpace::getMovedSimoxManipulatorObject(const memoryx::ObjectClassBasePtr& object,
                                                const PoseBasePtr& pose,
                                                memoryx::GridFileManagerPtr& fileManager) const
    {
        VirtualRobot::ManipulationObjectPtr mo = getSimoxManipulatorObject(
            object,
            fileManager,
            true,
            const_cast<VirtualRobot::CDManager*>(&cd)->getCollisionChecker());
 
        //move the object to the given position
        const auto objectPose = armarx::PosePtr::dynamicCast(pose);
 
        if (!objectPose)
        {
            std::stringstream s;
            s << "Can't convert pose of " << memoryx::ObjectClassPtr::dynamicCast(object)->getName()
              << " to armarx::Pose.";
            ARMARX_ERROR_S << s.str();
            throw armarx::InvalidArgumentException{s.str()};
        }
 
        mo->setGlobalPose(objectPose->toEigen());
        return mo;
    }
 
    void
    SimoxCSpace::addObjectsFromWorkingMemory(
        memoryx::WorkingMemoryInterfacePrx workingMemoryPrx,
        const std::vector<std::string>& excludedInstancesIds /* = std::vector<std::string>() */)
    {
        TIMING_START(LoadFromWM);
        //pass all objects from the scene to the cspace
        auto objInstPrx = workingMemoryPrx->getObjectInstancesSegment();
        const auto objs = objInstPrx->getAllEntities();
        auto agentSeg = workingMemoryPrx->getAgentInstancesSegment();
 
        for (const auto& entityBase : objs)
        {
            auto id = entityBase->getId();
            if (std::find(excludedInstancesIds.cbegin(), excludedInstancesIds.cend(), id) !=
                excludedInstancesIds.cend())
            {
                continue;
            }
 
            const memoryx::ObjectInstancePtr object =
                memoryx::ObjectInstancePtr::dynamicCast(entityBase);
            auto objPose = object->getPose();
            if (objPose->frame != GlobalFrame && !objPose->frame.empty())
            {
                objPose = FramedPosePtr::dynamicCast(
                    agentSeg->convertToWorldPose(objPose->agent, objPose));
            }
 
            ARMARX_CHECK_EXPRESSION(object);
 
            const std::string className = object->getMostProbableClass();
 
            memoryx::ObjectClassList classes = workingMemoryPrx->getPriorKnowledge()
                                                   ->getObjectClassesSegment()
                                                   ->getClassWithSubclasses(className);
 
            if (!classes.size())
            {
                ARMARX_INFO_S << "No classes for most probable class '" << className
                              << "' of object '" << object->getName() << "' with id " << id;
                continue;
            }
 
            addStationaryObject({classes.at(0), armarx::PoseBasePtr{objPose}});
        }
        TIMING_END(LoadFromWM);
    }
 
    SimoxCSpacePtr
    SimoxCSpace::PrefetchWorkingMemoryObjects(memoryx::WorkingMemoryInterfacePrx workingMemoryPrx,
                                              memoryx::CommonStorageInterfacePrx commonStoragePrx,
                                              armarx::RobotStateComponentInterfacePrx rsc)
    {
        SimoxCSpacePtr cspace(new SimoxCSpace(commonStoragePrx));
 
        AgentPlanningInformation agentData;
        agentData.agentProjectNames = rsc->getArmarXPackages();
        agentData.agentRelativeFilePath = rsc->getRobotFilename();
 
        cspace->setAgent(agentData);
 
        cspace->addObjectsFromWorkingMemory(workingMemoryPrx);
        cspace->initCollisionTest();
        return cspace;
    }
 
    void
    SimoxCSpace::addPlanarObject(const std::vector<Eigen::Vector3f>& convexHull)
    {
        stationaryPlanes.push_back(convexHull);
    }
 
    void
    SimoxCSpace::ice_postUnmarshal()
    {
        if (!commonStorage)
        {
            ARMARX_ERROR_S << "SimoxCSpace ice_postUnmarshal: commonStorage == null";
            throw std::invalid_argument{"SimoxCSpace ice_postUnmarshal: commonStorage == null"};
        }
 
        //should throw no direct exception since they were prevented when adding an agent
        initAgent();
    }
 
    template <class T, class Thrower>
    std::vector<std::vector<T>>
    transpose(const std::vector<std::vector<T>>& src, Thrower thrower)
    {
        std::size_t numDatapoints = src.size();
        if (!numDatapoints)
        {
            return {};
        }
        std::size_t numDimensions = src.at(0).size();
        std::vector<std::vector<T>> transposed(numDimensions, std::vector<T>(numDatapoints));
        for (std::size_t i = 0; i < numDatapoints; ++i)
        {
            auto& datapoint = src.at(i);
            if (datapoint.size() != static_cast<std::size_t>(numDimensions))
            {
                thrower(i, datapoint.size(), numDimensions);
            }
            for (std::size_t dim = 0; dim < numDimensions; ++dim)
            {
                transposed.at(dim).at(i) = datapoint.at(dim);
            }
        }
        return transposed;
    }
 
    template <class T>
    std::vector<std::vector<T>>
    transpose(const std::vector<std::vector<T>>& src)
    {
        return transpose(src,
                         [](std::size_t idx, std::size_t szi, std::size_t numDim)
                         {
                             std::stringstream ss;
                             ss << "transpose: Element " << idx << " has " << szi
                                << " dimensions. The Element 0 has " << numDim << "dimensions.";
                             throw std::invalid_argument{ss.str()};
                         });
    }
 
    TrajectoryPtr
    SimoxCSpace::pathToTrajectory(const VectorXfSeq& p) const
    {
        if (p.empty())
        {
            return nullptr;
        }
        auto thrower = [](std::size_t idx, std::size_t szi, std::size_t numDim)
        {
            std::stringstream ss;
            ss << "SimoxCSpace::pathToTrajectory: The datapoint " << idx << " has " << szi
               << " dimensions. The CSpace has " << numDim << "dimensions.";
            throw std::invalid_argument{ss.str()};
        };
        if (static_cast<std::size_t>(getDimensionality()) != p.front().size())
        {
            thrower(0, p.front().size(), static_cast<std::size_t>(getDimensionality()));
        }
        TrajectoryPtr traj = new Trajectory{
            transpose(p, thrower), Ice::DoubleSeq() /*timestamps*/, getAgentJointNames()};
        LimitlessStateSeq limitlessStates;
        for (auto& joint : getAgentJoints())
        {
            LimitlessState state;
            state.enabled = joint->isLimitless();
            state.limitHi = joint->getJointLimitHigh();
            state.limitLo = joint->getJointLimitLow();
            limitlessStates.push_back(state);
        }
        traj->setLimitless(limitlessStates);
        return traj;
    }
 
    FloatRangeSeq
    SimoxCSpace::getDimensionsBounds(const Ice::Current&) const
    {
        FloatRangeSeq dims;
        dims.reserve(getDimensionality());
        std::transform(agentJoints.begin(),
                       agentJoints.end(),
                       std::back_inserter(dims),
                       [](const VirtualRobot::RobotNodePtr& joint) {
                           return FloatRange{joint->getJointLimitLo(), joint->getJointLimitHi()};
                       });
        return dims;
    }
 
    Ice::StringSeq
    SimoxCSpace::getAgentJointNames() const
    {
        Ice::StringSeq result;
        result.reserve(getAgentJoints().size());
        for (const auto& e : getAgentJoints())
        {
            result.emplace_back(e->getName());
        }
        return result;
    }
 
    VectorXf
    SimoxCSpace::jointMapToVector(const std::map<std::string, float>& jointMap,
                                  bool checkForNonexistenJointsInCspace) const
    {
        VectorXf result(getDimensionality(), 0.f);
        std::size_t valuesFromMapUsed = 0;
        auto jointNames = getAgentJointNames();
        for (std::size_t i = 0; i < jointNames.size(); ++i)
        {
            auto it = jointMap.find(jointNames.at(i));
            if (it != jointMap.end())
            {
                result.at(i) = it->second;
                ++valuesFromMapUsed;
            }
        }
        if (checkForNonexistenJointsInCspace && (valuesFromMapUsed != jointMap.size()))
        {
            throw std::invalid_argument{
                "the joint map contained a joint not contained by the cspace!"};
        }
        return result;
    }
 
    SimoxCSpaceWith2DPose::SimoxCSpaceWith2DPose(
        memoryx::CommonStorageInterfacePrx commonStoragePrx,
        bool loadVisualizationModel,
        float stationaryObjectMargin) :
        SimoxCSpace(commonStoragePrx, loadVisualizationModel, stationaryObjectMargin)
    {
        ARMARX_CHECK_EXPRESSION(commonStoragePrx);
    }
 
    CSpaceBasePtr
    SimoxCSpaceWith2DPose::clone(bool loadVisualizationModel)
    {
        SimoxCSpaceWith2DPosePtr cloned = new SimoxCSpaceWith2DPose{
            commonStorage, loadVisualizationModel, stationaryObjectMargin};
        for (const auto& obj : stationaryObjects)
        {
            cloned->addStationaryObject(obj);
        }
        cloned->poseBounds = poseBounds;
        cloned->agentInfo = agentInfo;
        cloned->initAgent();
        return cloned;
    }
 
    FloatRangeSeq
    SimoxCSpaceWith2DPose::getDimensionsBounds(const Ice::Current&) const
    {
        FloatRangeSeq bounds(getDimensionality());
        bounds.at(0).min = poseBounds.min.e0;
        bounds.at(0).max = poseBounds.max.e0;
 
        bounds.at(1).min = poseBounds.min.e1;
        bounds.at(1).max = poseBounds.max.e1;
 
        bounds.at(2).min = poseBounds.min.e2;
        bounds.at(2).max = poseBounds.max.e2;
 
        auto parentBounds = SimoxCSpace::getDimensionsBounds();
        std::move(parentBounds.begin(), parentBounds.end(), bounds.begin() + 3);
        return bounds;
    }
 
    Ice::StringSeq
    SimoxCSpaceWith2DPose::getAgentJointNames() const
    {
        auto joints = SimoxCSpace::getAgentJointNames();
        joints.resize(joints.size() + 3);
        std::move_backward(joints.begin(), joints.end() - 3, joints.end());
        joints.at(0) = "x";
        joints.at(1) = "y";
        joints.at(2) = "alpha";
        return joints;
    }
 
    void
    SimoxCSpaceWith2DPose::setConfig(const float*& it)
    {
        globalPoseBuffer(0, 3) = *(it++);
        globalPoseBuffer(1, 3) = *(it++);
 
        const auto rot = *(it++);
 
        const auto crot = std::cos(rot);
        const auto srot = std::sin(rot);
 
        //see http://planning.cs.uiuc.edu/node102.html
        //row 0
        globalPoseBuffer(0, 0) = crot;
        globalPoseBuffer(0, 1) = -srot;
        //row 1
        globalPoseBuffer(1, 0) = srot;
        globalPoseBuffer(1, 1) = crot;
 
        agentSceneObj->setGlobalPoseNoChecks(globalPoseBuffer);
        SimoxCSpace::setConfig(it);
    }
 
    SimoxCSpaceWith3DPose::SimoxCSpaceWith3DPose(
        memoryx::CommonStorageInterfacePrx commonStoragePrx,
        bool loadVisualizationModel,
        float stationaryObjectMargin) :
        SimoxCSpace(commonStoragePrx, loadVisualizationModel, stationaryObjectMargin)
    {
        ARMARX_CHECK_EXPRESSION(commonStoragePrx);
    }
 
    CSpaceBasePtr
    SimoxCSpaceWith3DPose::clone(bool loadVisualizationModel)
    {
        SimoxCSpaceWith3DPosePtr cloned = new SimoxCSpaceWith3DPose{
            commonStorage, loadVisualizationModel, stationaryObjectMargin};
        for (const auto& obj : stationaryObjects)
        {
            cloned->addStationaryObject(obj);
        }
        cloned->poseBounds = poseBounds;
        cloned->agentInfo = agentInfo;
        cloned->initAgent();
        return cloned;
    }
 
    FloatRangeSeq
    SimoxCSpaceWith3DPose::getDimensionsBounds(const Ice::Current&) const
    {
        FloatRangeSeq bounds(getDimensionality());
        bounds.at(0).min = poseBounds.min.e0;
        bounds.at(0).max = poseBounds.max.e0;
 
        bounds.at(1).min = poseBounds.min.e1;
        bounds.at(1).max = poseBounds.max.e1;
 
        bounds.at(2).min = poseBounds.min.e2;
        bounds.at(2).max = poseBounds.max.e2;
 
        bounds.at(3).min = poseBounds.min.e3;
        bounds.at(3).max = poseBounds.max.e3;
 
        bounds.at(4).min = poseBounds.min.e4;
        bounds.at(4).max = poseBounds.max.e4;
 
        bounds.at(5).min = poseBounds.min.e5;
        bounds.at(5).max = poseBounds.max.e5;
        auto parentBounds = SimoxCSpace::getDimensionsBounds();
        std::move(parentBounds.begin(), parentBounds.end(), bounds.begin() + 6);
        return bounds;
    }
 
    void
    SimoxCSpaceWith3DPose::setConfig(const float*& it)
    {
        globalPoseBuffer(0, 3) = *(it++);
        globalPoseBuffer(1, 3) = *(it++);
        globalPoseBuffer(2, 3) = *(it++);
 
        const auto roll = *(it++);
        const auto pitch = *(it++);
        const auto yaw = *(it++);
 
        const auto croll = std::cos(roll);
        const auto sroll = std::sin(roll);
 
        const auto cpitch = std::cos(pitch);
        const auto spitch = std::sin(pitch);
 
        const auto cyaw = std::cos(yaw);
        const auto syaw = std::sin(yaw);
 
        //see http://planning.cs.uiuc.edu/node102.html
        //row 0
        globalPoseBuffer(0, 0) = cyaw * cpitch;
        globalPoseBuffer(0, 1) = cyaw * spitch * sroll - syaw * croll;
        globalPoseBuffer(0, 2) = cyaw * spitch * sroll + syaw * sroll;
        //row 1
        globalPoseBuffer(1, 0) = syaw * cpitch;
        globalPoseBuffer(1, 1) = syaw * spitch * sroll + cyaw * croll;
        globalPoseBuffer(1, 2) = syaw * spitch * croll - cyaw * sroll;
        //row 2
        globalPoseBuffer(2, 0) = -spitch;
        globalPoseBuffer(2, 1) = cpitch * sroll;
        globalPoseBuffer(2, 2) = cpitch * croll;
 
        agentSceneObj->setGlobalPoseNoChecks(globalPoseBuffer);
        SimoxCSpace::setConfig(it);
    }
 
    Ice::StringSeq
    SimoxCSpaceWith3DPose::getAgentJointNames() const
    {
        auto joints = SimoxCSpace::getAgentJointNames();
        joints.resize(joints.size() + 6);
        std::move_backward(joints.begin(), joints.end() - 6, joints.end());
        joints.at(0) = "x";
        joints.at(1) = "y";
        joints.at(2) = "z";
        joints.at(3) = "alpha";
        joints.at(4) = "betha";
        joints.at(5) = "gamma";
        return joints;
    }
 
    std::vector<armarx::DebugDrawerLineSet>
    SimoxCSpace::getTraceVisu(const armarx::VectorXfSeq& vecs,
                              const std::vector<std::string>& nodeNames,
                              float traceStepSize)
    {
        if (traceStepSize <= 0)
        {
            throw std::invalid_argument{"SimoxCSpace::getTraces: traceStepSize <= 0"};
        }
        std::vector<armarx::DebugDrawerLineSet> traces;
        if (nodeNames.empty())
        {
            return traces;
        }
        traces.resize(nodeNames.size());
        if (vecs.size() < 2)
        {
            return traces;
        }
        auto addPoints = [&]
        {
            for (std::size_t i = 0; i < nodeNames.size(); ++i)
            {
                const auto& name = nodeNames.at(i);
                armarx::DebugDrawerLineSet& trace = traces.at(i);
                const auto pose = getAgentSceneObj()->getRobotNode(name)->getGlobalPose();
                trace.points.emplace_back(
                    DebugDrawerPointCloudElement{pose(0, 3), pose(1, 3), pose(2, 3)});
            };
        };
        for (std::size_t vecI = 0; vecI < vecs.size() - 1; ++vecI)
        {
            const auto& vecStart = vecs.at(vecI);
            const auto& vecEnd = vecs.at(vecI + 1);
            assert(vecStart.size() == vecEnd.size());
            assert(vecStart.size() == static_cast<std::size_t>(getDimensionality()));
            auto dist = euclideanDistance(vecStart.begin(), vecStart.end(), vecEnd.begin());
            if (dist > traceStepSize)
            {
                VectorXf vecCfg = vecStart;
                Eigen::Map<const Eigen::VectorXf> eStart{vecStart.data(), getDimensionality()};
                Eigen::Map<const Eigen::VectorXf> eEnd{vecEnd.data(), getDimensionality()};
                Eigen::Map<Eigen::VectorXf> eCfg{vecCfg.data(), getDimensionality()};
 
                Eigen::VectorXf eStep = (eEnd - eStart).normalized() * traceStepSize;
                setConfig(vecCfg);
                while (dist > traceStepSize)
                {
                    addPoints(); //start
                    eCfg += eStep;
                    setConfig(vecCfg);
                    addPoints(); //end
                    dist -= traceStepSize;
                }
                addPoints(); //start
                setConfig(vecEnd);
                addPoints(); //end
            }
            else
            {
                setConfig(vecStart);
                addPoints(); //start
                setConfig(vecEnd);
                addPoints(); //end
            }
        }
        return traces;
    }
} // namespace armarx