HandPredicateProvider.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       2014
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "HandPredicateProvider.h"
 
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
 
#include <ArmarXCore/observers/variant/ChannelRef.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
namespace memoryx
{
    HandPredicateProvider::HandPredicateProvider() :
        graspedPredicate(PredicateInfo {"grasped", 3}),
        graspedBothPredicate(PredicateInfo {"graspedBoth", 2}),
        handEmptyPredicate(PredicateInfo {"handEmpty", 2})
    {
    }
 
 
    void HandPredicateProvider::onInitWorldStateUpdater()
    {
        usingProxy("WorkingMemory");
        usingProxy("PriorKnowledge");
    }
 
    void HandPredicateProvider::onConnectWorldStateUpdater()
    {
        wm = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
        prior = getProxy<PriorKnowledgeInterfacePrx>("PriorKnowledge");
        distanceThreshold = getProperty<float>("DistanceThreshold").getValue();
 
        agentInstances = wm->getAgentInstancesSegment();
        objectInstances = wm->getObjectInstancesSegment();
        objectClasses = prior->getObjectClassesSegment();
    }
 
    std::string HandPredicateProvider::getDefaultName() const
    {
        return "HandPredicateProvider";
    }
 
    PredicateInfoList HandPredicateProvider::getPredicateInfos(const Ice::Current&)
    {
        return {graspedPredicate, graspedBothPredicate, handEmptyPredicate};
    }
 
