AffordanceExtraction.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 Lesser General Public License as
 * published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 *
 * 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 Lesser 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    VisionX
 * @author     Peter Kaiser (peter dot kaiser at kit dot edu)
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl.txt
 *             GNU General Public License
 */
 
#include "AffordanceExtraction.h"
 
#include <MemoryX/libraries/memorytypes/entity/Affordance.h>
#include <MemoryX/libraries/memorytypes/entity/EnvironmentalPrimitive.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <AffordanceKit/embodiments/Armar3Embodiment.h>
#include <AffordanceKit/embodiments/Armar4Embodiment.h>
#include <AffordanceKit/embodiments/WalkManEmbodiment.h>
 
#include <VisionX/libraries/AffordanceKitArmarX/PrimitiveSetArmarX.h>
#include <VisionX/libraries/AffordanceKitArmarX/SceneArmarX.h>
 
using namespace armarx;
 
AffordanceExtraction::AffordanceExtraction() :
    hasNewPrimitives(false),
    lastProcessedTimestamp(0),
    enabled(true)
{
}
 
void AffordanceExtraction::reportNewPointCloudSegmentation(const Ice::Current& c)
{
    std::unique_lock lock(primitivesMutex);
 
    if (environmentalPrimitiveSegment)
    {
        primitiveSegmentation.reset(new AffordanceKitArmarX::PrimitiveSetArmarX(environmentalPrimitiveSegment));
        hasNewPrimitives = true;
 
        ARMARX_INFO << "New segmentation available: " << primitiveSegmentation->size() << " Primitives";
    }
    else
    {
        ARMARX_ERROR << "New segmentation available, but failed to access environmental primitives segment";
    }
}
 
