SceneArmarX.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    AffordanceKitArmarX
 * @author     Peter Kaiser ( peter dot kaiser at kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl.txt
 *             GNU General Public License
 */
 
#include "SceneArmarX.h"
 
#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
 
#include <RobotAPI/interface/core/PoseBase.h>
 
#include "PrimitiveSetArmarX.h"
#include <AffordanceKit/affordances/BimanualAlignedGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualAlignedPlatformGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualAlignedPrismaticGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualLiftAffordance.h>
#include <AffordanceKit/affordances/BimanualOpposedGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualOpposedPlatformGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualOpposedPrismaticGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualPlatformGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualPrismaticGraspAffordance.h>
#include <AffordanceKit/affordances/BimanualTurnAffordance.h>
#include <AffordanceKit/affordances/GraspAffordance.h>
#include <AffordanceKit/affordances/HoldAffordance.h>
#include <AffordanceKit/affordances/LeanAffordance.h>
#include <AffordanceKit/affordances/LiftAffordance.h>
#include <AffordanceKit/affordances/PlatformGraspAffordance.h>
#include <AffordanceKit/affordances/PrismaticGraspAffordance.h>
#include <AffordanceKit/affordances/PullAffordance.h>
#include <AffordanceKit/affordances/PushAffordance.h>
#include <AffordanceKit/affordances/SupportAffordance.h>
#include <AffordanceKit/affordances/TurnAffordance.h>
#include <MemoryX/libraries/memorytypes/segment/AffordanceSegment.h>
#include <MemoryX/libraries/memorytypes/segment/EnvironmentalPrimitiveSegment.h>
 
namespace AffordanceKitArmarX
{
    SceneArmarX::SceneArmarX()
    {
        generateAffordanceTypes();
    }
 
    SceneArmarX::SceneArmarX(
        const AffordanceKit::PrimitiveSetPtr& primitives,
        const std::vector<AffordanceKit::UnimanualAffordancePtr>& unimanualAffordances,
        const std::vector<AffordanceKit::BimanualAffordancePtr>& bimanualAffordances) :
        AffordanceKit::Scene(primitives, unimanualAffordances, bimanualAffordances)
    {
        generateAffordanceTypes();
    }
 
    SceneArmarX::SceneArmarX(memoryx::EnvironmentalPrimitiveSegmentBasePrx& primitiveSegment,
                             memoryx::AffordanceSegmentBasePrx& affordanceSegment)
    {
        auto t = IceUtil::Time::now();
 
        // Fetch affordances from memory
        memoryx::AffordanceBaseList affordances = affordanceSegment->getAffordances();
 
        if (affordances.size() == 0)
        {
            // If there are no affordances in memory, just fetch the most recent primitives
            primitives.reset(new PrimitiveSetArmarX(primitiveSegment));
            return;
        }
 
        long timestamp = (*affordances.begin())->getTime()->timestamp;
 
        // Fetch primitives from memory (only those with appropriate timestamp)
        primitives.reset(new PrimitiveSetArmarX(primitiveSegment, timestamp));
 
        std::cout << "Creating a primitive set from memory took: "
                  << (IceUtil::Time::now() - t).toMilliSeconds() << " ms" << std::endl;
 
        t = IceUtil::Time::now();
 
#pragma omp parallel for
        for (unsigned int i = 0; i < affordances.size(); i++)
        {
            AffordanceKit::UnimanualAffordancePtr uni;
            AffordanceKit::BimanualAffordancePtr bi;
 
            switch (affordances[i]->getType())
            {
                case memoryx::eAffordanceTypeGraspPlatform:
                    uni.reset(new AffordanceKit::PlatformGraspAffordance());
                    break;
 
                case memoryx::eAffordanceTypeGraspPrismatic:
                    uni.reset(new AffordanceKit::PrismaticGraspAffordance());
                    break;
 
                case memoryx::eAffordanceTypeGrasp:
                    uni.reset(new AffordanceKit::GraspAffordance());
                    break;
 
                case memoryx::eAffordanceTypeSupport:
                    uni.reset(new AffordanceKit::SupportAffordance());
                    break;
 
                case memoryx::eAffordanceTypeLean:
                    uni.reset(new AffordanceKit::LeanAffordance());
                    break;
 
                case memoryx::eAffordanceTypeHold:
                    uni.reset(new AffordanceKit::HoldAffordance());
                    break;
 
                case memoryx::eAffordanceTypeLift:
                    uni.reset(new AffordanceKit::LiftAffordance());
                    break;
 
                case memoryx::eAffordanceTypeTurn:
                    uni.reset(new AffordanceKit::TurnAffordance());
                    break;
 
                case memoryx::eAffordanceTypePush:
                    uni.reset(new AffordanceKit::PushAffordance());
                    break;
 
                case memoryx::eAffordanceTypePull:
                    uni.reset(new AffordanceKit::PullAffordance());
                    break;
 
                case memoryx::eAffordanceTypeBimanualGraspPlatform:
                    bi.reset(new AffordanceKit::BimanualPlatformGraspAffordance());
                    break;
 
                case memoryx::eAffordanceTypeBimanualGraspPrismatic:
                    bi.reset(new AffordanceKit::BimanualPrismaticGraspAffordance());
                    break;
 
                case memoryx::eAffordanceTypeBimanualGrasp:
                    bi.reset(new AffordanceKit::BimanualGraspAffordance());
                    break;
 
                case memoryx::eAffordanceTypeBimanualGraspOpposed:
                    bi.reset(new AffordanceKit::BimanualOpposedGraspAffordance());
                    break;
 
                case memoryx::eAffordanceTypeBimanualGraspAligned:
                    bi.reset(new AffordanceKit::BimanualAlignedGraspAffordance());
                    break;
 
                case memoryx::eAffordanceTypeBimanualOpposedGraspPlatform:
                    bi.reset(new AffordanceKit::BimanualOpposedPlatformGraspAffordance());
                    break;
 
                case memoryx::eAffordanceTypeBimanualOpposedGraspPrismatic:
                    bi.reset(new AffordanceKit::BimanualOpposedPrismaticGraspAffordance());
                    break;
 
                case memoryx::eAffordanceTypeBimanualAlignedGraspPlatform:
                    bi.reset(new AffordanceKit::BimanualAlignedPlatformGraspAffordance());
                    break;
 
                case memoryx::eAffordanceTypeBimanualAlignedGraspPrismatic:
                    bi.reset(new AffordanceKit::BimanualAlignedPrismaticGraspAffordance());
                    break;
 
                case memoryx::eAffordanceTypeBimanualTurn:
                    bi.reset(new AffordanceKit::BimanualTurnAffordance());
                    break;
 
                case memoryx::eAffordanceTypeBimanualLift:
                    bi.reset(new AffordanceKit::BimanualLiftAffordance());
                    break;
 
                case memoryx::eAffordanceTypeBimanualSupport:
                case memoryx::eAffordanceTypeBimanualLean:
                case memoryx::eAffordanceTypeBimanualHold:
                case memoryx::eAffordanceTypeBimanualPush:
                case memoryx::eAffordanceTypeBimanualPull:
                    ARMARX_WARNING_S << "Affordance type " << affordances[i]->getType()
                                     << " not yet included in AffordanceKitArmarX";
                    break;
            }
 
            AffordanceKit::PrimitivePtr p =
                primitives->getPrimitiveById(affordances[i]->getPrimitiveId());
            if (!p)
            {
                std::cout << "Warning: Affordance relates to unknown primitive Id" << std::endl;
                continue;
            }
 
            AffordanceKit::ThetaFunctionPtr theta(new AffordanceKit::ThetaFunction());
            (*theta)[AffordanceKit::PrimitivePair(p)] =
                armarx::MatrixFloatPtr::dynamicCast(affordances[i]->getCertaintyFunction())
                    ->toEigen();
 
            for (auto& observation : affordances[i]->getObservations())
            {
                AffordanceKit::ThetaFunctionPtr theta(new AffordanceKit::ThetaFunction());
                (*theta)[AffordanceKit::PrimitivePair(p)] =
                    armarx::MatrixFloatPtr::dynamicCast(observation)->toEigen();
 
                if (uni)
                {
                    uni->addObservation(
                        AffordanceKit::ObservationPtr(new AffordanceKit::Observation(theta)));
                }
                else
                {
                    bi->addObservation(
                        AffordanceKit::ObservationPtr(new AffordanceKit::Observation(theta)));
                }
            }
 
            if (uni)
            {
                uni->setTheta(theta);
                uni->setTimestamp(affordances[i]->getTime()->timestamp);
 
#pragma omp critical
                {
                    unimanualAffordances.push_back(uni);
                    AffordanceKit::Scene::affordances.push_back(uni);
                }
            }
            else if (bi)
            {
                bi->setTheta(theta);
                bi->setTimestamp(affordances[i]->getTime()->timestamp);
 
#pragma omp critical
                {
                    bimanualAffordances.push_back(bi);
                    AffordanceKit::Scene::affordances.push_back(bi);
                }
            }
        }
 
        std::cout << "Creating a scene from memory took: "
                  << (IceUtil::Time::now() - t).toMilliSeconds() << " ms ("
                  << unimanualAffordances.size() << " unimanual affordances and "
                  << bimanualAffordances.size() << " bimanual affordances)" << std::endl;
    }
 
    void
    SceneArmarX::writeToMemory(
        const memoryx::EnvironmentalPrimitiveSegmentBasePrx& primitiveSegment,
        const memoryx::AffordanceSegmentBasePrx& affordanceSegment) const
    {
        // Write primitives to memory
        PrimitiveSetArmarX p(primitives);
        p.writeToMemory(primitiveSegment);
 
        // Write affordances to memory
        std::vector<memoryx::EntityBasePtr> unimanualAffordanceList;
        unimanualAffordanceList.reserve(unimanualAffordances.size());
 
        for (auto& primitive : *primitives)
        {
            // Write unimanual affordances to memory
            for (std::vector<AffordanceKit::UnimanualAffordancePtr>::const_iterator a =
                     unimanualAffordances.begin();
                 a < unimanualAffordances.end();
                 a++)
            {
                AffordanceKit::UnimanualAffordancePtr affordance = *a;
                if (!affordance->existsForPrimitive(primitive))
                {
                    continue;
                }
 
                AffordanceKit::Belief value;
                Eigen::Matrix4f pose = affordance->getMostCertainPoseForPrimitive(primitive, value);
 
                if (affordanceTypes.find(affordance->getName()) != affordanceTypes.end())
                {
                    memoryx::AffordanceBasePtr a_mx(new memoryx::Affordance);
                    a_mx->setType(affordanceTypes.at(affordance->getName()));
                    a_mx->setPosition(new armarx::Vector3(Eigen::Vector3f(pose.block<3, 1>(0, 3))));
                    a_mx->setPrimitiveId(primitive->getId());
                    a_mx->setValidationStatus(memoryx::eAffordanceNotValidated);
                    a_mx->setTime(new armarx::TimestampVariant(primitive->getTimestamp()));
                    a_mx->setCertaintyFunction(new armarx::MatrixFloat(
                        affordance->getTheta(AffordanceKit::PrimitivePair(primitive))));
 
                    memoryx::AffordanceObservationList observations;
                    for (auto& observation : affordance->getObservations())
                    {
                        if (observation->existsForPrimitive(primitive))
                        {
                            observations.push_back(
                                new armarx::MatrixFloat(observation->getTheta(primitive)));
                        }
                    }
                    a_mx->setObservations(observations);
 
                    unimanualAffordanceList.push_back(a_mx);
                }
            }
        }
 
        std::vector<memoryx::EntityBasePtr> bimanualAffordanceList;
        bimanualAffordanceList.reserve(bimanualAffordances.size());
 
        for (auto& primitive : *primitives)
        {
            // Write bimanual affordances to memory
            for (std::vector<AffordanceKit::BimanualAffordancePtr>::const_iterator a =
                     bimanualAffordances.begin();
                 a < bimanualAffordances.end();
                 a++)
            {
                AffordanceKit::BimanualAffordancePtr affordance = *a;
                if (!affordance->existsForPrimitive(primitive))
                {
                    continue;
                }
 
                AffordanceKit::Belief value;
                std::pair<Eigen::Matrix4f, Eigen::Matrix4f> poses =
                    affordance->getMostCertainPosesForPrimitive(primitive, value);
                if (poses.first.isZero())
                {
                    continue;
                }
 
                if (affordanceTypes.find(affordance->getName()) != affordanceTypes.end())
                {
                    memoryx::AffordanceBasePtr a_mx(new memoryx::Affordance);
                    std::cout << affordance->getName() << std::endl;
                    a_mx->setType(affordanceTypes.at(affordance->getName()));
                    a_mx->setPosition(
                        new armarx::Vector3(Eigen::Vector3f(poses.first.block<3, 1>(0, 3))));
                    a_mx->setPrimitiveId(primitive->getId());
                    a_mx->setValidationStatus(memoryx::eAffordanceNotValidated);
                    a_mx->setTime(new armarx::TimestampVariant(primitive->getTimestamp()));
                    a_mx->setCertaintyFunction(new armarx::MatrixFloat(
                        affordance->getTheta(AffordanceKit::PrimitivePair(primitive, primitive))));
 
                    memoryx::AffordanceObservationList observations;
                    for (auto& observation : affordance->getObservations())
                    {
                        if (observation->existsForPrimitive(primitive))
                        {
                            observations.push_back(
                                new armarx::MatrixFloat(observation->getTheta(primitive)));
                        }
                    }
                    a_mx->setObservations(observations);
 
                    bimanualAffordanceList.push_back(a_mx);
                }
            }
        }
 
        affordanceSegment->clear();
        affordanceSegment->upsertEntityList(unimanualAffordanceList);
        affordanceSegment->upsertEntityList(bimanualAffordanceList);
    }
 
    void
    SceneArmarX::generateAffordanceTypes()
    {
        affordanceTypes["G"] = memoryx::eAffordanceTypeGrasp;
        affordanceTypes["G-Pl"] = memoryx::eAffordanceTypeGraspPlatform;
        affordanceTypes["G-Pr"] = memoryx::eAffordanceTypeGraspPrismatic;
        affordanceTypes["Sp"] = memoryx::eAffordanceTypeSupport;
        affordanceTypes["Ln"] = memoryx::eAffordanceTypeLean;
        affordanceTypes["Hd"] = memoryx::eAffordanceTypeHold;
        affordanceTypes["Ps"] = memoryx::eAffordanceTypePush;
        affordanceTypes["Lf"] = memoryx::eAffordanceTypeLift;
        affordanceTypes["Tn"] = memoryx::eAffordanceTypeTurn;
        affordanceTypes["Pl"] = memoryx::eAffordanceTypePull;
        affordanceTypes["Bi-G-Pl"] = memoryx::eAffordanceTypeBimanualGraspPlatform;
        affordanceTypes["Bi-G-Pr"] = memoryx::eAffordanceTypeBimanualGraspPrismatic;
        affordanceTypes["Bi-Op-G-Pl"] = memoryx::eAffordanceTypeBimanualOpposedGraspPlatform;
        affordanceTypes["Bi-Op-G-Pr"] = memoryx::eAffordanceTypeBimanualOpposedGraspPrismatic;
        affordanceTypes["Bi-Tn"] = memoryx::eAffordanceTypeBimanualTurn;
        affordanceTypes["Bi-Lf"] = memoryx::eAffordanceTypeBimanualLift;
    }
} // namespace AffordanceKitArmarX