    PredicateInstanceList HandPredicateProvider::calcPredicates(const Ice::Current&)
    {
        PredicateInstanceList result;
        //    const std::string agentName = "testAgent";
        //problem: hands can't be associated with the agent, this PP will try every agent with every hand
 
        auto agents = agentInstances->getAllAgentInstances();
 
        if (agents.empty())
        {
            ARMARX_WARNING << "No Agents found!";
        }
 
        for (const auto& agent : agents)
        {
            ARMARX_INFO << "Checking agent: " << agent->getName();
            if (armarx::Contains(agent->getName(), "human", true))
            {
                continue;
            }
 
            auto robot = agent->getSharedRobot();
            EntityRefBasePtr agentRef = agentInstances->getEntityRefById(agent->getId());
 
            std::vector<ObjectInstancePtr> hands;
            std::vector<ObjectInfo> graspableObjects;
 
            for (const auto& entity : objectInstances->getAllEntities())
            {
                ObjectInstancePtr object = ObjectInstancePtr::dynamicCast(entity);
                auto classes = objectClasses->getObjectClassByNameWithAllParents(object->getMostProbableClass())->getParentClasses();
 
                const bool isGraspable = std::find(classes.cbegin(), classes.cend(), "graspable") != classes.cend();
                const bool isBothHandGraspable = std::find(classes.cbegin(), classes.cend(), "bothhandsgraspable") != classes.cend();
 
                if (isGraspable)
                {
                    graspableObjects.push_back(ObjectInfo {object, ObjectInfo::ObjectType::OneHandGraspable});
                }
                else if (isBothHandGraspable)
                {
                    graspableObjects.push_back(ObjectInfo {object, ObjectInfo::ObjectType::TwoHandGraspable});
                }
                else if (object->getName() == "handleft3a" || object->getName() == "handright3a")
                {
                    hands.push_back(object);
                }
            }
 
            std::vector<std::pair<EntityRefBasePtr, ObjectInfo>> graspCandidates;
            std::vector<std::pair<EntityRefBasePtr, ObjectInfo>> bothHandsGraspCandidates;
 
            for (const auto& hand : hands)
            {
                EntityRefBasePtr handRef = objectInstances->getEntityRefById(hand->getId());
                const auto handPos = hand->getPosition()->toGlobal(robot)->toEigen();
                float minDistance = std::numeric_limits<float>::max();
                ObjectInfo bestObject {ObjectInstancePtr(), ObjectInfo::ObjectType::Other};
 
                for (const auto& object : graspableObjects)
                {
                    //                auto classes = prior->getObjectClassesSegment()->getObjectClassByNameWithAllParents(object->getMostProbableClass())->getParentClasses();
                    //                const bool isGraspable = std::find(classes.begin(), classes.end(), "graspable") != classes.end();
                    //                if (!isGraspable) continue;
 
 
                    armarx::FramedPositionPtr objPos = object.objectInstance->getPosition();
                    objPos = objPos->toGlobal(robot);
                    ARMARX_DEBUG << object.objectInstance->getName() << VAROUT(objPos);
                    ARMARX_DEBUG << hand->getName() << VAROUT(handPos);
                    const float dist = (objPos->toEigen() - handPos).norm();
                    ARMARX_DEBUG << "distance: " << dist;
 
                    if (dist < minDistance)
                    {
                        bestObject = object;
                        minDistance = dist;
                    }
                }
 
                if (bestObject.objectInstance)
                {
                    ARMARX_INFO << "Closest object for grasped: '" << bestObject.objectInstance->getName() << "' in hand '" << hand->getName() << "' with distance: " << minDistance;
                }
 
                if (minDistance < distanceThreshold)
                {
                    ARMARX_INFO << "grasp candidate: hand '" << handRef->entityName << "' and object '" << bestObject.objectInstance->getName() << "' with dist " << minDistance;
                    graspCandidates.push_back({handRef, bestObject});
                }
                if (minDistance < distanceThreshold * 1.5)
                {
                    ARMARX_INFO << "grasp candidate for both hand graspable: hand '" << handRef->entityName << "' and object '" << bestObject.objectInstance->getName() << "' with dist " << minDistance;
                    bothHandsGraspCandidates.push_back({handRef, bestObject});
                }
 
            }
 
            std::map<std::string, bool> handOccupiedMap;
            for (const auto& hand : hands)
            {
                handOccupiedMap[hand->getId()] = false;
            }
 
            for (auto firstIt = graspCandidates.cbegin(); firstIt != graspCandidates.cend(); ++firstIt)
            {
                const auto& candidate = *firstIt;
                const auto hand = candidate.first;
                const auto objectId = candidate.second.objectInstance->getId();
 
                //            if (candidate.second.type == ObjectInfo::ObjectType::TwoHandGraspable)
                //            {
                //                for (auto secondIt = std::next(firstIt); secondIt != graspCandidates.cend(); ++secondIt)
                //                {
                //                    const auto handOther = secondIt->first;
                //                    const auto objectIdOther = secondIt->second.objectInstance->getId();
                //                    if (hand->entityId != handOther->entityId && objectId == objectIdOther)
                //                    {
                //                        result.push_back(PredicateInstance {graspedBothPredicate.name, {agentRef, objectInstances->getEntityRefById(objectId)}, true});
                //                        handOccupiedMap[hand->entityId] = true;
                //                        handOccupiedMap[handOther->entityId] = true;
                //                        break;
                //                    }
                //                }
                //            }
                //            else
                if (candidate.second.type != ObjectInfo::ObjectType::TwoHandGraspable)
                {
                    result.push_back(PredicateInstance {graspedPredicate.name, {agentRef, hand, objectInstances->getEntityRefById(objectId)}, true});
                    handOccupiedMap[hand->entityId] = true;
                }
            }
 
            for (auto firstIt = bothHandsGraspCandidates.cbegin(); firstIt != bothHandsGraspCandidates.cend(); ++firstIt)
            {
                const auto& candidate = *firstIt;
                const auto hand = candidate.first;
                const auto objectId = candidate.second.objectInstance->getId();
 
                if (candidate.second.type == ObjectInfo::ObjectType::TwoHandGraspable)
                {
                    for (auto secondIt = std::next(firstIt); secondIt != bothHandsGraspCandidates.cend(); ++secondIt)
                    {
                        const auto handOther = secondIt->first;
                        const auto objectIdOther = secondIt->second.objectInstance->getId();
                        if (hand->entityId != handOther->entityId && objectId == objectIdOther)
                        {
                            result.push_back(PredicateInstance {graspedBothPredicate.name, {agentRef, objectInstances->getEntityRefById(objectId)}, true});
                            handOccupiedMap[hand->entityId] = true;
                            handOccupiedMap[handOther->entityId] = true;
                            break;
                        }
                    }
                }
            }
 
            for (const auto& hand : handOccupiedMap)
            {
                if (!hand.second)
                {
                    result.push_back(PredicateInstance {handEmptyPredicate.name, {agentRef, objectInstances->getEntityRefById(hand.first)}, true});
                }
            }
        }
 
        return result;
    }
}