GraspingManager.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2015-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::GraspingManager
 * @author     Valerij Wittenbeck (valerij dot wittenbeck at student dot kit dot edu)
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "GraspingManager.h"
 
#include <atomic>
#include <tuple>
 
#include <IceUtil/UUID.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/IK/ConstrainedOptimizationIK.h>
#include <VirtualRobot/IK/constraints/CollisionCheckConstraint.h>
#include <VirtualRobot/IK/constraints/OrientationConstraint.h>
#include <VirtualRobot/IK/constraints/PoseConstraint.h>
#include <VirtualRobot/IK/constraints/PositionConstraint.h>
#include <VirtualRobot/IK/constraints/ReferenceConfigurationConstraint.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/util/OnScopeExit.h>
#include <ArmarXCore/core/util/StringHelperTemplates.h>
 
#include <RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h>
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/Trajectory.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <RobotComponents/components/GraspSelectionManager/selectionCriteria/NaturalGraspFilter.h>
#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
#include <RobotComponents/components/MotionPlanning/CSpace/VoxelGridCSpace.h>
#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
 
using namespace armarx;
 
static const DrawColor COLOR_POSE_LINE{0.5f, 1.0f, 0.5f, 0.5f};
static const DrawColor COLOR_POSE_POINT{0.0f, 1.0f, 0.0f, 0.5f};
static const DrawColor COLOR_CONFIG_LINE{1.0f, 0.5f, 0.5f, 0.5f};
static const DrawColor COLOR_CONFIG_POINT{1.0f, 0.0f, 0.0f, 0.5f};
static const DrawColor COLOR_ROBOT{0.0f, 0.586f, 0.508f, 1.0f};
 
static const float LINE_WIDTH = 5.f;
static const float SPHERE_SIZE = 6.f;
 
static const std::map<std::string, std::string> TCP_HAND_MAPPING{{"TCP R", "handright3a"},
                                                                 {"TCP L", "handleft3a"},
                                                                 {"Hand L TCP", "left_hand"},
                                                                 {"Hand R TCP", "right_hand"}};
 
 
auto newId = []() mutable
{
    static std::atomic<int> i{0};
    return to_string(i++);
};
 
void
GraspingManager::onInitComponent()
{
    graspGeneratorName = getProperty<std::string>("GraspGeneratorName").getValue();
    robotPlacementName = getProperty<std::string>("RobotPlacementName").getValue();
    robotNodeSetNames =
        armarx::Split(getProperty<std::string>("RobotNodeSetNames").getValue(), ";");
    reachabilitySpaceFilePaths =
        armarx::Split(getProperty<std::string>("ReachabilitySpaceFilePaths").getValue(), ";");
 
    globalDescriptionPosition = new Vector3(0, 0, 0);
 
    //@TODO still not sure if this is the way to go
    for (auto& path : reachabilitySpaceFilePaths)
    {
        std::string packageName = std::filesystem::path{path}.begin()->string();
        ARMARX_CHECK_EXPRESSION(!packageName.empty())
            << "Path '" << path << "' could not be parsed correctly";
        armarx::CMakePackageFinder project(packageName);
        path = project.getDataDir() + "/" + path;
    }
 
    offeringTopic("DebugDrawerUpdates");
 
    usingProxy(graspGeneratorName);
    usingProxy("GraspSelectionManager");
    usingProxy("NaturalGraspFilter");
    usingProxy(robotPlacementName);
    usingProxy("PlannedMotionProvider");
 
    usingProxy("RobotIK");
    usingProxy("RobotStateComponent");
 
    usingProxy("CommonStorage");
    usingProxy("WorkingMemory");
    usingProxy("PriorKnowledge");
}
 
void
GraspingManager::onConnectComponent()
{
    entityDrawer = getTopic<memoryx::EntityDrawerInterfacePrx>("DebugDrawerUpdates");
    layerName = getDefaultName();
    getProxy(gg, graspGeneratorName);
    getProxy(gsm, "GraspSelectionManager");
    getProxy(rp, robotPlacementName);
    getProxy(pmp, "PlannedMotionProvider");
 
    //getProxy(rik, "RobotIK");
    getProxy(rsc, "RobotStateComponent");
    localRobot =
        armarx::RemoteRobot::createLocalCloneFromFile(rsc, VirtualRobot::RobotIO::eCollisionModel);
    ikRobot = localRobot->clone();
    //    localRobot->print();
 
    getProxy(cs, "CommonStorage");
    getProxy(wm, "WorkingMemory");
    getProxy(prior, "PriorKnowledge");
 
    //  register the SimplePlacementFilter with the GraspSelectionManager
    if (getProperty<bool>("FilterUnnaturalGrasps").getValue())
    {
        getProxy(gsc, "NaturalGraspFilter");
        gsm->registerAsGraspSelectionCriterion(gsc);
    }
    cacheCSpace = SimoxCSpace::PrefetchWorkingMemoryObjects(wm, cs, rsc);
 
    Ice::FloatSeq boundsStrings =
        Split<float>(getProperty<std::string>("PlanningBoundingBox").getValue(), ",");
    ARMARX_INFO << VAROUT(boundsStrings);
    planningBoundingBox.min.e0 = boundsStrings.at(0);
    planningBoundingBox.min.e1 = boundsStrings.at(1);
    planningBoundingBox.min.e2 = boundsStrings.at(2);
    planningBoundingBox.max.e0 = boundsStrings.at(3);
    planningBoundingBox.max.e1 = boundsStrings.at(4);
    planningBoundingBox.max.e2 = boundsStrings.at(5);
 
    //    bool hasUnloadedRNS = false;
    //    for (const auto& rnsName : robotNodeSetNames)
    //    {
    //        if (!rik->hasReachabilitySpace(rnsName))
    //        {
    //            ARMARX_INFO << "RNS '" << rnsName << "' has no reachability space defined.";
    //            hasUnloadedRNS = true;
    //            break;
    //        }
    //    }
 
    //    if (hasUnloadedRNS)
    //    {
    //        ARMARX_INFO << "at least one RNS has no reachability space defined; loading reachability files...";
    //        for (const auto& path : reachabilitySpaceFilePaths)
    //        {
    //            ARMARX_INFO << "trying to load from path '" << path << "'";
    //            bool rsLoaded = rik->loadReachabilitySpace(path);
    //            ARMARX_CHECK_EXPRESSION(rsLoaded) << "Could not load reachability space from path '" << path << "'";
    //            ARMARX_INFO << "reachability file successfully loaded";
    //        }
    //    }
 
    //    for (const auto& rnsName : robotNodeSetNames)
    //    {
    //        ARMARX_CHECK_EXPRESSION(rik->hasReachabilitySpace(rnsName)) <<
    //                                       "RNS '" << rnsName << "' still has no reachability space defined";
    //    }
}
 
