GraspablePredicateProvider.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    ArmarX::
* @author     Valerij Wittenbeck ( valerij.wittenbeck at kit dot edu)
* @date       2015
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "GraspablePredicateProvider.h"
 
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <VirtualRobot/Grasping/GraspSet.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
using namespace memoryx;
 
GraspablePredicateProvider::GraspablePredicateProvider() :
    leftColor(armarx::DrawColor {0.f, 0.f, 1.f, 0.25f}),
    rightColor(armarx::DrawColor {0.f, 1.f, 0.f, 0.25f}),
    leftGraspable(PredicateInfo {"leftgraspable", 1}),
    rightGraspable(PredicateInfo {"rightgraspable", 1})
{
}
 
std::vector<Box> GraspablePredicateProvider::stringToBoxes(const std::string& boxesString)
{
    std::vector<Box> result;
    std::vector<std::string> boxTuples = simox::alg::split(boxesString, ";");
 
    for (const auto& boxTupleString : boxTuples)
    {
        std::vector<std::string> boxTuple = simox::alg::split(boxTupleString, " ");
        ARMARX_CHECK_EXPRESSION(boxTuple.size() == 6);
 
        result.push_back(Box
        {
            std::stof(boxTuple[0]),
            std::stof(boxTuple[1]),
            std::stof(boxTuple[2]),
            std::stof(boxTuple[3]),
            std::stof(boxTuple[4]),
            std::stof(boxTuple[5])
        });
    }
 
    return result;
}
 
void GraspablePredicateProvider::onInitWorldStateUpdater()
{
    boxesL = stringToBoxes(getProperty<std::string>("BoundingBoxesL").getValue());
    boxesR = stringToBoxes(getProperty<std::string>("BoundingBoxesR").getValue());
 
    usingProxy("WorkingMemory");
    usingProxy("PriorKnowledge");
 
    debugDrawerTopicName = getProperty<std::string>("DebugDrawerTopicName").getValue();
 
    if (!debugDrawerTopicName.empty())
    {
        offeringTopic(debugDrawerTopicName);
    }
}
 
void GraspablePredicateProvider::onConnectWorldStateUpdater()
{
    wm = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
    objectInstances = wm->getObjectInstancesSegment();
    prior = getProxy<PriorKnowledgeInterfacePrx>("PriorKnowledge");
    objectClasses = prior->getObjectClassesSegment();
    fileManager = memoryx::GridFileManagerPtr(new memoryx::GridFileManager(prior->getCommonStorage()));
 
    if (!debugDrawerTopicName.empty())
    {
        armarx::DebugDrawerInterfacePrx debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>(debugDrawerTopicName);
 
        for (size_t i = 0; i < boxesL.size(); ++i)
        {
            boxesL[i].drawTo(debugDrawer, getDefaultName(), leftGraspable.name + "_box_" + std::to_string(i), leftColor);
        }
 
        for (size_t i = 0; i < boxesR.size(); ++i)
        {
            boxesR[i].drawTo(debugDrawer, getDefaultName(), rightGraspable.name + "_box_" + std::to_string(i), rightColor);
        }
    }
}
 
std::string GraspablePredicateProvider::getDefaultName() const
{
    return "GraspablePredicateProvider";
}
 
PredicateInfoList GraspablePredicateProvider::getPredicateInfos(const Ice::Current&)
{
    return {leftGraspable, rightGraspable};
}
 
void GraspablePredicateProvider::addPredicateIfInside(PredicateInstanceList& result, const std::string& predicateName, const ObjectInstancePtr& obj, const std::vector<Box>& boxes)
{
    bool inside = false;
    auto pos = obj->getPosition();
 
    if (pos->getFrame() != armarx::GlobalFrame && !pos->getFrame().empty())
    {
        pos = armarx::FramedPosePtr::dynamicCast(wm->getAgentInstancesSegment()->convertToWorldPose(pos->agent, obj->getPose()))->getPosition();
    }
 
    ARMARX_DEBUG << "Obj: " << obj->getName() << " pos: " << *pos;
 
    for (const auto& box : boxes)
    {
        inside |= box.inside(pos);
    }
 
    if (inside)
    {
        ARMARX_DEBUG << obj->getName() << " " << predicateName;
        result.push_back(PredicateInstance {predicateName, {objectInstances->getEntityRefById(obj->getId())}, true});
    }
}
 
PredicateInstanceList GraspablePredicateProvider::calcPredicates(const Ice::Current&)
{
    PredicateInstanceList result;
 
    for (const auto& entity : objectInstances->getAllEntities())
    {
        ObjectInstancePtr object = ObjectInstancePtr::dynamicCast(entity);
        const std::string className = object->getMostProbableClass();
 
        auto graspabilityDescriptorIt = std::find_if(graspabilityDescriptors.cbegin(), graspabilityDescriptors.cend(), [&](const GraspabilityDescriptor & gd)
        {
            return gd.className == className;
        });
 
        if (graspabilityDescriptorIt == graspabilityDescriptors.cend())
        {
            auto parents = objectClasses->getObjectClassByNameWithAllParents(className)->getParentClasses();
 
            bool hasGraspableSuperclass = std::find(parents.cbegin(), parents.cend(), "graspable") != parents.cend();
            hasGraspableSuperclass |= std::find(parents.cbegin(), parents.cend(), "bothhandsgraspable") != parents.cend();
            GraspabilityDescriptor gd {className, hasGraspableSuperclass, false, false};
 
            ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(objectClasses->getEntityByName(className));
            memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxWrapper = objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
            VirtualRobot::ManipulationObjectPtr mo = simoxWrapper->getManipulationObject();
 
            gd.hasLeftGrasp = false;
            gd.hasRightGrasp = false;
            if (mo)
            {
                auto hasGrasp = [](VirtualRobot::GraspSetPtr set)
                {
                    if (!set)
                    {
                        return false;
                    }
 
                    for (size_t i = 0; i < set->getSize(); ++i)
                    {
                        if (armarx::Contains(set->getGrasp(i)->getName(), "Grasp", true))
                        {
                            return true;
                        }
                    }
                    return false;
                };
                gd.hasLeftGrasp = hasGrasp(mo->getGraspSet("TCP L"));
                gd.hasRightGrasp = hasGrasp(mo->getGraspSet("TCP R"));
            }
 
            graspabilityDescriptors.push_back(gd);
            graspabilityDescriptorIt = std::prev(graspabilityDescriptors.cend());
 
            ARMARX_INFO << "new graspability descriptor: [className: '" << className
                        << "'; child of graspable/bothhandsgraspable: " << gd.hasGraspableSuperclass
                        << "; has left grasp: " << gd.hasLeftGrasp
                        << "; has right grasp: " << gd.hasRightGrasp << "]";
        }
 
        const auto& gd = *graspabilityDescriptorIt;
        ARMARX_DEBUG << "checking " << className << "/" << gd.className << " " << gd.hasGraspableSuperclass << " " << gd.hasLeftGrasp << " " << gd.hasRightGrasp;
 
        if (!gd.hasGraspableSuperclass)
        {
            continue;
        }
 
        if (gd.hasLeftGrasp)
        {
            addPredicateIfInside(result, leftGraspable.name, object, boxesL);
        }
 
        if (gd.hasRightGrasp)
        {
            addPredicateIfInside(result, rightGraspable.name, object, boxesR);
        }
    }
 
    return result;
}