void AffordanceExtraction::onInitComponent()
{
    std::string embodimentName = getProperty<std::string>("EmbodimentName").getValue();
 
    ARMARX_INFO << "Using embodiment '" << embodimentName << "'";
    if (embodimentName == "Armar3")
    {
        embodiment.reset(new AffordanceKit::Armar3Embodiment());
    }
    else if (embodimentName == "Armar4")
    {
        embodiment.reset(new AffordanceKit::Armar4Embodiment());
    }
    else if (embodimentName == "WalkMan")
    {
        embodiment.reset(new AffordanceKit::WalkManEmbodiment());
    }
    else
    {
        ARMARX_ERROR << "Unknown embodiment '" << embodimentName << "' specified. Affordance extraction will not work properly.";
        embodiment.reset(new AffordanceKit::Embodiment());
    }
 
    affordanceExtractionCertainty = getProperty<float>("AffordanceExtractionCertainty").getValue();
    ARMARX_INFO << "Affordance extraction certainty: " << affordanceExtractionCertainty;
 
    platformGraspAffordance.reset(new AffordanceKit::PlatformGraspAffordance(embodiment, affordanceExtractionCertainty));
    prismaticGraspAffordance.reset(new AffordanceKit::PrismaticGraspAffordance(embodiment, affordanceExtractionCertainty));
    graspAffordance.reset(new AffordanceKit::GraspAffordance(embodiment, platformGraspAffordance, prismaticGraspAffordance));
 
    supportAffordance.reset(new AffordanceKit::SupportAffordance(embodiment, platformGraspAffordance));
    leanAffordance.reset(new AffordanceKit::LeanAffordance(embodiment, platformGraspAffordance));
    liftAffordance.reset(new AffordanceKit::LiftAffordance(embodiment, prismaticGraspAffordance));
    holdAffordance.reset(new AffordanceKit::HoldAffordance(embodiment, prismaticGraspAffordance));
    turnAffordance.reset(new AffordanceKit::TurnAffordance(embodiment, prismaticGraspAffordance));
    pushAffordance.reset(new AffordanceKit::PushAffordance(embodiment, graspAffordance));
    pullAffordance.reset(new AffordanceKit::PullAffordance(embodiment, prismaticGraspAffordance));
 
    bimanualPlatformGraspAffordance.reset(new AffordanceKit::BimanualPlatformGraspAffordance(embodiment, platformGraspAffordance));
    bimanualPrismaticGraspAffordance.reset(new AffordanceKit::BimanualPrismaticGraspAffordance(embodiment, prismaticGraspAffordance));
    bimanualGraspAffordance.reset(new AffordanceKit::BimanualGraspAffordance(embodiment, bimanualPrismaticGraspAffordance, bimanualPlatformGraspAffordance));
    bimanualOpposedGraspAffordance.reset(new AffordanceKit::BimanualOpposedGraspAffordance(embodiment, bimanualGraspAffordance));
    bimanualAlignedGraspAffordance.reset(new AffordanceKit::BimanualAlignedGraspAffordance(embodiment, bimanualGraspAffordance));
    bimanualOpposedPlatformGraspAffordance.reset(new AffordanceKit::BimanualOpposedPlatformGraspAffordance(embodiment, bimanualPlatformGraspAffordance, bimanualOpposedGraspAffordance));
    bimanualOpposedPrismaticGraspAffordance.reset(new AffordanceKit::BimanualOpposedPrismaticGraspAffordance(embodiment, bimanualPrismaticGraspAffordance, bimanualOpposedGraspAffordance));
    bimanualAlignedPlatformGraspAffordance.reset(new AffordanceKit::BimanualAlignedPlatformGraspAffordance(embodiment, bimanualPlatformGraspAffordance, bimanualAlignedGraspAffordance));
    bimanualAlignedPrismaticGraspAffordance.reset(new AffordanceKit::BimanualAlignedPrismaticGraspAffordance(embodiment, bimanualPrismaticGraspAffordance, bimanualAlignedGraspAffordance));
    bimanualTurnAffordance.reset(new AffordanceKit::BimanualTurnAffordance(embodiment, bimanualOpposedPrismaticGraspAffordance));
    bimanualLiftAffordance.reset(new AffordanceKit::BimanualLiftAffordance(embodiment, bimanualPrismaticGraspAffordance, bimanualOpposedPrismaticGraspAffordance, bimanualOpposedPlatformGraspAffordance));
 
    affordanceTypes[graspAffordance->getName()] = memoryx::eAffordanceTypeGrasp;
    affordanceTypes[platformGraspAffordance->getName()] = memoryx::eAffordanceTypeGraspPlatform;
    affordanceTypes[prismaticGraspAffordance->getName()] = memoryx::eAffordanceTypeGraspPrismatic;
    affordanceTypes[supportAffordance->getName()] = memoryx::eAffordanceTypeSupport;
    affordanceTypes[leanAffordance->getName()] = memoryx::eAffordanceTypeLean;
    affordanceTypes[holdAffordance->getName()] = memoryx::eAffordanceTypeHold;
    affordanceTypes[pushAffordance->getName()] = memoryx::eAffordanceTypePush;
    affordanceTypes[pullAffordance->getName()] = memoryx::eAffordanceTypePull;
    affordanceTypes[liftAffordance->getName()] = memoryx::eAffordanceTypeLift;
    affordanceTypes[turnAffordance->getName()] = memoryx::eAffordanceTypeTurn;
    affordanceTypes[pushAffordance->getName()] = memoryx::eAffordanceTypePush;
    affordanceTypes[bimanualPlatformGraspAffordance->getName()] = memoryx::eAffordanceTypeBimanualGraspPlatform;
    affordanceTypes[bimanualPrismaticGraspAffordance->getName()] = memoryx::eAffordanceTypeBimanualGraspPrismatic;
    affordanceTypes[bimanualGraspAffordance->getName()] = memoryx::eAffordanceTypeBimanualGrasp;
    affordanceTypes[bimanualOpposedGraspAffordance->getName()] = memoryx::eAffordanceTypeBimanualGraspOpposed;
    affordanceTypes[bimanualAlignedGraspAffordance->getName()] = memoryx::eAffordanceTypeBimanualGraspAligned;
    affordanceTypes[bimanualOpposedPlatformGraspAffordance->getName()] = memoryx::eAffordanceTypeBimanualOpposedGraspPlatform;
    affordanceTypes[bimanualOpposedPrismaticGraspAffordance->getName()] = memoryx::eAffordanceTypeBimanualOpposedGraspPrismatic;
    affordanceTypes[bimanualAlignedPlatformGraspAffordance->getName()] = memoryx::eAffordanceTypeBimanualAlignedGraspPlatform;
    affordanceTypes[bimanualAlignedPrismaticGraspAffordance->getName()] = memoryx::eAffordanceTypeBimanualAlignedGraspPrismatic;
    affordanceTypes[bimanualTurnAffordance->getName()] = memoryx::eAffordanceTypeBimanualTurn;
    affordanceTypes[bimanualLiftAffordance->getName()] = memoryx::eAffordanceTypeBimanualLift;
 
    unimanualAffordances.push_back(platformGraspAffordance);
    unimanualAffordances.push_back(prismaticGraspAffordance);
    unimanualAffordances.push_back(graspAffordance);
    unimanualAffordances.push_back(supportAffordance);
    unimanualAffordances.push_back(leanAffordance);
    unimanualAffordances.push_back(liftAffordance);
    unimanualAffordances.push_back(holdAffordance);
    unimanualAffordances.push_back(turnAffordance);
    unimanualAffordances.push_back(pushAffordance);
    unimanualAffordances.push_back(pullAffordance);
 
    bimanualAffordances.push_back(bimanualPlatformGraspAffordance);
    bimanualAffordances.push_back(bimanualPrismaticGraspAffordance);
    bimanualAffordances.push_back(bimanualGraspAffordance);
    bimanualAffordances.push_back(bimanualOpposedGraspAffordance);
    bimanualAffordances.push_back(bimanualAlignedGraspAffordance);
    bimanualAffordances.push_back(bimanualOpposedPlatformGraspAffordance);
    bimanualAffordances.push_back(bimanualOpposedPrismaticGraspAffordance);
    bimanualAffordances.push_back(bimanualAlignedPlatformGraspAffordance);
    bimanualAffordances.push_back(bimanualAlignedPrismaticGraspAffordance);
    bimanualAffordances.push_back(bimanualTurnAffordance);
    bimanualAffordances.push_back(bimanualLiftAffordance);
 
    maxPrimitiveSamplingSize = getProperty<int>("MaxPrimitiveSamplingSize").getValue();
    ARMARX_INFO << "Ignoring primitives with samplings larger than " << maxPrimitiveSamplingSize << " entries";
 
    std::string affordanceRestrictionString = getProperty<std::string>("RestrictToAffordanceList").getValue();
    if (affordanceRestrictionString != "")
    {
        auto affordanceRestrictionList = Split(affordanceRestrictionString, ",");
        affordanceRestriction.insert(affordanceRestrictionList.begin(), affordanceRestrictionList.end());
    }
 
    std::vector<std::string> affordances;
    for (auto& a : unimanualAffordances)
    {
        if (affordanceRestriction.size() == 0 || affordanceRestriction.find(a->getName()) != affordanceRestriction.end())
        {
            affordances.push_back(a->getName());
        }
    }
 
    for (auto& a : bimanualAffordances)
    {
        if (affordanceRestriction.size() == 0 || affordanceRestriction.find(a->getName()) != affordanceRestriction.end())
        {
            affordances.push_back(a->getName());
        }
    }
 
    ARMARX_INFO << "Configured affordances: " << affordances;
 
    offeringTopic(getProperty<std::string>("AffordanceExtractionTopicName").getValue());
    usingTopic(getProperty<std::string>("PrimitiveExtractionTopicName").getValue());
 
    usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
 
    offeringTopic(getProperty<std::string>("DebugObserverName").getValue());
 
    affordanceExtractionTask = new PeriodicTask<AffordanceExtraction>(this, &AffordanceExtraction::affordanceExtractionThread, 1000);
}
 