void
GraspingManager::onDisconnectComponent()
{
}
 
void
GraspingManager::onExitComponent()
{
    cacheCSpace = NULL;
}
 
GeneratedGraspList
GraspingManager::generateGrasps(const std::string& objectInstanceEntityId)
{
    setNextStepDescription("Generating grasps");
    ARMARX_VERBOSE << "Step: generate grasps";
    GeneratedGraspList grasps = gg->generateGrasps(objectInstanceEntityId);
    std::sort(grasps.begin(),
              grasps.end(),
              [](const GeneratedGrasp& l, const GeneratedGrasp& r) { return l.score < r.score; });
    if (grasps.empty())
    {
        ARMARX_WARNING << " Step 'generate grasps' generated no grasps";
    }
    //    auto obj = wm->getObjectInstancesSegment()->getObjectInstanceById(objectInstanceEntityId);
    // auto classes = prior->getObjectClassesSegment();
    return grasps;
}
 
std::pair<std::string, Ice::StringSeq>
GraspingManager::visualizeGrasp(const GeneratedGrasp& grasp, int id, const DrawColor& color)
{
    float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor").getValue();
    std::pair<std::string, Ice::StringSeq> result;
    if (visuSlowdownFactor <= 0 || !getProperty<bool>("EnableVisualization"))
    {
        return result;
    }
    auto tcpName = localRobot->getEndEffector(grasp.eefName)->getTcp()->getName();
    if (!TCP_HAND_MAPPING.count(tcpName))
    {
        return result;
    }
    auto handName = TCP_HAND_MAPPING.at(tcpName);
    auto objClass = prior->getObjectClassesSegment()->getObjectClassByName(handName);
    if (objClass)
    {
        usleep(500000 * visuSlowdownFactor);
        //        entityDrawer->setPoseVisu("GeneratedGrasps", "GraspCandidate" + handName + to_string(id), grasp.framedPose);
        result.first = "GeneratedGrasps";
        result.second = {"GraspCandidate" + handName + to_string(id),
                         "GraspCandidatePrepose" + handName + to_string(id)};
        entityDrawer->setObjectVisu(
            "GeneratedGrasps", result.second.at(0), objClass, grasp.framedPose);
        entityDrawer->setObjectVisu(
            "GeneratedGrasps", result.second.at(1), objClass, grasp.framedPrePose);
 
        entityDrawer->updateObjectColor("GeneratedGrasps", result.second.at(0), color);
        auto darkerColor = color;
        darkerColor.r *= 0.6f;
        darkerColor.g *= 0.6f;
        darkerColor.b *= 0.6f;
        entityDrawer->updateObjectColor("GeneratedGrasps", result.second.at(1), darkerColor);
    }
    else
    {
        ARMARX_INFO << "Could not find hand with name " << handName << " in priorknowledge";
    }
    return result;
}
 
SimoxCSpacePtr
GraspingManager::createCSpace()
{
    TIMING_START(CSpaceCreation);
 
    SimoxCSpacePtr cspace;
    if (getProperty<bool>("UseVoxelGridCSpace").getValue())
    {
        cspace =
            new VoxelGridCSpace(getProxy<visionx::VoxelGridProviderInterfacePrx>(
                                    getProperty<std::string>("VoxelGridProviderName").getValue()),
                                cs);
    }
    else
    {
        cspace = new SimoxCSpace(cs, false);
    }
    cspace->addObjectsFromWorkingMemory(wm);
    cspace->setStationaryObjectMargin(50);
    AgentPlanningInformation agentData;
    agentData.agentProjectNames = rsc->getArmarXPackages();
    agentData.agentRelativeFilePath = rsc->getRobotFilename();
    //    agentData.kinemaicChainNames = robotNodeSetNames;
    agentData.collisionSetNames = {
        getProperty<std::string>("RobotCollisionNodeSet")
            .getValue()}; // TODO: Make a mapping between jointset and link set
    cspace->setAgent(agentData);
    cspace->initCollisionTest();
    TIMING_END(CSpaceCreation);
    return cspace;
}
 
GeneratedGraspList
GraspingManager::filterGrasps(const GeneratedGraspList& grasps)
{
    setNextStepDescription("Filtering grasps");
    float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor");
 
 
    ARMARX_VERBOSE << "Step: filter grasps";
    GeneratedGraspList filteredGrasps = gsm->filterGrasps(grasps);
    if (filteredGrasps.empty())
    {
        ARMARX_WARNING << " Step 'filter generated grasps' filtered out all grasps";
    }
    int i = 0;
    if (getProperty<bool>("EnableVisualization"))
        for (GeneratedGrasp& grasp : filteredGrasps)
        {
            visualizeGrasp(grasp, i);
            i++;
        }
    if (visuSlowdownFactor > 0 && getProperty<bool>("EnableVisualization"))
    {
        usleep(2000000 * visuSlowdownFactor);
    }
 
    entityDrawer->removeLayer("GeneratedGrasps");
 
    return filteredGrasps;
}
 
GraspingPlacementList
GraspingManager::filterPlacements(const GraspingPlacementList& placements)
{
    ARMARX_VERBOSE << "Step: filter placements";
    GraspingPlacementList filteredPlacements = gsm->filterPlacements(placements);
    if (filteredPlacements.empty())
    {
        ARMARX_WARNING << " Step 'filter generated grasps' filtered out all grasps";
    }
 
    return filteredPlacements;
}
 
GraspingPlacementList
GraspingManager::generateRobotPlacements(const GeneratedGraspList& grasps)
{
    //    TODO: Why is there a variable number of robot placements for each run??
    ARMARX_VERBOSE << "Step: generate robot placements";
    GraspingPlacementList graspPlacements = rp->generateRobotPlacementsEx(grasps);
    ARMARX_CHECK_EXPRESSION(!graspPlacements.empty())
        << "No placements for the robot platform were found.";
    return graspPlacements;
}
 
