VoxelGridCSpace.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2017, 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    ArmarX
 * @author     Mirko Waechter( mirko.waechter at kit dot edu)
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "VoxelGridCSpace.h"
 
#include <Eigen/Core>
 
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/TriMeshUtils.h>
#include <VirtualRobot/Visualization/VisualizationFactory.h>
 
#include <ArmarXCore/core/exceptions/user/NotImplementedYetException.h>
#include <ArmarXCore/core/util/algorithm.h>
#include <ArmarXCore/interface/core/BasicVectorTypes.h>
 
using namespace armarx;
 
VoxelGridCSpace::VoxelGridCSpace(visionx::VoxelGridProviderInterfacePrx voxelGridProvider,
                                 memoryx::CommonStorageInterfacePrx cs,
                                 bool loadVisualizationModel,
                                 float stationaryObjectMargin) :
    SimoxCSpace(cs, loadVisualizationModel, stationaryObjectMargin)
{
    this->voxelGridProvider = voxelGridProvider;
}
 
VirtualRobot::SceneObjectPtr
VoxelGridCSpace::createGridObstacle() const
{
    auto gridPositionsEigen = armarx::transform(
        gridPositions,
        +[](armarx::Vector3f const& data) { return Eigen::Vector3f(data.e0, data.e1, data.e2); });
 
 
    auto gridMesh = VirtualRobot::TriMeshUtils::CreateSparseBoxGrid(
        Eigen::Matrix4f::Identity(),
        gridPositionsEigen,
        gridSize,
        gridSize,
        gridSize,
        VirtualRobot::VisualizationFactory::Color::Blue());
    gridMesh->mergeVertices(10);
    gridMesh->fattenShrink(stationaryObjectMargin);
    Eigen::Matrix4f id = Eigen::Matrix4f::Identity();
    auto visu =
        VirtualRobot::CoinVisualizationFactory().createTriMeshModelVisualization(gridMesh, id);
    VirtualRobot::SceneObjectPtr obst(new VirtualRobot::SceneObject(
        "PointCloudMeshGrid",
        visu,
        VirtualRobot::CollisionModelPtr(new VirtualRobot::CollisionModel(
            visu, "PointCloudMeshGridCollisionModel", agentSceneObj->getCollisionChecker())),
        VirtualRobot::SceneObject::Physics(),
        const_cast<VirtualRobot::CDManager*>(&cd)->getCollisionChecker()));
    return obst;
}
 
armarx::CSpaceBasePtr
VoxelGridCSpace::clone(const Ice::Current&)
{
    return VoxelGridCSpaceBasePtr(new VoxelGridCSpace(*this));
}
 
void
VoxelGridCSpace::initCollisionTest(const Ice::Current& c)
{
    SimoxCSpace::initCollisionTest(c);
    if (gridPositions.empty())
    {
        gridPositions = voxelGridProvider->getFilledGridPositions();
        ARMARX_INFO << "Got grid with size: " << gridPositions.size();
        gridSize = voxelGridProvider->getGridSize();
    }
    auto obst = createGridObstacle();
    ARMARX_INFO << "Adding scene object with grid element count: " << gridPositions.size();
    stationaryObjectSet->addSceneObject(obst);
 
 
    ARMARX_INFO << "SceneObjects: "
                << armarx::transform(
                       stationaryObjectSet->getSceneObjects(),
                       +[](VirtualRobot::SceneObjectPtr const& obj) { return obj->getName(); });
    if (!cd.hasSceneObjectSet(stationaryObjectSet))
    {
        cd.addCollisionModel(stationaryObjectSet);
    }
}
 
CSpaceBasePtr
armarx::VoxelGridCSpace::clone(bool loadVisualizationModel)
{
    //    TIMING_START(SimoxCSpaceClone);
    //    VoxelGridCSpacePtr cloned = VoxelGridCSpacePtr::dynamicCast(clone(Ice::emptyCurrent));
    VoxelGridCSpacePtr cloned =
        new VoxelGridCSpace(voxelGridProvider, commonStorage, loadVisualizationModel);
    cloned->gridPositions = gridPositions;
    cloned->gridSize = gridSize;
    cloned->stationaryObjectMargin = stationaryObjectMargin;
    for (const auto& obj : stationaryObjects)
    {
        cloned->addStationaryObject(obj);
    }
    cloned->agentInfo = agentInfo;
    ARMARX_CHECK_EXPRESSION(agentSceneObj);
    cloned->initAgent();
    //    TIMING_END(SimoxCSpaceClone);
    return cloned;
}