void AffordanceExtraction::onConnectComponent()
{
    const Ice::CommunicatorPtr ic = getIceManager()->getCommunicator();
 
    workingMemoryPrx = getProxy<memoryx::WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
    if (!workingMemoryPrx)
    {
        ARMARX_ERROR << "Failed to obtain working memory proxy";
        return;
    }
 
    affordanceSegment = workingMemoryPrx->getAffordanceSegment();
    if (!affordanceSegment)
    {
        ARMARX_ERROR << "Failed to obtain affordance segment pointer";
        return;
    }
 
    environmentalPrimitiveSegment = workingMemoryPrx->getEnvironmentalPrimitiveSegment();
    if (!environmentalPrimitiveSegment)
    {
        ARMARX_ERROR << "Failed to obtain environmental primitive segment pointer";
        return;
    }
 
    debugObserver = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverName").getValue());
 
    listenerPrx = getTopic<AffordanceExtractionListenerPrx>(getProperty<std::string>("AffordanceExtractionTopicName").getValue());
    affordanceExtractionTask->start();
 
    hasNewPrimitives = false;
}
 
void AffordanceExtraction::onDisconnectComponent()
{
    affordanceExtractionTask->stop();
}
 
void AffordanceExtraction::onExitComponent()
{
}
 
void AffordanceExtraction::addObservation(const PoseBasePtr& endEffectorPose, const std::string& affordance, const std::string& primitiveId, float result, float certainty, float sigma_pos, float sigma_rot, const Ice::Current& c)
{
    std::unique_lock lock1(primitivesMutex);
    std::unique_lock lock2(affordancesMutex);
 
    Eigen::Matrix4f pose = PosePtr::dynamicCast(endEffectorPose)->toEigen();
 
    if (!primitiveSegmentation)
    {
        // This might happen if primitives have already been present in the memory on startup
        primitiveSegmentation.reset(new AffordanceKitArmarX::PrimitiveSetArmarX(environmentalPrimitiveSegment));
    }
 
    ARMARX_INFO << "Adding Experiment(primitive=" << primitiveId << ", affordance=" << affordance << ", result=" << result << ", certainty=" << certainty << ", sigma_pos=" << sigma_pos << ", sigma_rot=" << sigma_rot << ")";
 
    AffordanceKit::PrimitivePtr primitive = primitiveSegmentation->getPrimitiveById(primitiveId);
 
    bool found = false;
    for (auto& a : unimanualAffordances)
    {
        if (a->getName() == affordance && a->existsForPrimitive(primitive))
        {
            a->addObservation(AffordanceKit::ObservationPtr(new AffordanceKit::Observation(primitive, pose, result, certainty, sigma_pos, sigma_rot)));
            found = true;
        }
    }
 
    for (auto& a : bimanualAffordances)
    {
        if (a->getName() == affordance && a->existsForPrimitive(primitive))
        {
            a->addObservation(AffordanceKit::ObservationPtr(new AffordanceKit::Observation(primitive, pose, result, certainty, sigma_pos, sigma_rot)));
            found = true;
        }
    }
 
    if (!found)
    {
        ARMARX_WARNING << "Discarding experiment, because affordance or primitive were not found.";
    }
 
    // Export updated primitives to memory
    AffordanceKitArmarX::SceneArmarX s(primitiveSegmentation, unimanualAffordances, bimanualAffordances);
    s.writeToMemory(environmentalPrimitiveSegment, affordanceSegment);
    ARMARX_INFO << (unimanualAffordances.size() + bimanualAffordances.size()) << " affordances updated and reported";
 
    listenerPrx->reportNewAffordances();
}
 
