ClutteredSceneGenerator.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * 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    ArmarXSimulation::ArmarXObjects::ClutteredSceneGenerator
 * @author     Patrick Hegemann ( patrick dot hegemann at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ClutteredSceneGenerator.h"
 
#include <ArmarXSimulation/libraries/ClutteredSceneGenerator/objects/SimulatedObject.h>
#include <ArmarXSimulation/libraries/ClutteredSceneGenerator/objects/SimulatedObjectAsObject.h>
#include <ArmarXSimulation/libraries/ClutteredSceneGenerator/objects/SimulatedObjectAsRobot.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
 
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/Obstacle.h>
#include <VirtualRobot/SceneObjectSet.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/XML/ObjectIO.h>
 
#include <SimoxUtility/math/pose/pose.h>
 
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
 
#include <memory>
#include <string>
 
namespace armarx::simulation::scene_generation
{
    ClutteredSceneGenerator::ClutteredSceneGenerator(const armarx::SimulatorInterfacePrx& simulator,
                                                     const ClutteredSceneGenerator::Config &config) :
            config_(config),
            simulator_(simulator)
    {
        createCollisionWalls();
    }
 
    void ClutteredSceneGenerator::generateScene(int seed)
    {
        deleteLocalObjectCopies();
 
        simulator_->stop();
 
        rnd_.seed(seed);
 
        ARMARX_CHECK(!config_.objectSets.empty()) << "No object set configured.";
 
        for (uint i = 0; i < config_.amountObjects; ++i)
        {
            // Rotate through object sets
            const size_t objectSetIndex = i % config_.objectSets.size();
            const ObjectSet& objectSet = config_.objectSets[objectSetIndex];
            ARMARX_CHECK(!objectSet.objects.empty()) << "Object set is empty.";
 
            // Select random object from this set
            const boost::uniform_int<uint> uniform(0, objectSet.objects.size() - 1);
            const size_t objectIndex = uniform(rnd_);
            const ObjectSource& objectSource = objectSet.objects[objectIndex];
 
            // Spawn this object in the simulator
            std::string objectName = "obj_" + std::to_string(i);;
            if (objectSource.objectID)
            {
                armarx::ObjectID newID(objectSource.objectID.value());
                newID.setInstanceName(objectName);
                objectName = newID.str();
            }
            std::unique_ptr<SimulatedObject> object = makeObject(objectName, objectSource);
            object->setLocalPose(randomObjectPose(i));
            object->addToSimulator(simulator_);
            object->updatePoseToSimulator(simulator_);
            localObjectCopies_.push_back(std::move(object));
        }
 
        // Let objects fall onto the table
        dropObjects();
 
        // Try to reset some objects until all of them are on the table
        if (config_.autoResetInvalidObjects)
        {
            resetInvalidObjects();
        }
 
        simulator_->start();
    }
 
    void ClutteredSceneGenerator::createCollisionWalls()
    {
        // Initialize invisible walls for object collision checking
        const float wallHeight = 1000;
        frontWall_ = VirtualRobot::Obstacle::createBox(config_.boxW, 1, wallHeight);
        backWall_ = VirtualRobot::Obstacle::createBox(config_.boxW, 1, wallHeight);
        leftWall_ = VirtualRobot::Obstacle::createBox(1, config_.boxH, wallHeight);
        rightWall_ = VirtualRobot::Obstacle::createBox(1, config_.boxH, wallHeight);
 
        const float wallZ = config_.boxZ + config_.boxD + wallHeight / 2;
        frontWall_->setGlobalPose(simox::math::pose(Eigen::Vector3f{config_.boxX, config_.boxY - config_.boxH / 2.0f, wallZ}));
        backWall_->setGlobalPose(simox::math::pose(Eigen::Vector3f{config_.boxX, config_.boxY + config_.boxH / 2.0f, wallZ}));
        leftWall_->setGlobalPose(simox::math::pose(Eigen::Vector3f{config_.boxX - config_.boxW / 2.0f, config_.boxY, wallZ}));
        rightWall_->setGlobalPose(simox::math::pose(Eigen::Vector3f{config_.boxX + config_.boxW / 2.0f, config_.boxY, wallZ}));
 
        walls_.reset(new VirtualRobot::SceneObjectSet("walls"));
        walls_->addSceneObject(frontWall_);
        walls_->addSceneObject(backWall_);
        walls_->addSceneObject(leftWall_);
        walls_->addSceneObject(rightWall_);
    }
 
    armarx::PosePtr ClutteredSceneGenerator::randomObjectPose(uint heightIndex)
    {
        // Object position and angle random distributions
        boost::uniform_real<float> objectPositionDistX(config_.minObjectX, config_.maxObjectX);
        boost::uniform_real<float> objectPositionDistY(config_.minObjectY, config_.maxObjectY);
        boost::uniform_real<float> objectAngleDist(0, 2 * M_PI);
 
        // Sample random position and rotation around z-axis
        float x = objectPositionDistX(rnd_);
        float y = objectPositionDistY(rnd_);
        float z = config_.minObjectZ + static_cast<float>(heightIndex) * config_.objectSpacingZ;
        float angle = objectAngleDist(rnd_);
 
        Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
        armarx::PosePtr pose(new Pose(simox::math::pose(Eigen::Vector3f{x, y, z}, rotation)));
        return pose;
    }
 
    void ClutteredSceneGenerator::deleteLocalObjectCopies()
    {
        localObjectCopies_.clear();
    }
 
    std::unique_ptr<SimulatedObject>
    ClutteredSceneGenerator::makeObject(const std::string& name, const ObjectSource& objectSource)
    {
        switch (objectSource.type) {
            case AsObject:
                return std::make_unique<SimulatedObjectAsObject>(name, objectSource);
            case AsRobot:
                return std::make_unique<SimulatedObjectAsRobot>(name, objectSource);
            case Automatic:
                // TODO(patrick.hegemann): Implement (look up top-level tag in simox xml)
                ARMARX_INFO << __FUNCTION__ << " Unknown SimulationObjectType: TO DO: Implement";
                return nullptr;
            default:
                ARMARX_ERROR << __FUNCTION__ << " - unknown object type for " << name;
                return nullptr;
        }
    }
 
    void ClutteredSceneGenerator::updateLocalObjectCopies()
    {
        for (const auto& object : localObjectCopies_)
        {
            object->updatePoseFromSimulator(simulator_);
        }
    }
 
    void ClutteredSceneGenerator::dropObjects()
    {
        simulator_->stop();
        for (uint k = 0; k < config_.fallingSteps; ++k)
        {
            simulator_->step();
        }
        simulator_->start();
    }
 
    void ClutteredSceneGenerator::resetInvalidObjects()
    {
        VirtualRobot::CollisionCheckerPtr col = VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
        bool anyRespawn = false;
 
        // Try to reset the objects a number of times, or until all objects are inside the box
        for (uint i = 0; i < config_.maxResetTries; ++i)
        {
            updateLocalObjectCopies();
 
            // TODO(patrick.hegemann): clean up magic number
            int heightIndex = 6;
            anyRespawn = false;
 
            // Check and reset every object
            // for (const VirtualRobot::RobotPtr& robot : localObjectCopies_)
            for (const auto& object : localObjectCopies_)
            {
                // Check if object collides with the imaginary walls around the box
                bool collision = object->checkCollision(col, walls_);
                
                float z = object->getLocalPose()->position->z;
                bool isObjectOutsideOfTable = z < config_.minObjectZOtherwiseReset;
 
                // Respawn object if necessary
                if (collision || isObjectOutsideOfTable)
                {
                    anyRespawn = true;
                    object->setLocalPose(randomObjectPose(heightIndex));
                    object->updatePoseToSimulator(simulator_);
                    heightIndex++;
                }
            }
 
            // Let objects fall again if anything has changed
            if (!anyRespawn)
            {
                break;
            }
            dropObjects();
        }
    }
 
    const ClutteredSceneGenerator::Config &ClutteredSceneGenerator::getConfig() const
    {
        return config_;
    }
 
    void ClutteredSceneGenerator::setConfig(const ClutteredSceneGenerator::Config& conf)
    {
        ClutteredSceneGenerator::config_ = conf;
    }
 
    const SimulatorInterfacePrx &ClutteredSceneGenerator::getSimulator() const
    {
        return simulator_;
    }
 
    void ClutteredSceneGenerator::setSimulator(const SimulatorInterfacePrx &sim)
    {
        ClutteredSceneGenerator::simulator_ = sim;
    }
}  // namespace armarx::simulation::scene_generation