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 <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/Grasping/Grasp.h>
#include <VirtualRobot/Grasping/GraspSet.h>
 
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.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;
}