GraspingTrajectory
GraspingManager::planMotion(const MotionPlanningData& mpd)
{
    ARMARX_IMPORTANT << "Robot position: " << VAROUT(mpd.globalPoseStart->output())
                     << VAROUT(mpd.globalPoseGoal->output());
 
    if (getProperty<bool>("EnableVisualization"))
    {
        entityDrawer->setPoseVisu(
            layerName, "MotionPlanningPlatformTargetPose", mpd.globalPoseGoal);
    }
    Eigen::Vector3f bbcenter;
    bbcenter << (planningBoundingBox.max.e0 + planningBoundingBox.min.e0) / 2,
        (planningBoundingBox.max.e1 + planningBoundingBox.min.e1) / 2,
        (planningBoundingBox.max.e2 + planningBoundingBox.min.e2) / 2;
    Eigen::Vector3f bbSize;
    bbSize << planningBoundingBox.max.e0 - planningBoundingBox.min.e0,
        planningBoundingBox.max.e1 - planningBoundingBox.min.e1,
        planningBoundingBox.max.e2 - planningBoundingBox.min.e2;
    //    entityDrawer->setBoxVisu(layerName, "PlanningBoundingBox", new Pose(Eigen::Matrix3f::Identity(), Eigen::Vector3f(bbcenter)), new Vector3(bbSize), DrawColor {1, 0, 0, 0.1});
    ARMARX_VERBOSE << "Step: Motion Planning";
 
    if (getProperty<bool>("EnableVisualization"))
    {
        entityDrawer->setPoseVisu("Poses", "StartPoseAgent", mpd.globalPoseStart);
    }
 
    auto robotFileName = rsc->getRobotFilename();
    auto packageName = std::filesystem::path{robotFileName}.begin()->string();
    auto axPackages = rsc->getArmarXPackages();
    ARMARX_CHECK_EXPRESSION(std::find(axPackages.cbegin(), axPackages.cend(), packageName) !=
                            axPackages.cend())
        << "Could not determine package name from path '" << robotFileName << "', "
        << "because the determined package name '" << packageName
        << "' is not in the following list: " << axPackages;
    //    SimoxCSpacePtr cspace = SimoxCSpacePtr::dynamicCast(this->cspace->clone(false));
    SimoxCSpacePtr armCSpace = SimoxCSpacePtr::dynamicCast(cspace->clone());
    //    armCSpace->addObjectsFromWorkingMemory(wm);
    //    for (auto& obj : this->cspace->getStationaryObjects())
    //    {
    //        armCSpace->addStationaryObject(obj);
    //    }
    AgentPlanningInformation agentData;
    agentData.agentPose = mpd.globalPoseGoal;
    agentData.agentProjectNames = rsc->getArmarXPackages();
    agentData.agentRelativeFilePath = robotFileName;
    //    agentData.kinemaicChainNames = robotNodeSetNames;
    agentData.kinemaicChainNames = {mpd.rnsToUse};
    auto collisionSetMapping = getJointSetCollisionSetMapping();
    std::string armCollisionSet = collisionSetMapping.at(mpd.rnsToUse);
 
    auto robotCol = cspace->getAgentSceneObj()->getRobotNodeSet(
        getProperty<std::string>("RobotCollisionNodeSet").getValue());
    auto robotColNodeNames = robotCol->getNodeNames();
    agentData.collisionObjectNames =
        getRobotNodeSetNodesWithoutAllwaysColliding(robotColNodeNames, armCollisionSet);
    agentData.collisionSetNames = {armCollisionSet};
 
    agentData.initialJointValues = localRobot->getConfig()->getRobotNodeJointValueMap();
    for (auto& pair : mpd.configStart)
    {
        agentData.initialJointValues[pair.first] = pair.second;
    }
    armCSpace->setAgent(agentData);
 
 
    SimoxCSpacePtr cspacePlatform = SimoxCSpacePtr::dynamicCast(cspace->clone());
    agentData.agentPose = mpd.globalPoseStart;
    agentData.kinemaicChainNames = {"PlatformPlanning"};
 
    agentData.collisionObjectNames = robotColNodeNames;
    agentData.collisionSetNames = {};
    for (auto& pair : collisionSetMapping)
    {
        if (std::find(agentData.collisionSetNames.begin(),
                      agentData.collisionSetNames.end(),
                      pair.second) == agentData.collisionSetNames.end())
        {
            agentData.collisionSetNames.push_back(pair.second);
            agentData.collisionObjectNames = getRobotNodeSetNodesWithoutAllwaysColliding(
                agentData.collisionObjectNames, pair.second);
        }
    }
 
 
    cspacePlatform->setAgent(agentData);
    //    cspacePlatform->addObjectsFromWorkingMemory(wm);
    //    for (auto& obj : armCSpace->getStationaryObjects())
    //    {
    //        cspacePlatform->addStationaryObject(obj);
    //    }
 
 
    //    cspacePlatform->setPoseBounds(planningBoundingBox);
 
    ARMARX_VERBOSE << "CSpace created";
 
    auto planningResult = pmp->planMotion(armCSpace, cspacePlatform, mpd);
    return planningResult;
}
 
