SimpleRobotPlacement.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::SimpleRobotPlacement
 * @author     Harry Arnst (harry dot arnst at student dot kit dot edu),
 *             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 "SimpleRobotPlacement.h"
 
#include <VirtualRobot/Grasping/Grasp.h>
#include <VirtualRobot/Grasping/GraspSet.h>
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Workspace/Manipulability.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/util/algorithm.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
//#include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
#include <RobotComponents/components/MotionPlanning/CSpace/VoxelGridCSpace.h>
//#include <RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Task.h>
#include <math.h>
 
#include <IceUtil/UUID.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/math/Helpers.h>
 
#include <VisionX/interface/components/VoxelGridProviderInterface.h>
 
#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
 
using namespace armarx;
using namespace memoryx;
using namespace VirtualRobot;
 
static const DrawColor COLOR_ROBOT{1.0f, 1.0f, 0.5f, 1.0f};
 
SimpleRobotPlacementPropertyDefinitions::SimpleRobotPlacementPropertyDefinitions(
    std::string prefix) :
    ComponentPropertyDefinitions(prefix)
{
    defineOptionalProperty<std::string>("RobotName", "Armar3", "Name of the robot to use");
    defineOptionalProperty<std::string>(
        "RobotFilePath", "RobotAPI/robots/Armar3/ArmarIII.xml", "File path of the robot to use");
    defineOptionalProperty<std::string>(
        "CollisionModel", "PlatformTorsoHeadColModel", "Collisionmodel of the robot");
    defineOptionalProperty<std::string>(
        "WorkspaceFilePaths",
        "Armar3/reachability/reachability_right_hand_smoothened.bin;Armar3/reachability/"
        "reachability_left_hand_smoothened.bin",
        "Paths to manipulability and reachability files (';' delimited)");
    //            defineOptionalProperty<bool>("VisualizeCollisionSpace",
    //                                         false,
    //                                         "If true adds cspace task to MotionPlanning Server");
    defineOptionalProperty<float>("MinimumDistanceToEnvironment",
                                  0.0f,
                                  "Minimum distance to the environment for all robot placements. "
                                  "Much faster if set to zero.");
    defineOptionalProperty<float>("VisualizationSlowdownFactor",
                                  1.0f,
                                  "1.0 is a good value for clear visualization, 0 the "
                                  "visualization should not slow down the process",
                                  PropertyDefinitionBase::eModifiable);
    defineOptionalProperty<bool>("EnableVisualization",
                                 true,
                                 "If false no visualization is done.",
                                 PropertyDefinitionBase::eModifiable);
    defineOptionalProperty<int>("PlacmentsPerGrasp",
                                3,
                                "Number of robot placement that will be generated for each grasp",
                                PropertyDefinitionBase::eModifiable);
    defineOptionalProperty<float>(
        "MinManipulabilityDecreaseFactor",
        0.9f,
        "Min initial manipulability in relation to max manipulabity value and factor by which this "
        "threshold is decreased each trial",
        PropertyDefinitionBase::eModifiable);
 
    defineOptionalProperty<bool>("UseVoxelGridCSpace",
                                 false,
                                 "If true, the VoxelGridCSpace is used instead of the SimoxCSpace.",
                                 PropertyDefinitionBase::eModifiable);
    defineOptionalProperty<std::string>("VoxelGridProviderName",
                                        "VoxelGridProvider",
                                        "Name of the Voxel Grid Provider",
                                        PropertyDefinitionBase::eModifiable);
 
    defineOptionalProperty<std::string>(
        "EefNamePreferenceFilter",
        "R",
        "Prefer grasps where eef name contains this name by setting grasp success probability to "
        "1. Set to empty string to disable.");
    defineOptionalProperty<float>("MinPlacementDistance",
                                  400,
                                  "Minimum Distance for a Placement",
                                  PropertyDefinitionBase::eModifiable);
    defineOptionalProperty<float>("MaxPlacementDistance",
                                  1000,
                                  "Maximum Distance for a Placement",
                                  PropertyDefinitionBase::eModifiable);
}
 
void
SimpleRobotPlacement::onInitComponent()
{
    drawRobotId = "local_robot_";
    visuLayerName = "SimpleRobotPlacement";
 
    robotName = getProperty<std::string>("RobotName").getValue();
    robotFilePath = getProperty<std::string>("RobotFilePath").getValue();
    // retrieve absolute robot file path
    if (!ArmarXDataPath::getAbsolutePath(robotFilePath, robotFilePath))
    {
        ARMARX_ERROR << "Could not find robot file: " << robotFilePath;
    }
 
    workspaceFilePaths =
        armarx::Split(getProperty<std::string>("WorkspaceFilePaths").getValue(), ";");
    // retrieve absolute workspace file paths
    for (std::string& path : workspaceFilePaths)
    {
        std::string packageName = std::filesystem::path{path}.begin()->string();
        ARMARX_CHECK_EXPRESSION(!packageName.empty())
            << "Workspace file path '" << path
            << "' could not be parsed correctly, because package name is empty";
        armarx::CMakePackageFinder project(packageName);
        path = project.getDataDir() + "/" + path;
        if (!std::filesystem::exists(path))
        {
            throw LocalException("File not found at ") << path;
        }
    }
 
    colModel = getProperty<std::string>("CollisionModel").getValue();
 
    usingProxy("WorkingMemory");
    usingProxy("PriorKnowledge");
 
    usingProxy("RobotIK");
    usingProxy("RobotStateComponent");
    offeringTopic("DebugDrawerUpdates");
}
 
void
SimpleRobotPlacement::onConnectComponent()
{
    srand(IceUtil::Time::now().toSeconds());
    getProxy(wm, "WorkingMemory");
    getProxy(prior, "PriorKnowledge");
 
    getProxy(rik, "RobotIK");
    getProxy(robotStateComponentPrx, "RobotStateComponent");
 
    objectInstances = wm->getObjectInstancesSegment();
    agentInstances = wm->getAgentInstancesSegment();
    objectClasses = prior->getObjectClassesSegment();
 
    fileManager =
        memoryx::GridFileManagerPtr(new memoryx::GridFileManager(prior->getCommonStorage()));
 
    debugDrawerPrx = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
    entityDrawerPrx = getTopic<memoryx::EntityDrawerInterfacePrx>("DebugDrawerUpdates");
 
    cacheCSpace = SimoxCSpace::PrefetchWorkingMemoryObjects(
        wm, prior->getCommonStorage(), robotStateComponentPrx);
    loadRobot();
    loadWorkspaces();
}
 
