Affordance.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    MemoryX::Core
* @author     Peter Kaiser <peter dot kaiser at kit dot edu>
* @date       2014
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "Affordance.h"
 
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
 
namespace memoryx
{
    Affordance::Affordance(const std::string& name, const std::string& id) :
        Entity()
    {
        setName(name);
        setId(id);
    }
 
    void Affordance::setType(AffordanceType type, const Ice::Current& c)
    {
        putAttribute("type", static_cast<int>(type));
    }
 
    AffordanceType Affordance::getType(const Ice::Current& c) const
    {
        return static_cast<AffordanceType>(getAttributeValue("type")->getInt());
    }
 
    void Affordance::setPosition(const armarx::Vector3BasePtr& position, const Ice::Current& c)
    {
        putAttribute("position", position);
    }
 
    armarx::Vector3BasePtr Affordance::getPosition(const Ice::Current& c) const
    {
        return getAttributeValue("position")->getClass<armarx::Vector3Base>();
    }
 
    void Affordance::setPrimitiveId(const std::string& id, const Ice::Current& c)
    {
        putAttribute("primitiveId", id);
    }
 
    std::string Affordance::getPrimitiveId(const Ice::Current& c) const
    {
        return getAttributeValue("primitiveId")->getString();
    }
 
    void Affordance::setValidationStatus(AffordanceValidationStatus status, const Ice::Current& c)
    {
        putAttribute("validationStatus", static_cast<int>(status));
    }
 
    AffordanceValidationStatus Affordance::getValidationStatus(const Ice::Current& c) const
    {
        return static_cast<AffordanceValidationStatus>(getAttributeValue("validationStatus")->getInt());
    }
 
    float Affordance::getProbability(const Ice::Current& c) const
    {
        return getAttributeValue("probability")->getFloat();
    }
 
    void Affordance::setProbability(float probability, const Ice::Current& c)
    {
        putAttribute("probability", probability);
    }
 
    armarx::TimestampBasePtr  Affordance::getTime(const Ice::Current&) const
    {
        return getAttributeValue("time")->getClass<armarx::TimestampBase>();
    }
 
    void Affordance::setTime(const armarx::TimestampBasePtr& time, const Ice::Current&)
    {
        putAttribute("time", time);
    }
 
    armarx::MatrixFloatBasePtr Affordance::getCertaintyFunction(const Ice::Current& c) const
    {
        auto entity = getAttribute("certaintyFunction");
        if (!entity || entity->size() <= 0)
        {
            return armarx::MatrixFloatBasePtr(new armarx::MatrixFloat(2, 0));
        }
 
        return armarx::VariantPtr::dynamicCast(entity->getValueAt(0))->getClass<armarx::MatrixFloat>();
    }
 
    void Affordance::setCertaintyFunction(const armarx::MatrixFloatBasePtr& certainties, const Ice::Current& c)
    {
        armarx::VariantPtr variant(new armarx::Variant);
        variant->set<armarx::MatrixFloat>(certainties);
 
        EntityAttributeBasePtr entity = new EntityAttribute("certaintyFunction");
        entity->addValue(variant);
        putAttribute(entity);
    }
 
    AffordanceObservationList Affordance::getObservations(const Ice::Current& c) const
    {
        AffordanceObservationList result;
 
        auto entity = getAttribute("observations");
        if (!entity || entity->size() <= 0)
        {
            return result;
        }
 
        for (int i = 0; i < entity->size(); i++)
        {
            result.push_back(armarx::VariantPtr::dynamicCast(entity->getValueAt(i))->getClass<armarx::MatrixFloat>());
        }
 
        return result;
    }
 
    void Affordance::setObservations(const AffordanceObservationList& observations, const Ice::Current& c)
    {
        EntityAttributeBasePtr entity = new EntityAttribute("observations");
 
        for (auto& observation : observations)
        {
            armarx::VariantPtr variant(new armarx::Variant);
            variant->set<armarx::MatrixFloat>(armarx::MatrixFloatPtr::dynamicCast(observation));
            entity->addValue(variant);
        }
 
        putAttribute(entity);
    }
 
    void Affordance::addObservation(const armarx::MatrixFloatBasePtr& observation, const Ice::Current& c)
    {
        EntityAttributeBasePtr entity = new EntityAttribute("observations");
 
        armarx::VariantPtr variant(new armarx::Variant);
        variant->set<armarx::MatrixFloat>(observation);
        entity->addValue(variant);
 
        putAttribute(entity);
    }
 
    std::string Affordance::getTypeName() const
    {
        switch (getType())
        {
            case memoryx::eAffordanceTypeGrasp:
                return "G";
 
            case memoryx::eAffordanceTypeGraspPlatform:
                return "G-Pl";
 
            case memoryx::eAffordanceTypeGraspPrismatic:
                return "G-Pr";
 
            case memoryx::eAffordanceTypeSupport:
                return "Sp";
 
            case memoryx::eAffordanceTypeLean:
                return "Ln";
 
            case memoryx::eAffordanceTypeHold:
                return "Hd";
 
            case memoryx::eAffordanceTypePush:
                return "Ps";
 
            case memoryx::eAffordanceTypeLift:
                return "Lf";
 
            case memoryx::eAffordanceTypeTurn:
                return "Tn";
 
            case memoryx::eAffordanceTypeBimanualGraspPlatform:
                return "Bi-G-Pl";
 
            case memoryx::eAffordanceTypeBimanualOpposedGraspPlatform:
                return "Bi-OG-Pl";
 
            case memoryx::eAffordanceTypeBimanualGraspPrismatic:
                return "Bi-G-Pr";
 
            case memoryx::eAffordanceTypeBimanualOpposedGraspPrismatic:
                return "Bi-OG-Pr";
 
            case memoryx::eAffordanceTypeBimanualTurn:
                return "Bi-Tn";
 
            case memoryx::eAffordanceTypeBimanualLift:
                return "Bi-Lf";
 
            case memoryx::eAffordanceTypeBimanualPush:
                return "Bi-Ps";
 
            default:
                return "Unknown";
        }
    }
 
    Ice::ObjectPtr Affordance::ice_clone() const
    {
        return this->clone();
    }
 
    AffordancePtr Affordance::clone(const Ice::Current& c) const
    {
        return new Affordance(*this);
    }
}