void
GraspingManager::drawTrajectory(const GraspingTrajectory& t, float visuSlowdownFactor)
{
    try
    {
        if (visuSlowdownFactor < 0)
        {
            return;
        }
        ARMARX_VERBOSE << "Step: Draw trajectory";
 
        TrajectoryPtr poseTrajectory = TrajectoryPtr::dynamicCast(t.poseTrajectory);
        TrajectoryPtr configTrajectory = TrajectoryPtr::dynamicCast(t.configTrajectory);
        ARMARX_CHECK_EXPRESSION(poseTrajectory->size() != 0);
        ARMARX_CHECK_EXPRESSION(configTrajectory->size() != 0);
 
 
        auto robotId = newId();
 
        auto robotConfig = rsc->getSynchronizedRobot()->getConfig();
        localRobot->setJointValues(robotConfig);
 
        entityDrawer->setRobotVisu(
            layerName, robotId, rsc->getRobotFilename(), "RobotAPI", FullModel);
        entityDrawer->updateRobotColor(layerName, robotId, COLOR_ROBOT);
        entityDrawer->updateRobotPose(layerName, robotId, new Pose(localRobot->getGlobalPose()));
        entityDrawer->updateRobotConfig(
            layerName, robotId, localRobot->getConfig()->getRobotNodeJointValueMap());
 
        ARMARX_INFO << VAROUT(poseTrajectory->output());
 
 
        std::vector<PosePtr> poseData;
        ARMARX_CHECK_EXPRESSION(poseTrajectory->dim() >= 3) << "dim: " << poseTrajectory->dim();
 
        std::transform(poseTrajectory->begin(),
                       poseTrajectory->end(),
                       std::back_inserter(poseData),
                       [](const Trajectory::TrajData& data) -> PosePtr
                       {
                           Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
                           VirtualRobot::MathTools::rpy2eigen4f(0, 0, data.getPosition(2), pose);
                           pose(0, 3) = data.getPosition(0);
                           pose(1, 3) = data.getPosition(1);
                           pose(2, 3) = 1;
                           return new Pose(pose);
                       });
        ARMARX_INFO << __LINE__;
        int stepSize = 1; //std::max<int>(1, poseData.size() / 20);
        for (auto it = poseData.cbegin(); it != poseData.cend(); it += 1)
        {
            Vector3Ptr currPos = new Vector3((*it)->toEigen());
            auto nextIt = std::next(it);
            entityDrawer->setSphereVisu(layerName, newId(), currPos, COLOR_POSE_POINT, SPHERE_SIZE);
            usleep(30000 * visuSlowdownFactor);
            if (nextIt != poseData.cend())
            {
                Vector3Ptr nextPos = new Vector3((*nextIt)->toEigen());
                entityDrawer->setLineVisu(
                    layerName, newId(), currPos, nextPos, LINE_WIDTH, COLOR_POSE_LINE);
                Eigen::Matrix4f pose = localRobot->getGlobalPose();
                pose.block<3, 1>(0, 3) = nextPos->toEigen();
                entityDrawer->updateRobotPose(layerName, robotId, *nextIt);
                usleep(1000000 * visuSlowdownFactor / poseData.size());
            }
        }
        ARMARX_INFO << VAROUT(configTrajectory->output());
        auto targetPose = poseData.back()->toEigen();
        localRobot->setGlobalPose(targetPose);
 
        std::vector<NameValueMap> configData;
        ARMARX_CHECK_EXPRESSION(configTrajectory->dim() > 0);
        auto jointLabels = configTrajectory->getDimensionNames();
        std::transform(configTrajectory->begin(),
                       configTrajectory->end(),
                       std::back_inserter(configData),
                       [&](const Trajectory::TrajData& data) -> NameValueMap
                       {
                           NameValueMap result;
                           for (size_t i = 0; i < jointLabels.size(); ++i)
                           {
                               result.insert({jointLabels[i], data.getPosition(i)});
                           }
                           return result;
                       });
 
 
        VirtualRobot::RobotNodeSetPtr rns = localRobot->getRobotNodeSet(t.rnsToUse);
        ARMARX_CHECK_EXPRESSION(rns && rns->getTCP());
        const auto tcpName = rns->getTCP()->getName();
 
        std::vector<Vector3Ptr> tcpPoseList;
        std::transform(configData.cbegin(),
                       configData.cend(),
                       std::back_inserter(tcpPoseList),
                       [&](const NameValueMap& config)
                       {
                           localRobot->setJointValues(config);
                           return new Vector3(localRobot->getRobotNode(tcpName)->getGlobalPose());
                       });
 
        //        stepSize = std::max<int>(1, tcpPoseList.size() / 20);
        int i = 0;
        for (auto it = tcpPoseList.cbegin(); it != tcpPoseList.cend();
             it += stepSize, i += stepSize)
        {
            auto nextIt = std::next(it);
            auto currPose = *it;
            entityDrawer->setSphereVisu(
                layerName, newId(), currPose, COLOR_CONFIG_POINT, SPHERE_SIZE);
 
            if (nextIt != tcpPoseList.cend())
            {
                auto nextPose = *nextIt;
                entityDrawer->updateRobotConfig(layerName, robotId, configData.at(i));
                entityDrawer->setLineVisu(
                    layerName, newId(), currPose, nextPose, SPHERE_SIZE, COLOR_CONFIG_LINE);
                usleep(1000000 * visuSlowdownFactor / tcpPoseList.size());
            }
        }
 
        entityDrawer->updateRobotConfig(layerName, robotId, configData.back());
 
        ARMARX_CHECK_EXPRESSION(TCP_HAND_MAPPING.find(tcpName) != TCP_HAND_MAPPING.end())
            << "Unknown TCP '" << tcpName << "'";
        auto handObjectClass =
            prior->getObjectClassesSegment()->getObjectClassByName(TCP_HAND_MAPPING.at(tcpName));
        ARMARX_CHECK_EXPRESSION(handObjectClass) << TCP_HAND_MAPPING.at(tcpName);
 
        localRobot->setJointValues(configData.back());
        entityDrawer->setObjectVisu(layerName,
                                    handObjectClass->getName(),
                                    handObjectClass,
                                    new Pose(localRobot->getRobotNode(tcpName)->getGlobalPose()));
        entityDrawer->updateObjectColor(layerName, handObjectClass->getName(), COLOR_ROBOT);
    }
    catch (...)
    {
        armarx::handleExceptions();
    }
}
 
void
GraspingManager::setDescriptionPositionForObject(const std::string& objId)
{
    if (!objId.empty())
    {
        auto objInstance = wm->getObjectInstancesSegment()->getObjectInstanceById(objId);
        FramedPositionPtr p =
            armarx::FramedPositionPtr::dynamicCast(objInstance->getPositionBase());
        p->changeToGlobal(localRobot);
        p->z += 400;
        globalDescriptionPosition = p;
    }
}
 
void
GraspingManager::setNextStepDescription(const std::string& description)
{
    step++;
    if (getProperty<bool>("EnableVisualization"))
        entityDrawer->setTextVisu(layerName,
                                  "stepDescription",
                                  "Step " + to_string(step) + ": " + description,
                                  globalDescriptionPosition,
                                  DrawColor{0, 1, 0, 1},
                                  15);
}
 
void
GraspingManager::resetStepDescription()
{
    step = 0;
    globalDescriptionPosition = new Vector3(0, 0, 0);
    entityDrawer->removeTextVisu(layerName, "stepDescription");
}
 