void AffordanceExtraction::exportScene(const std::string& filename, const Ice::Current& c)
{
    ARMARX_INFO << "Exporting current scene to '" << filename << "'";
 
    AffordanceKitArmarX::SceneArmarX s(environmentalPrimitiveSegment, affordanceSegment);
    s.save(filename);
}
 
void AffordanceExtraction::importScene(const std::string& filename, const Ice::Current& c)
{
    ARMARX_INFO << "Importing scene from '" << filename << "'";
 
    AffordanceKitArmarX::SceneArmarX s;
    s.load(filename);
    s.writeToMemory(environmentalPrimitiveSegment, affordanceSegment);
 
    listenerPrx->reportNewAffordances();
}
 
void AffordanceExtraction::affordanceExtractionThread()
{
    AffordanceKit::PrimitiveSetPtr primitives;
 
    {
        std::unique_lock lock(primitivesMutex);
 
        if (!hasNewPrimitives)
        {
            return;
        }
        hasNewPrimitives = false;
 
        primitives = primitiveSegmentation;
    }
 
    {
        std::unique_lock lock(enableMutex);
 
        if (!enabled)
        {
            return;
        }
    }
 
    long originalTimestamp = 0;
    if (primitives->size() > 0)
    {
        originalTimestamp = (*primitives->begin())->getTimestamp();
 
        IceUtil::Time ts = IceUtil::Time::microSeconds(originalTimestamp);
        std::string timestampString = ts.toDateTime().substr(ts.toDateTime().find(' ') + 1);
 
        ARMARX_INFO << "Processing new set of primitives (timestamp: " << timestampString << ")";
 
        environmentalPrimitiveSegment->removeOlderPrimitives(new armarx::TimestampVariant(originalTimestamp));
 
        ARMARX_INFO << "Primitives older than " << timestampString << " removed";
    }
 
    int spatialStepSize = getProperty<float>("SpatialSamplingDistance").getValue();
    int numOrientationalSteps = getProperty<int>("NumOrientationalSamplings").getValue();
 
    {
        std::unique_lock lock(affordancesMutex);
 
        for (auto& a : unimanualAffordances)
        {
            if (affordanceRestriction.size() == 0 || affordanceRestriction.find(a->getName()) != affordanceRestriction.end())
            {
                a->reset();
            }
        }
        for (auto& a : bimanualAffordances)
        {
            if (affordanceRestriction.size() == 0 || affordanceRestriction.find(a->getName()) != affordanceRestriction.end())
            {
                a->reset();
            }
        }
 
        bool samplingNeeded = true;
        for (auto& primitive : *primitives)
        {
            if (primitive->getSamplingSize() > 0)
            {
                samplingNeeded = false;
            }
 
            if (maxPrimitiveSamplingSize > 0 && primitive->getSamplingSize() > maxPrimitiveSamplingSize)
            {
                // Sampling was read from memory, so we need to apply the max sampling size policy again
                ARMARX_INFO << "Ignoring primitive " << primitive->getId() << ", sampling size " << primitive->getSamplingSize() << " is too large";
                primitive->resetSampling();
            }
        }
 
        IceUtil::Time t = IceUtil::Time::now();
        if (samplingNeeded)
        {
            primitives->sample(spatialStepSize, numOrientationalSteps, maxPrimitiveSamplingSize);
            ARMARX_INFO << "Sampling of primitives took " << (IceUtil::Time::now() - t).toMilliSeconds() << " ms";
        }
        else
        {
            ARMARX_INFO << "Sampling not necessary";
        }
 
        for (auto& a : unimanualAffordances)
        {
            if (affordanceRestriction.size() == 0 || affordanceRestriction.find(a->getName()) != affordanceRestriction.end())
            {
                IceUtil::Time t = IceUtil::Time::now();
                a->evaluatePrimitiveSet(primitives);
                ARMARX_INFO << "Evaluation of affordance '" << a->getName() << "' took " << (IceUtil::Time::now() - t).toMilliSeconds() << " ms";
            }
        }
 
        for (auto& a : bimanualAffordances)
        {
            if (affordanceRestriction.size() == 0 || affordanceRestriction.find(a->getName()) != affordanceRestriction.end())
            {
                IceUtil::Time t = IceUtil::Time::now();
                a->evaluatePrimitiveSet(primitives);
                ARMARX_INFO << "Evaluation of affordance '" << a->getName() << "' took " << (IceUtil::Time::now() - t).toMilliSeconds() << " ms";
            }
        }
 
        ARMARX_INFO << "Total time for affordance extraction: " << (IceUtil::Time::now() - t).toMilliSeconds() << " ms";
 
        {
            std::unique_lock lock(enableMutex);
 
            if (!enabled)
            {
                ARMARX_INFO << "Affordance extraction interrupted (pipeline step disabled)";
                return;
            }
        }
 
        t = IceUtil::Time::now();
        AffordanceKitArmarX::SceneArmarX s(primitives, unimanualAffordances, bimanualAffordances);
        s.writeToMemory(environmentalPrimitiveSegment, affordanceSegment);
 
        std::string affordanceName = "";
        float uncertainty = s.getTotalUncertainty(affordanceName);
        float positiveDecisionRate = s.getPositiveDecisionRate(affordanceName);
        float negativeDecisionRate = s.getNegativeDecisionRate(affordanceName);
        float conflict = s.getTotalConflict(affordanceName);
 
        armarx::StringVariantBaseMap valueMap;
        valueMap["uncertainty"] = new Variant(uncertainty);
        valueMap["positiveDecisionRate"] = new Variant(positiveDecisionRate);
        valueMap["negativeDecisionRate"] = new Variant(negativeDecisionRate);
        valueMap["conflict"] = new Variant(conflict);
        debugObserver->setDebugChannel(getName(), valueMap);
 
 
        ARMARX_INFO << (unimanualAffordances.size() + bimanualAffordances.size()) << " new affordances written to memory: " << (IceUtil::Time::now() - t).toMilliSeconds() << " ms";
 
        {
            std::unique_lock lock(timestampMutex);
            lastProcessedTimestamp = originalTimestamp;
        }
 
        listenerPrx->reportNewAffordances();
        ARMARX_INFO << (unimanualAffordances.size() + bimanualAffordances.size()) << " affordances extracted and reported";
    }
}
 
PropertyDefinitionsPtr AffordanceExtraction::createPropertyDefinitions()
{
    return PropertyDefinitionsPtr(new AffordanceExtractionPropertyDefinitions(getConfigIdentifier()));
}
 
void armarx::AffordanceExtraction::enablePipelineStep(const Ice::Current& c)
{
    std::unique_lock lock(enableMutex);
 
    ARMARX_INFO << "Pipeline step enabled";
    enabled = true;
}
 
void AffordanceExtraction::disablePipelineStep(const Ice::Current& c)
{
    std::unique_lock lock(enableMutex);
 
    ARMARX_INFO << "Pipeline step disabled";
    enabled = false;
}
 
bool AffordanceExtraction::isPipelineStepEnabled(const Ice::Current& c)
{
    std::unique_lock lock(enableMutex);
    return enabled;
}
 
armarx::TimestampBasePtr armarx::AffordanceExtraction::getLastProcessedTimestamp(const Ice::Current& c)
{
    std::unique_lock lock(timestampMutex);
    return new armarx::TimestampVariant(lastProcessedTimestamp);
}