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 <ArmarXCore/interface/core/BasicVectorTypes.h>
#include <VirtualRobot/Visualization/TriMeshUtils.h>
#include <ArmarXCore/core/exceptions/user/NotImplementedYetException.h>
#include <ArmarXCore/core/util/algorithm.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/VisualizationFactory.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;
}