std::vector<MotionPlanningData>
GraspingManager::calculateIKs(const GraspingPlacementList& graspPlacements)
{
    //    TODO: Why is the left hand grasp always discarded for the IK?
    //    Eigen::Vector3f rpy;
    //    VirtualRobot::MathTools::eigen4f2rpy(localRobot->getGlobalPose(), rpy);
    //    armarx::VectorXf startPos {localRobot->getGlobalPose()(0, 3), localRobot->getGlobalPose()(1, 3), rpy(2)};
    //    MotionPlanningServerInterfacePrx mps = this->getProxy<MotionPlanningServerInterfacePrx>("MotionPlanningServer", false, "", false);
    //    if (mps)
    //    {
    //        cspace = new SimoxCSpaceWith2DPose(prior->getCommonStorage(), false, 50);
    //        AgentPlanningInformation agentData;
    //        agentData.agentProjectNames = rsc->getArmarXPackages();
    //        agentData.agentRelativeFilePath = rsc->getRobotFilename();
    //        //    agentData.kinemaicChainNames = robotNodeSetNames;
    //        agentData.kinemaicChainNames = {};
    //        agentData.collisionSetNames = {getProperty<std::string>("RobotCollisionNodeSet").getValue()};
    //        agentData.initialJointValues = localRobot->getConfig()->getRobotNodeJointValueMap();
    //        cspace->setAgent(agentData);
    //        cspace->addObjectsFromWorkingMemory(wm);
    //        //        cspace->setStationaryObjectMargin(getProperty<float>("MinimumDistanceToEnvironment").getValue());
    //        //        cspace->initCollisionTest();
    //        SimoxCSpaceWith2DPosePtr tmpCSpace = SimoxCSpaceWith2DPosePtr::dynamicCast(cspace->clone());
    //        auto agent = tmpCSpace->getAgent();
    //        agent.agentPose = new Pose(localRobot->getGlobalPose());
    //        tmpCSpace->setAgent(agent);
 
    //        CSpaceVisualizerTaskHandle taskHandle = mps->enqueueTask(new CSpaceVisualizerTask(tmpCSpace, startPos, getDefaultName() + "Visu" + IceUtil::generateUUID()));
    //        planningTasks.push_back(taskHandle);
    //    }
 
    std::vector<MotionPlanningData> mpdList;
    //    VirtualRobot::RobotPtr localRobot = RemoteRobot::createLocalCloneFromFile(rsc, VirtualRobot::RobotIO::eStructure);
 
    Eigen::Vector3f robotPos = localRobot->getGlobalPose().block<3, 1>(0, 3);
 
    ARMARX_IMPORTANT << "Robot position: " << VAROUT(robotPos);
 
    //    auto disableGraspVisu = [&](int id)
    //    {
    //        entityDrawer->updateObjectColor("GeneratedGrasps", "GraspCandidate" + to_string(id), DrawColor {1.0, 0.0, 0.0, 0.5});
    //    };
    robotVisuId = ""; // if empty, robot visu will be created
    int i = 0;
 
    for (const GraspingPlacement& gp : graspPlacements)
    {
        NameValueMap currentConfig = rsc->getSynchronizedRobot()->getConfig();
        ARMARX_CHECK_EXPRESSION(!currentConfig.empty());
        auto graspVisuId = visualizeGrasp(gp.grasp, i);
        Eigen::Matrix4f currentRobotPose = localRobot->getGlobalPose();
        auto desiredRobotPose =
            PosePtr::dynamicCast(FramedPosePtr::dynamicCast(gp.robotPose)->toGlobal(localRobot));
        auto desiredRobotPoseEigen = desiredRobotPose->toEigen();
        // objectPose is actually the tcp pose...
        //        FramedPosePtr desiredTCPPose = FramedPosePtr::dynamicCast(gp.grasp.framedPose);
        // TODO: move along gcp z axis
        //        desiredTCPPose->changeFrame(localRobot, localRobot->getEndEffector(gp.grasp.eefName)->getGCP()->getName());
        //        desiredTCPPose = FramedPosePtr::dynamicCast(gp.grasp.framedPose)->toGlobal(localRobot);
        auto desiredTCPPose =
            FramedPosePtr::dynamicCast(gp.grasp.framedPose)->toGlobalEigen(localRobot);
        auto desiredTCPPoseRelativeToRobotEigen = desiredRobotPoseEigen.inverse() * desiredTCPPose;
        FramedPosePtr desiredTCPPoseRelativeToRobot{
            new FramedPose(desiredTCPPoseRelativeToRobotEigen,
                           localRobot->getRootNode()->getName(),
                           localRobot->getName())};
        auto desiredTCPPrepose =
            FramedPosePtr::dynamicCast(gp.grasp.framedPrePose)->toGlobalEigen(localRobot);
        auto desiredTCPPreposeRelativeToRobotEigen =
            desiredRobotPoseEigen.inverse() * desiredTCPPrepose;
        FramedPosePtr desiredTCPPreposeRelativeToRobot{
            new FramedPose(desiredTCPPreposeRelativeToRobotEigen,
                           localRobot->getRootNode()->getName(),
                           localRobot->getName())};
 
        Ice::StringSeq potentialRNs;
        auto tcpName = localRobot->getEndEffector(gp.grasp.eefName)->getTcp()->getName();
        for (const auto& rnsName : robotNodeSetNames)
        {
            ARMARX_CHECK_EXPRESSION(localRobot->hasRobotNodeSet(rnsName))
                << "Could not find RNS '" << rnsName << "' in RNS list of robot "
                << localRobot->getName();
            if (localRobot->getRobotNodeSet(rnsName)->getTCP()->getName() == tcpName)
            {
                potentialRNs.push_back(rnsName);
            }
        }
 
        float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor").getValue();
 
 
        if (potentialRNs.empty())
        {
            ARMARX_WARNING << "Could not find RNS with tcp '" << tcpName
                           << "'; will not process the corresponding generated grasp...";
            continue;
        }
        std::string rnsToUse;
        if (/*rik->isFramedPoseReachable(rnsToUse, objectPoseRelativeToRobot) || */ true)
        {
            if (getProperty<bool>("EnableVisualization"))
            {
                if (robotVisuId.empty())
                {
                    robotVisuId = newId();
                    entityDrawer->setRobotVisu(layerName,
                                               robotVisuId,
                                               rsc->getRobotFilename(),
                                               simox::alg::join(rsc->getArmarXPackages(), ","),
                                               FullModel);
                    entityDrawer->updateRobotColor(layerName, robotVisuId, COLOR_ROBOT);
                }
                entityDrawer->updateRobotColor(
                    layerName, robotVisuId, DrawColor{1.0, 1.0f, 1.0, 0.5});
                entityDrawer->updateRobotPose(layerName, robotVisuId, desiredRobotPose);
                entityDrawer->updateRobotConfig(
                    layerName, robotVisuId, localRobot->getConfig()->getRobotNodeJointValueMap());
            }
            ARMARX_VERBOSE << "Pose " << VAROUT(*desiredTCPPoseRelativeToRobot) << " with RNS '"
                           << potentialRNs << "' is reachable";
            NameValueMap ikSolution;
            for (auto& rns : potentialRNs)
            {
                ikSolution = calculateSingleIK(
                    rns, gp.grasp.eefName, gp.robotPose, desiredTCPPoseRelativeToRobot, false);
                if (ikSolution.empty())
                {
                    continue;
                }
                ikSolution = calculateSingleIK(
                    rns, gp.grasp.eefName, gp.robotPose, desiredTCPPreposeRelativeToRobot);
                if (!ikSolution.empty())
                {
                    rnsToUse = rns;
                    break;
                }
                else
                {
                    ARMARX_INFO << "Could not find collision free IK for prepose!";
                }
            }
            if (ikSolution.empty())
            {
                if (getProperty<bool>("EnableVisualization"))
                {
                    usleep(1000000 * visuSlowdownFactor);
                    auto graspVisuId = visualizeGrasp(gp.grasp, i, DrawColor{1.0, 0, 0, 1});
                    entityDrawer->updateRobotColor(
                        layerName, robotVisuId, DrawColor{1.0, 0.0f, 0.0, 1});
                    ARMARX_VERBOSE << "...but has no IK solution";
                    usleep(1000000 * visuSlowdownFactor);
                    for (auto id : graspVisuId.second)
                    {
                        entityDrawer->removeObjectVisu(graspVisuId.first, id);
                    }
                }
                else
                {
                    ARMARX_VERBOSE << "...but has no IK solution";
                }
                continue;
            }
 
            ARMARX_VERBOSE << "... and has an IK solution";
            if (getProperty<bool>("EnableVisualization"))
            {
                usleep(1000000 * visuSlowdownFactor);
                entityDrawer->updateRobotColor(
                    layerName, robotVisuId, DrawColor{0.0, 1.0f, 0.0, 1});
                entityDrawer->updateRobotPose(layerName, robotVisuId, desiredRobotPose);
                entityDrawer->updateRobotConfig(layerName, robotVisuId, ikSolution);
                usleep(2000000 * visuSlowdownFactor);
            }
            //            usleep(500000);
 
            for (auto it = currentConfig.begin(); it != currentConfig.end();)
            {
                if (ikSolution.find(it->first) == ikSolution.end())
                {
                    it = currentConfig.erase(it);
                }
                else
                {
                    ++it;
                }
            }
 
            for (const auto& entry : ikSolution)
            {
                if (currentConfig.find(entry.first) == currentConfig.end())
                {
                    ARMARX_INFO << "Current config: " << currentConfig;
                    ARMARX_INFO << "Desired config: " << ikSolution;
                    ARMARX_CHECK_EXPRESSION(false)
                        << "calculated configuration contains a joint '" << entry.first
                        << "' whose current value is unknown";
                }
            }
            ARMARX_CHECK_EXPRESSION(currentConfig.size() == ikSolution.size());
            ARMARX_CHECK_EXPRESSION(!ikSolution.empty());
            //            entityDrawer->setPoseVisu("Poses" , "TCPTargetPose" + to_string(i), objectPoseRelativeToRobot->toGlobal(rsc->getSynchronizedRobot()));
            ARMARX_INFO << VAROUT(currentRobotPose) << VAROUT(desiredRobotPose->output());
            mpdList.push_back({new FramedPose(currentRobotPose, GlobalFrame, ""),
                               desiredRobotPose,
                               currentConfig,
                               ikSolution,
                               rnsToUse,
                               gp.grasp.eefName,
                               gp.grasp});
        }
        else
        {
            ARMARX_VERBOSE << "Pose " << VAROUT(*desiredTCPPoseRelativeToRobot) << " with RNS '"
                           << potentialRNs << "' not reachable";
        }
        if (getProperty<bool>("EnableVisualization"))
            for (auto id : graspVisuId.second)
            {
                entityDrawer->removeObjectVisu(graspVisuId.first, id);
            }
        i++;
    }
    if (getProperty<bool>("EnableVisualization"))
    {
        entityDrawer->removeRobotVisu(layerName, robotVisuId);
    }
    return mpdList;
}
 