void
SimpleRobotPlacement::onDisconnectComponent()
{
}
 
void
SimpleRobotPlacement::onExitComponent()
{
}
 
GraspingPlacementList
SimpleRobotPlacement::generateRobotPlacements(const GeneratedGraspList& grasps, const Ice::Current&)
{
    //    TIMING_START(RobotPlacement);
    planningTasks.clear();
    GraspingPlacementList result;
    AgentInstancePtr agent =
        AgentInstancePtr::dynamicCast(agentInstances->getAgentInstanceByName(robotName));
    std::shared_ptr<RemoteRobot> remoteRobot(
        new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
    robot->setGlobalPose(remoteRobot->getGlobalPose());
 
    // first, we get rid of all generated grasps, whose tcp is not given in any of the preloaded workspaces
    GeneratedGraspList filteredGrasps = filterGrasps(grasps);
    visualizedGrid.reset();
    entityDrawerPrx->clearLayer(visuLayerName);
 
    // init collision space
    RemoteRobot::synchronizeLocalClone(robot, robotStateComponentPrx);
    if (getProperty<bool>("UseVoxelGridCSpace").getValue())
    {
        cspace =
            new VoxelGridCSpace(getProxy<visionx::VoxelGridProviderInterfacePrx>(
                                    getProperty<std::string>("VoxelGridProviderName").getValue()),
                                prior->getCommonStorage());
    }
    else
    {
        cspace = new SimoxCSpace(prior->getCommonStorage(), false, 30);
    }
    cspace->addObjectsFromWorkingMemory(wm);
    AgentPlanningInformation agentData;
    agentData.agentProjectNames = robotStateComponentPrx->getArmarXPackages();
    agentData.agentRelativeFilePath = robotStateComponentPrx->getRobotFilename();
    //    agentData.kinemaicChainNames = robotNodeSetNames;
    agentData.kinemaicChainNames = {};
    agentData.collisionSetNames = {colModel};
    agentData.initialJointValues = robot->getConfig()->getRobotNodeJointValueMap();
    cspace->setAgent(agentData);
    cspace->setStationaryObjectMargin(
        getProperty<float>("MinimumDistanceToEnvironment").getValue());
    cspace->initCollisionTest();
 
    int placementsPerGrasp = getProperty<int>("PlacmentsPerGrasp");
    ARMARX_VERBOSE << "Searching " << placementsPerGrasp << " poses for " << filteredGrasps.size()
                   << " grasps";
    for (const auto& g : filteredGrasps)
    {
        Eigen::Matrix4f graspPose = FramedPosePtr::dynamicCast(g.framedPose)->toGlobalEigen(robot);
        /**
         *  Goes through each grasp and creates an empty map with PlacementCandidates. These placementCandidates are
         *  filled with placementsPerGrasp pairs of score <-> GraspingPlacement. When enough pairs have been created,
         *  the candidate with the highest score is added to result.
         **/
        std::map<float, GraspingPlacement> placementCandidates;
        for (int i = 0; i < placementsPerGrasp; ++i)
        {
            //        std::transform(filteredGrasps.begin(), filteredGrasps.end(), std::back_inserter(placementCandidates), [&](const GeneratedGrasp & g)
 
            FramedPosePtr newPose = new FramedPose(robot->getGlobalPose(), GlobalFrame, "");
 
            // create a workspace grid and find a suitable position
            TIMING_START(CreateWorksspaceGrid);
            VirtualRobot::WorkspaceGridPtr reachGrid = createWorkspaceGrid(g, graspPose);
            TIMING_END(CreateWorksspaceGrid);
            TIMING_START(DrawWorksspaceGrid);
            drawWorkspaceGrid(visualizedGrid);
            //            sleep(2);
            TIMING_END(DrawWorksspaceGrid);
 
 
            // bool return value is ignored. if placement fails -> random pose???
            float xGoal, yGoal, platformRotation;
            int score;
            getSuitablePosition(g, reachGrid, graspPose, xGoal, yGoal, platformRotation, score);
 
            newPose->position->x = xGoal;
            newPose->position->y = yGoal;
            Eigen::Matrix4f newPoseEigen = newPose->toGlobalEigen(robot);
 
            // we rotate the pose around its local z axis
            float x = newPoseEigen(0, 3);
            float y = newPoseEigen(1, 3);
            float z = newPoseEigen(2, 3);
            newPoseEigen.block<3, 1>(0, 3) = Eigen::Vector3f::Zero();
            Eigen::Affine3f transform;
            transform = Eigen::Translation<float, 3>(x, y, z) *
                        Eigen::AngleAxisf(platformRotation, Eigen::Vector3f(0, 0, 1));
            newPoseEigen = transform * newPoseEigen;
 
            //        drawRobot(newPoseEigen);
 
            armarx::FramedPosePtr resultPose(new FramedPose(newPoseEigen, GlobalFrame, ""));
            ARMARX_INFO << "Inserting robot placement: " << resultPose->output();
            placementCandidates[score] = GraspingPlacement{g, resultPose, 0};
        }
        if (!placementCandidates.empty())
        {
            /*
             * TODO: Is it the expected behavior that only the best placementCandidate is returned for each grasp??
             * Should it rather return placementPerGrasp candidates for each grasp?
             **/
            result.push_back(placementCandidates.rbegin()->second);
        }
    }
    Eigen::Matrix4f inverseRobotPose = robot->getGlobalPose().inverse();
    std::map<std::pair<float, float>, GraspingPlacement> orderedMap;
    std::string EefNamePreferenceFilter = getProperty<std::string>("EefNamePreferenceFilter");
    for (GraspingPlacement& gp : result)
    {
        Eigen::Matrix4f mat =
            inverseRobotPose * FramedPosePtr::dynamicCast(gp.robotPose)->toEigen();
        Eigen::AngleAxisf aa(mat.block<3, 3>(0, 0));
        float distanceLhs = mat.block<3, 1>(0, 3).norm() * aa.angle() * 50;
 
        // secondhands demo...
        float handPreferenceScore =
            Contains(gp.grasp.eefName, EefNamePreferenceFilter, true) ? 0.f : 1.f;
        orderedMap[{handPreferenceScore, distanceLhs}] = gp;
    }
 
    //    TIMING_END(RobotPlacement);
    entityDrawerPrx->removeLayer(visuLayerName);
    return getMapValues<>(orderedMap);
}
 
struct PlacementInfo
{
    GeneratedGrasp grasp;
    Eigen::Matrix4f graspPose;
    std::vector<SimpleRobotPlacement::RobotPlacement> placements;
    VirtualRobot::WorkspaceGridPtr reachGrid;
 
    void
    sortplacements()
    {
        std::sort(placements.begin(),
                  placements.end(),
                  [](auto& a, auto& b) { return a.score > b.score; });
    }
 
    std::pair<float, float> sortingInfo{0, 0};
    armarx::FramedPosePtr resultPose;
};
 
GraspingPlacementList
SimpleRobotPlacement::generateRobotPlacementsEx(const GeneratedGraspList& grasps,
                                                const Ice::Current&)
{
    //    TIMING_START(RobotPlacement);
    planningTasks.clear();
    GraspingPlacementList result;
    AgentInstancePtr agent =
        AgentInstancePtr::dynamicCast(agentInstances->getAgentInstanceByName(robotName));
    RemoteRobotPtr remoteRobot(new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
    robot->setGlobalPose(remoteRobot->getGlobalPose());
 
    // first, we get rid of all generated grasps, whose tcp is not given in any of the preloaded workspaces
    GeneratedGraspList filteredGrasps = filterGrasps(grasps);
    visualizedGrid.reset();
    entityDrawerPrx->clearLayer(visuLayerName);
 
    // init collision space
    RemoteRobot::synchronizeLocalClone(robot, robotStateComponentPrx);
    if (getProperty<bool>("UseVoxelGridCSpace").getValue())
    {
        cspace =
            new VoxelGridCSpace(getProxy<visionx::VoxelGridProviderInterfacePrx>(
                                    getProperty<std::string>("VoxelGridProviderName").getValue()),
                                prior->getCommonStorage());
    }
    else
    {
        cspace = new SimoxCSpace(prior->getCommonStorage(), false, 30);
    }
    cspace->addObjectsFromWorkingMemory(wm);
    AgentPlanningInformation agentData;
    agentData.agentProjectNames = robotStateComponentPrx->getArmarXPackages();
    agentData.agentRelativeFilePath = robotStateComponentPrx->getRobotFilename();
    //    agentData.kinemaicChainNames = robotNodeSetNames;
    agentData.kinemaicChainNames = {};
    agentData.collisionSetNames = {colModel};
    agentData.initialJointValues = robot->getConfig()->getRobotNodeJointValueMap();
    cspace->setAgent(agentData);
    cspace->setStationaryObjectMargin(
        getProperty<float>("MinimumDistanceToEnvironment").getValue());
    cspace->initCollisionTest();
 
    int placementsPerGrasp = getProperty<int>("PlacmentsPerGrasp");
    ARMARX_VERBOSE << "Searching " << placementsPerGrasp << " poses for " << filteredGrasps.size()
                   << " grasps";
 
    std::vector<PlacementInfo> pis;
    for (const auto& g : filteredGrasps)
    {
        PlacementInfo pi;
        pi.grasp = g;
        pi.graspPose = FramedPosePtr::dynamicCast(g.framedPose)->toGlobalEigen(robot);
        pi.reachGrid = createWorkspaceGrid(g, pi.graspPose);
        drawWorkspaceGrid(visualizedGrid);
        pis.push_back(pi);
    }
 
    int maxIterations = 1;
    for (int n = 0; n < maxIterations; n++)
    {
        for (PlacementInfo& pi : pis)
        {
            std::vector<SimpleRobotPlacement::RobotPlacement> placements =
                getSuitablePositions(pi.grasp, pi.reachGrid, pi.graspPose, 0.1, 10, 100);
            pi.placements.insert(pi.placements.end(), placements.begin(), placements.end());
        }
        //requiredManipulabilityFraction *= minManipulabilityDecreaseFactor;
    }
 
    std::string EefNamePreferenceFilter = getProperty<std::string>("EefNamePreferenceFilter");
 
    for (PlacementInfo& pi : pis)
    {
        if (pi.placements.size() == 0)
        {
            continue;
        }
        pi.sortplacements();
        for (RobotPlacement rp : pi.placements)
        {
            ARMARX_IMPORTANT << VAROUT(rp.score);
        }
        RobotPlacement rp = pi.placements.at(0);
        Eigen::Matrix4f robotpose = ::math::Helpers::CreatePose(
            Eigen::Vector3f(rp.x, rp.y, 0),
            Eigen::AngleAxisf(rp.z, Eigen::Vector3f::UnitZ()).toRotationMatrix());
        pi.resultPose = new FramedPose(robotpose, GlobalFrame, "");
        pi.sortingInfo.first =
            Contains(pi.grasp.eefName, EefNamePreferenceFilter, true) ? 0.f : 1.f;
        pi.sortingInfo.second = -rp.score;
    }
    entityDrawerPrx->removeLayer(visuLayerName);
    std::sort(
        pis.begin(), pis.end(), [](auto& a, auto& b) { return a.sortingInfo < b.sortingInfo; });
    for (PlacementInfo& pi : pis)
    {
        if (pi.placements.size() > 0)
        {
            GraspingPlacement gp;
            gp.grasp = pi.grasp;
            gp.score = pi.placements.at(0).score;
            gp.robotPose = pi.resultPose;
            //ARMARX_IMPORTANT
            result.push_back(gp);
        }
    }
    return result;
}
 
GraspingPlacementList
SimpleRobotPlacement::generateRobotPlacementsForGraspPose(const std::string& endEffectorName,
                                                          const FramedPoseBasePtr& target,
                                                          const PlanarObstacleList& planarObstacles,
                                                          const ConvexHull& placementArea,
                                                          const Ice::Current& c)
{
    planningTasks.clear();
    GraspingPlacementList result;
 
    if (!hasWorkspace(endEffectorName))
    {
        ARMARX_ERROR << "No pre-loaded workspace found for EEF '" << endEffectorName << "'";
        return result;
    }
 
    FramedPosePtr target_pose = FramedPosePtr::dynamicCast(target);
    target_pose->changeToGlobal(robotStateComponentPrx->getSynchronizedRobot());
 
    std::shared_ptr<RemoteRobot> remoteRobot(
        new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
    robot->setGlobalPose(remoteRobot->getGlobalPose());
    RemoteRobot::synchronizeLocalClone(robot, robotStateComponentPrx);
 
    AgentPlanningInformation agentData;
    agentData.agentProjectNames = robotStateComponentPrx->getArmarXPackages();
    agentData.agentRelativeFilePath = robotStateComponentPrx->getRobotFilename();
    agentData.kinemaicChainNames = {};
    agentData.collisionSetNames = {colModel};
    agentData.initialJointValues = robot->getConfig()->getRobotNodeJointValueMap();
 
    visualizedGrid.reset();
    entityDrawerPrx->clearLayer(visuLayerName);
 
    // Initialize Simox collision space
    if (getProperty<bool>("UseVoxelGridCSpace").getValue())
    {
        cspace =
            new VoxelGridCSpace(getProxy<visionx::VoxelGridProviderInterfacePrx>(
                                    getProperty<std::string>("VoxelGridProviderName").getValue()),
                                prior->getCommonStorage());
    }
    else
    {
        cspace = new SimoxCSpace(prior->getCommonStorage(), false, 30);
        cspace->addObjectsFromWorkingMemory(wm);
    }
    cspace->setAgent(agentData);
    cspace->setStationaryObjectMargin(
        getProperty<float>("MinimumDistanceToEnvironment").getValue());
 
 
    for (auto& obstacle : planarObstacles)
    {
        std::vector<Eigen::Vector3f> plane;
 
        for (auto& p : obstacle)
        {
            plane.push_back(FramedPositionPtr::dynamicCast(p)->toGlobalEigen(
                robotStateComponentPrx->getSynchronizedRobot()));
        }
 
        cspace->addPlanarObject(plane);
    }
 
    cspace->initCollisionTest();
 
    GeneratedGrasp g;
    g.score = 1;
    g.eefName = endEffectorName;
    g.framedPose = target_pose;
 
    int placmentsPerGrasp = getProperty<int>("PlacmentsPerGrasp");
    ARMARX_INFO << "Searching " << placmentsPerGrasp << " poses for EEF pose "
                << target_pose->toEigen();
 
    // Construct placement area as convex hull
    VirtualRobot::MathTools::ConvexHull2DPtr placementArea_ch;
    if (placementArea.size() > 2)
    {
        std::vector<Eigen::Vector2f> area;
        for (auto& p : placementArea)
        {
            area.push_back(FramedPositionPtr::dynamicCast(p)->toGlobalEigen(robot).head(2));
        }
        placementArea_ch = VirtualRobot::MathTools::createConvexHull2D(area);
 
        ARMARX_INFO << "Suitable placement area:";
        for (auto& p : placementArea_ch->vertices)
        {
            ARMARX_INFO << p;
        }
    }
 
    for (int i = 0; i < placmentsPerGrasp; ++i)
    {
        // Create Simox workspace grid
        VirtualRobot::WorkspaceGridPtr grid = createWorkspaceGrid(g, target_pose->toEigen());
        drawWorkspaceGrid(visualizedGrid);
 
        float xGoal, yGoal, platformRotation;
        int score;
        getSuitablePosition(g,
                            grid,
                            target_pose->toEigen(),
                            xGoal,
                            yGoal,
                            platformRotation,
                            score,
                            placementArea_ch);
 
        if (xGoal == 0 && yGoal == 0 && platformRotation == 0)
        {
            // This indicates that no suitable pose has been found
            continue;
        }
 
        FramedPosePtr newPose = new FramedPose(robot->getGlobalPose(), GlobalFrame, "");
        newPose->position->x = xGoal;
        newPose->position->y = yGoal;
        Eigen::Matrix4f newPoseEigen = newPose->toGlobalEigen(robot);
 
        // we rotate the pose around its local z axis
        float x = newPoseEigen(0, 3);
        float y = newPoseEigen(1, 3);
        float z = newPoseEigen(2, 3);
        newPoseEigen.block<3, 1>(0, 3) = Eigen::Vector3f::Zero();
        Eigen::Affine3f transform;
        transform = Eigen::Translation<float, 3>(x, y, z) *
                    Eigen::AngleAxisf(platformRotation, Eigen::Vector3f(0, 0, 1));
        newPoseEigen = transform * newPoseEigen;
 
        armarx::FramedPosePtr resultPose(new FramedPose(newPoseEigen, GlobalFrame, ""));
        result.push_back(GraspingPlacement{g, resultPose, score});
    }
 
    entityDrawerPrx->removeLayer(visuLayerName);
    return result;
}
 
VirtualRobot::WorkspaceRepresentationPtr
SimpleRobotPlacement::getWorkspaceRepresentation(GeneratedGrasp const& g)
{
 
    for (VirtualRobot::WorkspaceRepresentationPtr workspace : workspaces)
    {
        if (workspace->getTCP()->getName() == robot->getEndEffector(g.eefName)->getTcp()->getName())
        {
            return workspace;
            break;
        }
    }
    return VirtualRobot::WorkspaceRepresentationPtr();
}
 
VirtualRobot::WorkspaceGridPtr
SimpleRobotPlacement::createWorkspaceGrid(GeneratedGrasp g, const Eigen::Matrix4f& globalObjectPose)
{
    static int counter = 0;
    std::string graspName = "some_random_grasp_" + to_string(counter++);
    std::string robotType = robotName;
    std::string eef = g.eefName;
 
    // dummy manipulation object
    VirtualRobot::ManipulationObjectPtr dummyObject(new ManipulationObject("dummyObject"));
    dummyObject->setGlobalPose(globalObjectPose);
 
    // dummy grasp
    Eigen::Matrix4f tcpPoseGlobal = FramedPosePtr::dynamicCast(g.framedPose)->toGlobalEigen(robot);
    Eigen::Matrix4f tcpPrePoseGlobal =
        FramedPosePtr::dynamicCast(g.framedPrePose)->toGlobalEigen(robot);
    Eigen::Matrix4f objectPoseInTcpFrame = tcpPoseGlobal.inverse() * globalObjectPose;
    VirtualRobot::GraspPtr dummyGrasp(new Grasp(graspName, robotType, eef, objectPoseInTcpFrame));
    VirtualRobot::GraspPtr dummyPrepose(
        new Grasp(graspName, robotType, eef, tcpPrePoseGlobal.inverse() * globalObjectPose));
 
    VirtualRobot::WorkspaceRepresentationPtr ws;
    // find a workspace whose tcp is equal to the tcp of the generated grasp
    ws = getWorkspaceRepresentation(g);
 
    // create workspace grid and fill it
    Eigen::Vector3f minBB, maxBB;
    ws->getWorkspaceExtends(minBB, maxBB);
    VirtualRobot::WorkspaceGridPtr reachGridPrepose;
    reachGridPrepose.reset(new WorkspaceGrid(
        minBB(0), maxBB(0), minBB(1), maxBB(1), ws->getDiscretizeParameterTranslation()));
    reachGridPrepose->setGridPosition(globalObjectPose(0, 3), globalObjectPose(1, 3));
    reachGridPrepose->fillGridData(ws, dummyObject, dummyPrepose, robot->getRootNode());
 
    //    reachGrid->fillGridData(ws, dummyObject, dummyPrepose, robot->getRootNode());
    VirtualRobot::WorkspaceGridPtr reachGridGrasp;
    reachGridGrasp.reset(new WorkspaceGrid(
        minBB(0), maxBB(0), minBB(1), maxBB(1), ws->getDiscretizeParameterTranslation()));
    reachGridGrasp->setGridPosition(globalObjectPose(0, 3), globalObjectPose(1, 3));
    ARMARX_INFO << " grasp pose: " << dummyGrasp->getTcpPoseGlobal(globalObjectPose);
    ARMARX_INFO << " prepose pose: " << dummyPrepose->getTcpPoseGlobal(globalObjectPose);
    reachGridGrasp->fillGridData(ws, dummyObject, dummyGrasp, robot->getRootNode());
    //    drawWorkspaceGrid(reachGridGrasp);
    //    debugDrawerPrx->setTextVisu(visuLayerName, "GridLabel", "GraspGrid", new Vector3(dummyObject->getGlobalPose()(0, 3), dummyObject->getGlobalPose()(1, 3), 500), DrawColor {0, 0, 1, 1}, 30);
    //    sleep(2);
    //    //    if (!visualizedGrid)
    //    {
 
    //        visualizedGrid.reset(new WorkspaceGrid(minBB(0), maxBB(0), minBB(1), maxBB(1), ws->getDiscretizeParameterTranslation()));
    TIMING_START(GridMerge);
    visualizedGrid =
        reachGridGrasp; //VirtualRobot::WorkspaceGrid::MergeWorkspaceGrids({reachGridGrasp, reachGridPrepose});
    TIMING_END(GridMerge);
    //    visualizedGrid->setGridPosition(globalObjectPose(0, 3), globalObjectPose(1, 3));
    //    }
    //    visualizedGrid->fillGridData(ws, dummyObject, dummyGrasp, robot->getRootNode());
 
    //    drawWorkspaceGrid(visualizedGrid);
    //    debugDrawerPrx->setTextVisu(visuLayerName, "GridLabel", "MergedGrid", new Vector3(dummyObject->getGlobalPose()(0, 3), dummyObject->getGlobalPose()(1, 3), 500), DrawColor {0, 0, 1, 1}, 30);
    //    sleep(2);
    //    visualizedGrid->fillGridData(ws, dummyObject, dummyPrepose, robot->getRootNode());
    //    drawWorkspaceGrid(visualizedGrid);
    //    sleep(2);
    return reachGridGrasp;
    //    return VirtualRobot::WorkspaceGrid::MergeWorkspaceGrids({reachGridGrasp, reachGridPrepose});
}
 
bool
SimpleRobotPlacement::getSuitablePosition(
    const GeneratedGrasp& g,
    VirtualRobot::WorkspaceGridPtr reachGrid,
    const Eigen::Matrix4f& globalObjectPose,
    float& storeGlobalX,
    float& storeGlobalY,
    float& storeGlobalYaw,
    int& score,
    const VirtualRobot::MathTools::ConvexHull2DPtr& placementArea)
{
 
    // robot pose
    Eigen::Matrix4f originalRobotPoseGlobal = robot->getGlobalPose();
    Eigen::Matrix4f tmpRobotPoseGlobal = originalRobotPoseGlobal;
 
    // workspace grid params, workspace is a grid filled with reachability scores for each x,y position
    float minX, maxX, minY, maxY;
    reachGrid->getExtends(minX, maxX, minY, maxY);
    int nX = 0;
    int nY = 0;
    reachGrid->getCells(nX, nY);
    int maxEntry = reachGrid->getMaxEntry();
    int minRequiredEntry = maxEntry;
 
    bool collision = true;
    std::vector<GraspPtr> dummyGrasps;
    float minCollisionDistance = getProperty<float>("MinimumDistanceToEnvironment").getValue();
    int counter = 0;
    auto collisionCheckVisu = "collisionCheckRobotVisu";
    if (getProperty<bool>("EnableVisualization"))
    {
        entityDrawerPrx->setRobotVisu(
            visuLayerName,
            collisionCheckVisu,
            robotStateComponentPrx->getRobotFilename(),
            simox::alg::join(robotStateComponentPrx->getArmarXPackages(), ","),
            FullModel);
        auto config = robotStateComponentPrx->getSynchronizedRobot()->getConfig();
        ARMARX_INFO << "Using config: " << config;
        entityDrawerPrx->updateRobotConfig(visuLayerName, collisionCheckVisu, config);
    }
 
    float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor");
    auto wsr = getWorkspaceRepresentation(g);
    float minManipulabilityDecreaseFactor =
        getProperty<float>("MinManipulabilityDecreaseFactor").getValue();
    int maxTrials = 1000;
    bool success = false;
 
    const float minDistance2D = getProperty<float>("MinPlacementDistance").getValue();
    const float maxDistance2D = getProperty<float>("MaxPlacementDistance").getValue();
 
    while (true)
    {
        counter++;
        if (counter >= maxTrials)
        {
            ARMARX_ERROR << "Could not find a collision free robot placement.";
            storeGlobalX = 0;
            storeGlobalY = 0;
            storeGlobalYaw = 0;
            break;
        }
 
        minRequiredEntry *= minManipulabilityDecreaseFactor;
        minRequiredEntry = std::max<int>(minRequiredEntry, maxEntry * 0.1f);
        int entries;
        if (!reachGrid->getRandomPos(
                minRequiredEntry, storeGlobalX, storeGlobalY, dummyGrasps, 100, &entries))
        {
            continue;
        }
        float distance2D =
            (Eigen::Vector2f(storeGlobalX, storeGlobalY) - globalObjectPose.block<2, 1>(0, 3))
                .norm();
        if (distance2D > maxDistance2D)
        {
            //            ARMARX_INFO << VAROUT(globalObjectPose) << " 2d: " << globalObjectPose.block<2, 1>(0, 3) << " candidate: " << Eigen::Vector2f(storeGlobalX, storeGlobalY);
            ARMARX_INFO << "Placement too far away: " << distance2D;
            continue;
        }
        if (distance2D < minDistance2D)
        {
            ARMARX_INFO << "Placement too close: " << distance2D;
            continue;
        }
 
        if (placementArea != nullptr &&
            !VirtualRobot::MathTools::isInside(Eigen::Vector2f(storeGlobalX, storeGlobalY),
                                               placementArea))
        {
            ARMARX_INFO << "Generated placement (" << storeGlobalX << ", " << storeGlobalY
                        << ") lies outside the permitted area => Retry.";
            continue;
        }
 
        // update robot position
        tmpRobotPoseGlobal = originalRobotPoseGlobal;
        tmpRobotPoseGlobal(0, 3) = storeGlobalX;
        tmpRobotPoseGlobal(1, 3) = storeGlobalY;
 
        storeGlobalYaw = getPlatformRotation(tmpRobotPoseGlobal, globalObjectPose);
        score = reachGrid->getEntry(storeGlobalX, storeGlobalY);
 
        // we rotate the pose around its local z axis
        float x = tmpRobotPoseGlobal(0, 3);
        float y = tmpRobotPoseGlobal(1, 3);
        float z = tmpRobotPoseGlobal(2, 3);
        if (std::isnan(x) || std::isnan(y) || std::isnan(z))
        {
            ARMARX_INFO << "Position contains NaN";
            continue;
        }
        tmpRobotPoseGlobal.block<3, 1>(0, 3) = Eigen::Vector3f::Zero();
        Eigen::Affine3f transform;
        transform = Eigen::Translation<float, 3>(x, y, z) *
                    Eigen::AngleAxisf(storeGlobalYaw, Eigen::Vector3f(0, 0, 1));
        tmpRobotPoseGlobal = transform * tmpRobotPoseGlobal;
 
        robot->setGlobalPose(tmpRobotPoseGlobal);
        cspace->getAgentSceneObj()->setGlobalPose(tmpRobotPoseGlobal);
        int max = wsr->getMaxEntry();
        ARMARX_VERBOSE
            << "Candidate pose: "
            << 100. * wsr->getEntry(FramedPosePtr::dynamicCast(g.framedPrePose)->toEigen()) / max
            << "% min. required: " << minRequiredEntry << " trial: " << counter * 100 / maxTrials
            << "%";
        if (!wsr->isCovered(FramedPosePtr::dynamicCast(g.framedPrePose)->toEigen()))
        {
            ARMARX_INFO << "not reachable";
            continue;
        }
 
        collision = cspace->getCD().isInCollision();
        if (minCollisionDistance > 0)
        {
            float distance = cspace->getCD().getDistance();
            if (distance < minCollisionDistance)
            {
                collision = true;
            }
            ARMARX_DEBUG << "distance to objects for placement: " << distance;
        }
 
        //if (!collision)
        if (getProperty<bool>("EnableVisualization"))
        {
            updateRobot(collisionCheckVisu,
                        tmpRobotPoseGlobal,
                        collision ? DrawColor{1.0, 0.0, 0.0, 1} : DrawColor{0.0, 1.0, 0.0, 1});
            usleep(500000 * visuSlowdownFactor);
        }
 
 
        if (!collision)
        {
            ARMARX_IMPORTANT << "Found collision free placement";
            success = true;
 
            //            if (getProperty<bool>("VisualizeCollisionSpace").getValue())
            //            {
            //                Eigen::Vector3f rpy;
            //                VirtualRobot::MathTools::eigen4f2rpy(tmpRobotPoseGlobal, rpy);
            //                armarx::VectorXf startPos {tmpRobotPoseGlobal(0, 3), tmpRobotPoseGlobal(1, 3), rpy(2)};
            //                MotionPlanningServerInterfacePrx mps = getProxy<MotionPlanningServerInterfacePrx>("MotionPlanningServer", false, "", false);
            //                if (mps)
            //                {
            //                    SimoxCSpaceWith2DPosePtr tmpCSpace = SimoxCSpaceWith2DPosePtr::dynamicCast(cspace->clone());
            //                    auto agent = tmpCSpace->getAgent();
            //                    agent.agentPose = new Pose(tmpRobotPoseGlobal);
            //                    tmpCSpace->setAgent(agent);
 
            //                    CSpaceVisualizerTaskHandle taskHandle = mps->enqueueTask(new CSpaceVisualizerTask(tmpCSpace, startPos, getDefaultName() + "Visu" + IceUtil::generateUUID()));
            //                    planningTasks.push_back(taskHandle);
            //                }
            //            }
 
            break;
        }
        else
        {
            ARMARX_INFO << "In collision";
        }
    }
    ARMARX_INFO << "Returning result" << VAROUT(storeGlobalX) << VAROUT(storeGlobalY);
    //entityDrawerPrx->removeRobotVisu(visuLayerName, collisionCheckVisu);
    robot->setGlobalPose(originalRobotPoseGlobal);
    return success;
}
 
std::vector<SimpleRobotPlacement::RobotPlacement>
SimpleRobotPlacement::getSuitablePositions(const GeneratedGrasp& g,
                                           WorkspaceGridPtr reachGrid,
                                           const Eigen::Matrix4f& globalObjectPose,
                                           float requiredReachabilityFraction,
                                           int requiredCount,
                                           int maxIterations,
                                           const MathTools::ConvexHull2DPtr& placementArea)
{
 
    std::vector<SimpleRobotPlacement::RobotPlacement> placements;
    // robot pose
    Eigen::Matrix4f originalRobotPoseGlobal = robot->getGlobalPose();
    Eigen::Matrix4f tmpRobotPoseGlobal = originalRobotPoseGlobal;
 
    // workspace grid params, workspace is a grid filled with reachability scores for each x,y position
    float minX, maxX, minY, maxY;
    reachGrid->getExtends(minX, maxX, minY, maxY);
    int nX = 0;
    int nY = 0;
    reachGrid->getCells(nX, nY);
    int maxEntry = reachGrid->getMaxEntry();
    int minRequiredEntry = maxEntry * requiredReachabilityFraction;
 
    bool collision = true;
    std::vector<GraspPtr> dummyGrasps;
    float minCollisionDistance = getProperty<float>("MinimumDistanceToEnvironment").getValue();
    auto collisionCheckVisu = "collisionCheckRobotVisu";
    if (getProperty<bool>("EnableVisualization"))
    {
        entityDrawerPrx->setRobotVisu(
            visuLayerName,
            collisionCheckVisu,
            robotStateComponentPrx->getRobotFilename(),
            simox::alg::join(robotStateComponentPrx->getArmarXPackages(), ","),
            FullModel);
        auto config = robotStateComponentPrx->getSynchronizedRobot()->getConfig();
        ARMARX_INFO << "Using config: " << config;
        entityDrawerPrx->updateRobotConfig(visuLayerName, collisionCheckVisu, config);
    }
 
    float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor");
    auto wsr = getWorkspaceRepresentation(g);
 
    for (int n = 0; n < maxIterations; n++)
    {
 
        int entries;
        float storeGlobalX, storeGlobalY;
        if (!reachGrid->getRandomPos(
                minRequiredEntry, storeGlobalX, storeGlobalY, dummyGrasps, 100, &entries))
        {
            continue;
        }
        float distance2D =
            (Eigen::Vector2f(storeGlobalX, storeGlobalY) - globalObjectPose.block<2, 1>(0, 3))
                .norm();
        if (distance2D > 1000)
        {
            //            ARMARX_INFO << VAROUT(globalObjectPose) << " 2d: " << globalObjectPose.block<2, 1>(0, 3) << " candidate: " << Eigen::Vector2f(storeGlobalX, storeGlobalY);
            ARMARX_INFO << "Placement too far away: " << distance2D;
            continue;
        }
        if (distance2D < 400)
        {
            ARMARX_INFO << "Placement too close: " << distance2D;
            continue;
        }
 
        if (placementArea != nullptr &&
            !VirtualRobot::MathTools::isInside(Eigen::Vector2f(storeGlobalX, storeGlobalY),
                                               placementArea))
        {
            ARMARX_INFO << "Generated placement (" << storeGlobalX << ", " << storeGlobalY
                        << ") lies outside the permitted area => Retry.";
            continue;
        }
 
        // update robot position
        tmpRobotPoseGlobal = originalRobotPoseGlobal;
        tmpRobotPoseGlobal(0, 3) = storeGlobalX;
        tmpRobotPoseGlobal(1, 3) = storeGlobalY;
 
        float storeGlobalYaw = getPlatformRotation(tmpRobotPoseGlobal, globalObjectPose);
        float score = reachGrid->getEntry(storeGlobalX, storeGlobalY);
 
        // we rotate the pose around its local z axis
        float x = tmpRobotPoseGlobal(0, 3);
        float y = tmpRobotPoseGlobal(1, 3);
        float z = tmpRobotPoseGlobal(2, 3);
        if (std::isnan(x) || std::isnan(y) || std::isnan(z))
        {
            ARMARX_INFO << "Position contains NaN";
            continue;
        }
        tmpRobotPoseGlobal.block<3, 1>(0, 3) = Eigen::Vector3f::Zero();
        Eigen::Affine3f transform;
        transform = Eigen::Translation<float, 3>(x, y, z) *
                    Eigen::AngleAxisf(storeGlobalYaw, Eigen::Vector3f(0, 0, 1));
        tmpRobotPoseGlobal = transform * tmpRobotPoseGlobal;
 
        robot->setGlobalPose(tmpRobotPoseGlobal);
        cspace->getAgentSceneObj()->setGlobalPose(tmpRobotPoseGlobal);
        int max = wsr->getMaxEntry();
        ARMARX_VERBOSE
            << "Candidate pose: "
            << 100. * wsr->getEntry(FramedPosePtr::dynamicCast(g.framedPrePose)->toEigen()) / max
            << "% min. required: " << minRequiredEntry;
        if (!wsr->isCovered(FramedPosePtr::dynamicCast(g.framedPrePose)->toEigen()))
        {
            ARMARX_INFO << "not reachable";
            continue;
        }
 
        collision = cspace->getCD().isInCollision();
        if (minCollisionDistance > 0)
        {
            float distance = cspace->getCD().getDistance();
            if (distance < minCollisionDistance)
            {
                collision = true;
            }
            ARMARX_DEBUG << "distance to objects for placement: " << distance;
        }
 
        //if (!collision)
        if (getProperty<bool>("EnableVisualization"))
        {
            updateRobot(collisionCheckVisu,
                        tmpRobotPoseGlobal,
                        collision ? DrawColor{1.0, 0.0, 0.0, 1} : DrawColor{0.0, 1.0, 0.0, 1});
            usleep(500000 * visuSlowdownFactor);
        }
 
 
        if (!collision)
        {
            ARMARX_IMPORTANT << "Found collision free placement";
            ARMARX_IMPORTANT << VAROUT(dummyGrasps.size());
 
            RobotPlacement pl;
            pl.x = storeGlobalX;
            pl.y = storeGlobalY;
            pl.z = storeGlobalYaw;
            pl.score = score;
            placements.push_back(pl);
        }
        else
        {
            ARMARX_INFO << "In collision";
        }
        if ((int)placements.size() > requiredCount)
        {
            break;
        }
    }
 
    ARMARX_INFO << "Found " << placements.size() << " collision free placements";
    robot->setGlobalPose(originalRobotPoseGlobal);
    return placements;
}
 
float
SimpleRobotPlacement::getPlatformRotation(const Eigen::Matrix4f& frameGlobal,
                                          const Eigen::Matrix4f& globalTarget)
{
    Eigen::Matrix4f localTarget = frameGlobal.inverse() * globalTarget;
    float x = localTarget(0, 3);
    float y = localTarget(1, 3);
 
    float alpha = std::atan2(y, x);
    alpha -= M_PI / 2; // armars face direction is the positive y-axis, therefore -pi/2
    return alpha;
}
 
void
SimpleRobotPlacement::loadRobot()
{
    robot = VirtualRobot::RobotIO::loadRobot(robotFilePath);
    if (!robot)
    {
        ARMARX_ERROR << "Failed to load robot: " << robotFilePath;
        return;
    }
 
    std::shared_ptr<RemoteRobot> remoteRobot(
        new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
    robot->setGlobalPose(remoteRobot->getGlobalPose());
}
 
void
SimpleRobotPlacement::loadWorkspaces()
{
    for (std::string wsFile : workspaceFilePaths)
    {
        VirtualRobot::WorkspaceRepresentationPtr newSpace;
        bool success = false;
 
        // 1st try to load as manipulability file
        try
        {
            newSpace.reset(new Manipulability(robot));
            newSpace->load(wsFile);
            success = true;
 
            ARMARX_INFO << "Map '" << wsFile << "' loaded as Manipulability map";
        }
        catch (...)
        {
        }
 
        // 2nd try to load as reachability file
        if (!success)
        {
            try
            {
                newSpace.reset(new Reachability(robot));
                newSpace->load(wsFile);
                success = true;
 
                ARMARX_INFO << "Map '" << wsFile << "' loaded as Reachability map";
            }
            catch (...)
            {
                armarx::handleExceptions();
            }
        }
 
        if (success)
        {
            workspaces.push_back(newSpace);
        }
        else
        {
            ARMARX_ERROR << "Failed to load map '" << wsFile << "'";
        }
    }
}
 
bool
SimpleRobotPlacement::hasWorkspace(std::string tcp)
{
    for (VirtualRobot::WorkspaceRepresentationPtr ws : workspaces)
    {
        if (ws->getTCP()->getName() == tcp)
        {
            return true;
        }
    }
    return false;
}
 
GeneratedGraspList
SimpleRobotPlacement::filterGrasps(const GeneratedGraspList grasps)
{
    GeneratedGraspList filteredGrasps = grasps;
    GeneratedGraspList::iterator it = filteredGrasps.begin();
    while (it != filteredGrasps.end())
    {
        GeneratedGrasp g = (*it);
        auto tcpName = robot->getEndEffector(g.eefName)->getTcp()->getName();
        if (!hasWorkspace(tcpName))
        {
            ARMARX_VERBOSE << "Removing grasp because tcp " << tcpName
                           << " is not available in workspace";
            it = filteredGrasps.erase(it);
        }
        else
        {
            ++it;
        }
    }
    return filteredGrasps;
}
 
void
SimpleRobotPlacement::drawNewRobot(Eigen::Matrix4f globalPose)
{
    if (!getProperty<bool>("EnableVisualization"))
    {
        return;
    }
    static int suffix = 0;
    std::string id = drawRobotId + to_string(suffix++);
    entityDrawerPrx->setRobotVisu(
        visuLayerName,
        id,
        robotStateComponentPrx->getRobotFilename(),
        simox::alg::join(robotStateComponentPrx->getArmarXPackages(), ","),
        FullModel);
 
    auto config = robotStateComponentPrx->getSynchronizedRobot()->getConfig();
    ARMARX_INFO << "Using config: " << config;
    entityDrawerPrx->updateRobotConfig(visuLayerName, id, config);
    updateRobot(id, globalPose, COLOR_ROBOT);
}
 
void
SimpleRobotPlacement::updateRobot(std::string id, Eigen::Matrix4f globalPose, DrawColor color)
{
    if (!getProperty<bool>("EnableVisualization"))
    {
        return;
    }
    entityDrawerPrx->updateRobotColor(visuLayerName, id, color);
    entityDrawerPrx->updateRobotPose(visuLayerName, id, new Pose(globalPose));
}
 
void
SimpleRobotPlacement::drawWorkspaceGrid(const GeneratedGrasp& grasp,
                                        const std::string& objectInstanceEntityId)
{
    if (!getProperty<bool>("EnableVisualization"))
    {
        return;
    }
    ObjectInstancePtr instance =
        ObjectInstancePtr::dynamicCast(objectInstances->getEntityById(objectInstanceEntityId));
    Eigen::Matrix4f objectPose = instance->getPose()->toGlobalEigen(robot);
 
    drawWorkspaceGrid(createWorkspaceGrid(grasp, objectPose));
}
 
void
SimpleRobotPlacement::drawWorkspaceGrid(VirtualRobot::WorkspaceGridPtr reachGrid)
{
    if (!getProperty<bool>("EnableVisualization"))
    {
        return;
    }
    int counter = 0;
    Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
    float minX, maxX, minY, maxY;
    reachGrid->getExtends(minX, maxX, minY, maxY);
    pose(0, 3) = minX;
    pose(1, 3) = minY;
 
    int nX = 0;
    int nY = 0;
    reachGrid->getCells(nX, nY);
 
    int maxEntry = reachGrid->getMaxEntry();
 
    float sizeX = (maxX - minX) / (float)nX;
    float sizeY = (maxY - minY) / (float)nY;
    auto batch = debugDrawerPrx->ice_batchOneway();
    // iterate through the workspace grid
    for (int x = 0; x < nX; x++)
    {
        float xPos = minX + (float)x * sizeX + 0.5f * sizeX; // x-center of voxel
 
        for (int y = 0; y < nY; y++)
        {
            int cellEntry = reachGrid->getCellEntry(x, y);
 
            if (cellEntry > 0)
            {
                float intensity = (float)cellEntry / (float)maxEntry;
 
                float yPos = minY + (float)y * sizeY + 0.5f * sizeY; // y-center of voxel
                pose(0, 3) = xPos;
                pose(1, 3) = yPos;
 
                armarx::Vector3Ptr dimensions = new armarx::Vector3(30, 30, 1);
                armarx::PosePtr tmpPose = new armarx::Pose(pose);
 
                VirtualRobot::ColorMap cm = VirtualRobot::ColorMap::eHot;
                VirtualRobot::CoinVisualizationFactory::Color color = cm.getColor(intensity);
 
                armarx::DrawColor voxelColor;
                voxelColor.r = color.r;
                voxelColor.g = color.g;
                voxelColor.b = color.b;
                voxelColor.a = 0.5;
 
                batch->setBoxVisu(visuLayerName,
                                  "reachGridVoxel_" + to_string(counter++),
                                  tmpPose,
                                  dimensions,
                                  voxelColor);
            }
        }
    }
    batch->ice_flushBatchRequests();
}