NameValueMap
GraspingManager::calculateSingleIK(const std::string& robotNodeSetName,
                                   const std::string& eef,
                                   const PoseBasePtr& globalRobotPose,
                                   const FramedPoseBasePtr& tcpPose,
                                   bool checkCollisionFree)
{
    TIMING_START(SingleIK);
    auto ikRobot = cspace->getAgentSceneObj();
    ikRobot->setConfig(localRobot->getConfig());
    auto rns = ikRobot->getRobotNodeSet(robotNodeSetName);
    ikRobot->setGlobalPose(PosePtr::dynamicCast(globalRobotPose)->toEigen());
    auto tcp = ikRobot->getEndEffector(eef)->getTcp();
    Eigen::Matrix4f targetPose = FramedPosePtr::dynamicCast(tcpPose)->toGlobalEigen(ikRobot);
    VirtualRobot::ConstrainedOptimizationIK ik(ikRobot, rns);
    VirtualRobot::PoseConstraintPtr poseConstraint(
        new VirtualRobot::PoseConstraint(ikRobot, rns, tcp, targetPose));
    if (checkCollisionFree)
    {
        auto colSetEnv = cspace->getStationaryObjectSet();
        VirtualRobot::CDManagerPtr cdm(
            new VirtualRobot::CDManager(cspace->getCD().getCollisionChecker()));
        VirtualRobot::SceneObjectSetPtr sosRns(
            new VirtualRobot::SceneObjectSet("RNS", cspace->getCD().getCollisionChecker()));
        std::string armCollisionSet = getJointSetCollisionSetMapping().at(robotNodeSetName);
        ARMARX_INFO << "Using Arm Collision Set '" << armCollisionSet << "' and platform set '"
                    << getProperty<std::string>("RobotCollisionNodeSet").getValue() << "'";
        sosRns->addSceneObjects(cspace->getAgentSceneObj()->getRobotNodeSet(armCollisionSet));
        cdm->addCollisionModelPair(colSetEnv, sosRns);
 
        VirtualRobot::SceneObjectSetPtr sosPlatform(
            new VirtualRobot::SceneObjectSet("platform", cspace->getCD().getCollisionChecker()));
 
 
        auto robotCol = cspace->getAgentSceneObj()->getRobotNodeSet(
            getProperty<std::string>("RobotCollisionNodeSet").getValue());
        auto robotColNodeNames = robotCol->getNodeNames();
        std::vector<std::string> remainingNodeNames =
            getRobotNodeSetNodesWithoutAllwaysColliding(robotColNodeNames, armCollisionSet);
        for (const auto& colNodeName : remainingNodeNames)
        {
            sosPlatform->addSceneObject(cspace->getAgentSceneObj()->getRobotNode(colNodeName));
        }
        ARMARX_INFO << "RobotCollisionNodeSet contains the following nodes after filtering allways "
                       "colliding nodes:"
                    << std::endl
                    << VAROUT(remainingNodeNames);
 
        cdm->addCollisionModelPair(sosPlatform, sosRns);
        //    ik.addConstraint(poseConstraint);
        ik.addConstraint(
            VirtualRobot::ConstraintPtr(new VirtualRobot::CollisionCheckConstraint(rns, cdm)));
    }
 
    VirtualRobot::ConstraintPtr posConstraint(
        new VirtualRobot::PositionConstraint(ikRobot,
                                             rns,
                                             tcp,
                                             targetPose.block<3, 1>(0, 3),
                                             VirtualRobot::IKSolver::CartesianSelection::All));
    posConstraint->setOptimizationFunctionFactor(1);
 
    VirtualRobot::ConstraintPtr oriConstraint(
        new VirtualRobot::OrientationConstraint(ikRobot,
                                                rns,
                                                tcp,
                                                targetPose.block<3, 3>(0, 0),
                                                VirtualRobot::IKSolver::CartesianSelection::All,
                                                VirtualRobot::MathTools::deg2rad(2)));
    oriConstraint->setOptimizationFunctionFactor(1000);
    ik.addConstraint(posConstraint);
    ik.addConstraint(oriConstraint);
 
 
    NameValueMap result;
    NaturalIKResult natIKResult =
        getNaturalIK()->solveIK(eef,
                                FramedPosePtr::dynamicCast(tcpPose)->toRootEigen(ikRobot),
                                true,
                                new armarx::aron::data::dto::Dict());
    if (!natIKResult.reached)
    {
        ARMARX_WARNING << "natik failed!";
        //return result;
    }
    else
    {
        VirtualRobot::ReferenceConfigurationConstraintPtr natIKConstraint(
            new VirtualRobot::ReferenceConfigurationConstraint(ikRobot, rns));
        ARMARX_IMPORTANT << VAROUT(natIKResult.jointValues);
        Eigen::VectorXf referenceConfig(natIKResult.jointValues.size());
        for (size_t i = 0; i < natIKResult.jointValues.size(); i++)
        {
            referenceConfig(i) = natIKResult.jointValues.at(i);
        }
        ARMARX_IMPORTANT << VAROUT(referenceConfig);
        natIKConstraint->setReferenceConfiguration(referenceConfig);
        ik.addConstraint(natIKConstraint);
    }
 
    if (!ik.initialize())
    {
        ARMARX_WARNING << "IK Initialization failed!";
        return result;
    }
    bool success = ik.solve();
    if (success)
    {
 
        for (size_t i = 0; i < rns->getSize(); ++i)
        {
            result[rns->getNode((int)i)->getName()] = rns->getNode(i)->getJointValue();
        }
        //if (PositionControllerHelper::OptimizeNullspace(tcp, rns, FramedPosePtr::dynamicCast(tcpPose)->toRootEigen(ikRobot)))
        //{
        //    for (size_t i = 0; i < rns->getSize(); ++i)
        //    {
        //        result[rns->getNode((int)i)->getName()] = rns->getNode(i)->getJointValue();
        //    }
        //}
    }
    else if (natIKResult.reached)
    {
        ARMARX_WARNING << "constrained ik failed. using natik instead.";
        for (size_t i = 0; i < rns->getSize(); ++i)
        {
            result[rns->getNode((int)i)->getName()] = natIKResult.jointValues.at(i);
        }
    }
    TIMING_END(SingleIK);
 
 
    return result;
}
 
StringStringDictionary
GraspingManager::getJointSetCollisionSetMapping()
{
    StringStringDictionary result;
    auto propString = getProperty<std::string>("JointToLinkSetMapping").getValue();
    auto pairs = armarx::Split(propString, ";", true, true);
    for (auto& pairStr : pairs)
    {
        auto pair = armarx::Split(pairStr, ":", true, true);
        ARMARX_CHECK_EXPRESSION(pair.size() == 2);
        result[pair.at(0)] = pair.at(1);
    }
    return result;
}
 
GraspingTrajectoryList
GraspingManager::generateGraspingTrajectoryListForGraspListInternal(
    const GeneratedGraspList& grasps)
{
    std::vector<MotionPlanningData> mpdList = generateIKsInternal(grasps);
    if (mpdList.empty())
    {
        ARMARX_WARNING << "Step 'check reachability / solve IK' produced no valid results";
        return GraspingTrajectoryList{};
    }
 
    //    std::vector<std::tuple<Eigen::Matrix4f, Eigen::Matrix4f, NameValueMap, NameValueMap, std::string>> sourceTargetConfigs;
 
 
    GraspingTrajectoryList result;
    setNextStepDescription("Planning motion");
 
    for (auto& graspingData : mpdList)
    {
        try
        {
            GraspingTrajectory gt = planMotion(graspingData);
 
            result.push_back(gt);
        }
        catch (...)
        {
            handleExceptions();
        }
        if (!result.empty())
        {
            break;
        }
    }
    return result;
}
 
GraspingTrajectory
GraspingManager::generateGraspingTrajectory(const std::string& objectInstanceEntityId,
                                            const Ice::Current&)
{
    ScopedLock lock(graspManagerMutex);
    ARMARX_ON_SCOPE_EXIT
    {
        resetStepDescription();
        entityDrawer->clearLayer(layerName);
    };
    RemoteRobot::synchronizeLocalClone(localRobot, rsc);
    setDescriptionPositionForObject(objectInstanceEntityId);
 
    float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor");
 
    auto grasps = generateGrasps(objectInstanceEntityId);
 
    auto result = generateGraspingTrajectoryListForGraspListInternal(grasps);
    if (result.empty())
    {
        throw LocalException("Could not find any valid solution");
    }
 
    if (getProperty<bool>("EnableVisualization"))
    {
        drawTrajectory(result.front(), visuSlowdownFactor);
        sleep(2 * visuSlowdownFactor);
        entityDrawer->removeLayer(layerName);
    }
    return result.front();
}
 
GraspingTrajectoryList
GraspingManager::generateGraspingTrajectoryList(const std::string& objectInstanceEntityId,
                                                const Ice::Current&)
{
    ScopedLock lock(graspManagerMutex);
    ARMARX_ON_SCOPE_EXIT
    {
        resetStepDescription();
        entityDrawer->clearLayer(layerName);
    };
    RemoteRobot::synchronizeLocalClone(localRobot, rsc);
    setDescriptionPositionForObject(objectInstanceEntityId);
 
    auto grasps = generateGrasps(objectInstanceEntityId);
    return generateGraspingTrajectoryListForGraspListInternal(grasps);
}
 
GraspingTrajectoryList
GraspingManager::generateGraspingTrajectoryListForGraspList(const GeneratedGraspList& grasps,
                                                            const Ice::Current&)
{
    if (grasps.empty())
        return GraspingTrajectoryList{};
    ScopedLock lock(graspManagerMutex);
    ARMARX_ON_SCOPE_EXIT
    {
        resetStepDescription();
        entityDrawer->clearLayer(layerName);
    };
    RemoteRobot::synchronizeLocalClone(localRobot, rsc);
    FramedPosePtr pose =
        FramedPosePtr::dynamicCast(grasps.front().framedPose)->toGlobal(localRobot);
    pose->position->z += 400;
    globalDescriptionPosition = Vector3Ptr::dynamicCast(pose->position);
 
    auto result = generateGraspingTrajectoryListForGraspListInternal(grasps);
    if (result.empty())
    {
        throw LocalException("Could not find any valid solution");
    }
 
    if (getProperty<bool>("EnableVisualization"))
    {
        float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor");
        drawTrajectory(result.front(), visuSlowdownFactor);
        sleep(2 * visuSlowdownFactor);
        entityDrawer->removeLayer(layerName);
    }
    return result;
}
 
void
GraspingManager::visualizeGraspingTrajectory(const GraspingTrajectory& trajectory,
                                             float visuSlowdownFactor,
                                             const Ice::Current&)
{
    drawTrajectory(trajectory, visuSlowdownFactor);
    sleep(2 * visuSlowdownFactor);
    entityDrawer->removeLayer(layerName);
}
 
MotionPlanningDataList
GraspingManager::generateIKs(const std::string& objectInstanceEntityId, const Ice::Current&)
{
    ScopedLock lock(graspManagerMutex);
    ARMARX_ON_SCOPE_EXIT
    {
        resetStepDescription();
        entityDrawer->clearLayer(layerName);
    };
    RemoteRobot::synchronizeLocalClone(localRobot, rsc);
    setDescriptionPositionForObject(objectInstanceEntityId);
    auto grasps = generateGrasps(objectInstanceEntityId);
    return generateIKsInternal(grasps);
}
 
GeneratedGraspList
GraspingManager::generateGraspsByObjectName(const std::string& objectName, const Ice::Current&)
{
    GeneratedGraspList grasps = gg->generateGraspsByObjectName(objectName);
    std::sort(grasps.begin(),
              grasps.end(),
              [](const GeneratedGrasp& l, const GeneratedGrasp& r) { return l.score < r.score; });
    return grasps;
}
 
MotionPlanningDataList
GraspingManager::generateIKsInternal(const GeneratedGraspList& grasps)
{
    ikRobot->setConfig(localRobot->getConfig());
    auto filteredGrasps = filterGrasps(grasps);
 
    GraspingPlacementList graspPlacements;
 
    cspace = createCSpace();
 
    setNextStepDescription("Robot placement");
    const float maxDistance2D = getProperty<float>("MaxDistanceForDirectGrasp").getValue();
    GeneratedGraspList notDirectGrasps;
    for (const auto& g : filteredGrasps)
    {
        const Eigen::Vector3f position =
            FramedPosePtr::dynamicCast(g.framedPose)->getPosition()->toGlobalEigen(localRobot);
        const float distance2D =
            (position.head(2) - localRobot->getGlobalPose().block<2, 1>(0, 3)).norm();
        ARMARX_VERBOSE << "2D Distance to object: " << distance2D;
        // if already close to object, try to grasp directly without platform movement
        if (distance2D < maxDistance2D)
        {
            GraspingPlacement pl;
            pl.grasp = g;
            pl.robotPose = new FramedPose(localRobot->getGlobalPose(), armarx::GlobalFrame, "");
            graspPlacements.push_back(pl);
        }
        else
        {
            notDirectGrasps.push_back(g);
        }
    }
    if (!notDirectGrasps.empty())
    {
        auto graspPlacementsNotDirect = generateRobotPlacements(notDirectGrasps);
        graspPlacements.insert(graspPlacements.end(),
                               graspPlacementsNotDirect.begin(),
                               graspPlacementsNotDirect.end());
    }
    GraspingPlacementList filteredGraspPlacements = filterPlacements(graspPlacements);
 
    ARMARX_VERBOSE << "Step: check reachability / solve IK from current pose";
    setNextStepDescription("Calculating IK");
    return calculateIKs(filteredGraspPlacements);
}
 
std::vector<std::string>
GraspingManager::getRobotNodeSetNodesWithoutAllwaysColliding(
    const std::vector<std::string>& robotColNodeNames,
    const std::string& armCollisionSet)
{
    const auto rnsColNames =
        cspace->getAgentSceneObj()->getRobotNodeSet(armCollisionSet)->getNodeNames();
    std::set<std::string> rnsColNamesIncludingIgnore;
    rnsColNamesIncludingIgnore.insert(rnsColNames.begin(), rnsColNames.end());
    for (const auto& rnsNodeName : rnsColNames)
    {
        const auto rn = cspace->getAgentSceneObj()->getRobotNode(rnsNodeName);
        const auto ignored = rn->getIgnoredCollisionModels();
        rnsColNamesIncludingIgnore.insert(ignored.begin(), ignored.end());
        rnsColNamesIncludingIgnore.insert(rn->getParent()->getName());
    }
 
    std::vector<std::string> remainingNodeNames;
    for (const auto& colNodeName : robotColNodeNames)
    {
        const auto it = rnsColNamesIncludingIgnore.find(colNodeName);
        if (it == rnsColNamesIncludingIgnore.end())
        {
            remainingNodeNames.push_back(colNodeName);
        }
    }
    return remainingNodeNames;
}