DMPComponent.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2015-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    RobotAPI::ArmarXObjects::DMPComponent
 * @author     Mirko Waechter ( mirko dot waechter at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "DMPComponent.h"
 
#include <Ice/ObjectAdapter.h>
 
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunknown-pragmas"
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
#include <dmp/io/MMMConverter.h>
#include <dmp/testing/testdataset.h>
#pragma GCC diagnostic pop
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
 
using namespace armarx;
 
void
DMPComponent::onInitComponent()
{
    ARMARX_INFO << "initializing DMP component";
    usingProxy(getProperty<std::string>("LongtermMemoryName").getValue());
    ARMARX_INFO << "successfully initialized DMP component";
}
 
void
DMPComponent::onConnectComponent()
{
    ARMARX_INFO << "connecting DMP component";
 
    try
    {
        longtermMemoryPrx = getProxy<memoryx::LongtermMemoryInterfacePrx>(
            getProperty<std::string>("LongtermMemoryName").getValue());
    }
    catch (...)
    {
        ARMARX_ERROR << "cannot get longterm memory proxy";
        return;
    }
 
    try
    {
        dmpDataMemoryPrx = longtermMemoryPrx->getDMPSegment();
    }
    catch (...)
    {
        ARMARX_ERROR << "cannot get dmp segment of longterm memory";
        return;
    }
 
    ARMARX_INFO << "successfully connected DMP component";
}
 
void
DMPComponent::onDisconnectComponent()
{
    ARMARX_INFO << "disconnecting DMP component";
}
 
void
DMPComponent::onExitComponent()
{
    ARMARX_INFO << "exiting DMP component";
}
 
DMPInstanceBasePrx
DMPComponent::findInstancePrx(std::string name)
{
    DMPInstancePair dmpInstPair = dmpPool.find(name)->second;
    DMPInstanceBasePrx result = dmpInstPair.second;
    if (result)
    {
        return result;
    }
    else
    {
        ARMARX_INFO << "DMP not found.";
        return DMPInstanceBasePrx();
    }
}
 
DMPInstancePtr
DMPComponent::findInstancePtr(std::string name)
{
    DMPInstancePair dmpInstPair = dmpPool.find(name)->second;
    DMPInstancePtr result = dmpInstPair.first;
    if (result)
    {
        return result;
    }
    else
    {
        //TODO Correct?
        ARMARX_INFO << "DMP not found.";
        return nullptr;
    }
}
 
DMPInstanceBasePrx
DMPComponent::getDMP(const std::string& dmpName, const Ice::Current&)
{
    return findInstancePrx(dmpName);
}
 
DMPInstanceBasePrx
DMPComponent::instantiateDMP(const std::string& dmpName,
                             const std::string& DMPType,
                             int kernelSize,
                             const Ice::Current&)
{
    ARMARX_INFO << "instantiate DMP with name " << dmpName;
 
    boost::serialization::extended_type_info const* eti =
        boost::serialization::extended_type_info::find(DMPType.c_str());
 
    DMPInterfacePtr dmp;
 
    if (eti)
    {
        dmp.reset(static_cast<DMP::DMPInterface*>(eti->construct()));
    }
    else
    {
        DMPFactory dmpFactory;
        dmp = dmpFactory.getDMP(DMPType, kernelSize);
    }
 
    DMPInstancePtr dmpPtr = new DMPInstance(dmp, DMPType);
    dmpPtr->setTimeStep(timestep);
    dmpPtr->setCurrentTime(ctime);
 
    DMPInstanceBasePrx instprx = createDMPInstancePrx(dmpPtr);
    DMPInstancePair dmpInstPair(dmpPtr, instprx);
    dmpPool.insert(DMPPair(dmpName, dmpInstPair));
 
    ARMARX_INFO << "Successfully instantiate DMP ... ";
    return instprx;
}
 
// Database related functions
DMPInstanceBasePrx
DMPComponent::getDMPFromDatabase(const std::string& dmpEntityName,
                                 const std::string& dmpName,
                                 const Ice::Current&)
{
    ARMARX_INFO << "getting DMP from database";
 
    if (!dmpDataMemoryPrx->hasEntityByName(dmpEntityName))
    {
        ARMARX_ERROR << "DMP with name " + dmpEntityName + " does not exist in the database";
        return DMPInstanceBasePrx();
    }
 
    memoryx::DMPEntityPtr dmpEntity =
        memoryx::DMPEntityPtr::dynamicCast(dmpDataMemoryPrx->getDMPEntityByName(dmpEntityName));
 
 
    std::string textStr = dmpEntity->getDMPtextStr();
    ARMARX_INFO << textStr;
    std::stringstream istr;
    istr.str(textStr);
 
    boost::archive::text_iarchive ar(istr);
 
    DMP::DMPInterface* readPtr;
 
    ar >> boost::serialization::make_nvp("dmp", readPtr);
 
    DMP::DMPInterfacePtr dptr(readPtr);
    DMPInstancePtr dmpPtr(new DMPInstance(dptr));
 
    DMPInstanceBasePrx dmpPrx = createDMPInstancePrx(dmpPtr);
    std::string dmpLocalName = dmpName;
    if (dmpName == "UNKNOWN")
    {
        dmpLocalName = dmpPrx->ice_getIdentity().name;
    }
 
    if (dmpPool.find(dmpLocalName) != dmpPool.end())
    {
        ARMARX_ERROR << "DMP with DMPName " + dmpLocalName +
                            " exists in DMP pool. It is not allowed to overwrite it.";
        return dmpPrx;
    }
    dmpPtr->setTimeStep(timestep);
    dmpPtr->setCurrentTime(ctime);
    dmpPool.insert(DMPPair(dmpLocalName, DMPInstancePair(dmpPtr, dmpPrx)));
 
 
    ARMARX_INFO << "DMP with name: " + dmpEntityName + " is loaded into DMP Pool with dmpName " +
                       dmpLocalName + " and dmpType " + dmpPtr->getDMPType();
    ARMARX_INFO << "successfully got dmp from database.";
 
    return dmpPrx;
}
 
DMPInstanceBasePrx
DMPComponent::getDMPFromDatabaseById(const std::string& dbId, const Ice::Current&)
{
    ARMARX_INFO << "getting DMP from database";
 
    if (!dmpDataMemoryPrx->hasEntityById(dbId))
    {
        ARMARX_ERROR << "DMP with ID " + dbId + " does not exist in the database";
        return DMPInstanceBasePrx();
    }
 
    memoryx::DMPEntityPtr dmpEntity =
        memoryx::DMPEntityPtr::dynamicCast(dmpDataMemoryPrx->getDMPEntityById(dbId));
    std::string dmpEntityName = dmpEntity->getDMPName();
 
    return getDMPFromDatabase(dmpEntityName);
}
 
DMPInstanceBasePrx
DMPComponent::getDMPFromFile(const std::string& fileName,
                             const std::string& dmpName,
                             const Ice::Current&)
{
    ARMARX_INFO << "getting DMP from file";
 
    if (!std::filesystem::exists(fileName))
    {
        ARMARX_ERROR << "The dmp file: " << fileName << " does not exist!!!";
        return DMPInstanceBasePrx();
    }
 
    std::string ext =
        fileName.rfind(".") == fileName.npos ? fileName : fileName.substr(fileName.rfind(".") + 1);
 
    std::ifstream file(fileName);
    DMP::DMPInterface* readPtr;
 
    if (ext == "xml")
    {
        boost::archive::xml_iarchive ar(file);
        ar >> boost::serialization::make_nvp("dmp", readPtr);
    }
    else
    {
        ARMARX_ERROR << "The extension of the file cannot be recognized!!!";
        return DMPInstanceBasePrx();
    }
 
 
    DMP::DMPInterfacePtr dptr(readPtr);
    DMPInstancePtr dmpPtr(new DMPInstance(dptr));
    DMPInstanceBasePrx dmpPrx = createDMPInstancePrx(dmpPtr);
 
    std::string dmpLocalName = dmpName;
    if (dmpLocalName == "UNKNOWN" || dmpLocalName == "")
    {
        //        dmpLocalName = dptr->getDMPName();
 
        //        if (dmpLocalName == "UNKNOWN")
        //        {
        dmpLocalName = dmpPrx->ice_getIdentity().name;
        //        }
    }
    dmpPtr->setTimeStep(timestep);
    dmpPtr->setCurrentTime(ctime);
    if (dmpPool.find(dmpLocalName) != dmpPool.end())
    {
        ARMARX_ERROR << "DMP with DMPName " + dmpLocalName +
                            " exists in DMP pool. It is not allowed to overwrite it.";
        return dmpPrx;
    }
 
    dmpPool.insert(DMPPair(dmpLocalName, DMPInstancePair(dmpPtr, dmpPrx)));
 
    ARMARX_INFO << "DMP with name: " + dmpName + " is loaded into DMP Pool with dmpName " +
                       dmpLocalName + " and dmpType " + dmpPtr->getDMPType();
    ARMARX_INFO << "successfully got dmp from file.";
    file.close();
 
    return dmpPrx;
}
 
void
DMPComponent::storeDMPInFile(const std::string& fileName,
                             const std::string& dmpName,
                             const Ice::Current&)
{
    ARMARX_INFO << "storing DMP in the file";
 
    std::string ext =
        fileName.rfind(".") == fileName.npos ? fileName : fileName.substr(fileName.rfind(".") + 1);
    ofstream ofs(fileName);
 
    DMPInstancePtr dmpPtr = findInstancePtr(dmpName);
    DMPInterface* savedPtr = dmpPtr->getDMP().get();
    //    savedPtr->setDMPName(dmpName);
 
    if (ext == "xml")
    {
        boost::archive::xml_oarchive ar(ofs);
        ar << boost::serialization::make_nvp("dmp", savedPtr);
    }
    else
    {
        ARMARX_ERROR << "The extension of the file cannot be recognized!!!";
        return;
    }
 
    ARMARX_INFO << "successfully stored DMP in the file!!!";
    ofs.close();
 
    return;
}
 
void
DMPComponent::storeDMPInDatabase(const std::string& dmpName,
                                 const std::string& dmpEntityName,
                                 const ::Ice::Current&)
{
 
    ARMARX_INFO << "storing DMP in the database";
    memoryx::DMPEntityPtr dmpEntity = new memoryx::DMPEntity(dmpEntityName);
 
    DMPInstancePtr dmpPtr = findInstancePtr(dmpName);
 
    dmpEntity->setDMPType(dmpPtr->getDMP()->getDMPType());
    dmpEntity->setDMPName(dmpName);
 
    std::stringstream dmptext;
    boost::archive::text_oarchive ar(dmptext);
 
    DMPInterface* savedPtr = dmpPtr->getDMP().get();
    ar << boost::serialization::make_nvp("dmp", savedPtr);
 
    dmpEntity->setDMPtextStr(dmptext.str());
 
    std::cout << dmpEntity << std::endl;
    const std::string entityID = dmpDataMemoryPrx->addEntity(dmpEntity);
    dmpEntity->setId(entityID);
 
    ARMARX_INFO << "successfully stored DMP";
}
 
void
DMPComponent::removeDMPFromDatabase(const std::string& dmpEntityName, const ::Ice::Current&)
{
 
    ARMARX_INFO << "removing DMP from database";
 
    if (!dmpDataMemoryPrx->hasEntityByName(dmpEntityName))
    {
        ARMARX_ERROR << "DMP with name " + dmpEntityName + " does not exist in the database";
        return;
    }
 
    memoryx::DMPEntityPtr dmpEntity =
        memoryx::DMPEntityPtr::dynamicCast(dmpDataMemoryPrx->getDMPEntityByName(dmpEntityName));
    dmpDataMemoryPrx->removeEntity(dmpEntity->getId());
 
    if (!dmpDataMemoryPrx->hasEntityByName(dmpEntityName))
    {
        ARMARX_INFO << "successfully removed dmp from database";
    }
}
 
void
DMPComponent::removeDMPFromDatabaseById(const std::string& dbId, const Ice::Current&)
{
    ARMARX_INFO << "removing DMP from database";
 
    if (!dmpDataMemoryPrx->hasEntityById(dbId))
    {
        ARMARX_ERROR << "DMP with id " + dbId + " does not exist in the database";
        return;
    }
 
    memoryx::DMPEntityPtr dmpEntity =
        memoryx::DMPEntityPtr::dynamicCast(dmpDataMemoryPrx->getDMPEntityById(dbId));
    dmpDataMemoryPrx->removeEntity(dmpEntity->getId());
 
    if (!dmpDataMemoryPrx->hasEntityById(dbId))
    {
        ARMARX_INFO << "successfully removed dmp from database";
    }
}
 
// DMP trainner
void
DMPComponent::trainDMP(const std::string& dmpName, const ::Ice::Current&)
{
    // learn dmp
    ARMARX_INFO << "In Train DMP (name: " << dmpName << ")";
 
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        dmpPrx->trainDMP();
    }
    else
    {
        ARMARX_ERROR << "trainDMP: DMP not found.";
        return;
    }
    ARMARX_INFO << "Exit Train DMP (name: " << dmpName << ")";
}
 
void
DMPComponent::readTrajectoryFromFile(const std::string& dmpName,
                                     const std::string& file,
                                     double times,
                                     const Ice::Current&)
{
 
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        dmpPrx->readTrajectoryFromFile(file, times);
    }
    else
    {
        ARMARX_ERROR << "readTrajectoryFromFile: DMP not found.";
        return;
    }
}
 
// convenient setter functions
void
DMPComponent::setDMPState(const std::string& dmpName,
                          const ::armarx::cStateVec& state,
                          const ::Ice::Current&)
{
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        dmpPrx->setDMPState(state);
    }
    else
    {
        ARMARX_INFO << "setDMPState: DMP not found.";
        return;
    }
}
 
void
DMPComponent::setAmpl(const std::string& dmpName, int dim, double value, const Ice::Current&)
{
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        dmpPrx->setAmpl(dim, value);
    }
    else
    {
        ARMARX_ERROR << "setAmpl: DMP not found.";
        return;
    }
}
 
void
DMPComponent::setGoal(const std::string& dmpName, const Ice::DoubleSeq& value, const Ice::Current&)
{
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        dmpPrx->setGoal(value);
    }
    else
    {
        ARMARX_ERROR << "setGoal: DMP not found.";
        return;
    }
}
 
void
DMPComponent::setStartPosition(const std::string& dmpName,
                               const Ice::DoubleSeq& value,
                               const Ice::Current&)
{
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        dmpPrx->setStartPosition(value);
    }
    else
    {
        ARMARX_ERROR << "setStartPosition: DMP not found.";
    }
}
 
void
DMPComponent::setCanonicalValues(const std::string& dmpName,
                                 const Ice::DoubleSeq& value,
                                 const Ice::Current&)
{
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        return dmpPrx->setCanonicalValues(value);
    }
    else
    {
        ARMARX_ERROR << "setCanonicalValues: DMP not found.";
        return;
    }
}
 
void
DMPComponent::setTemporalFactor(const std::string& dmpName, double tau, const Ice::Current&)
{
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        dmpPrx->setTemporalFactor(tau);
    }
    else
    {
        ARMARX_ERROR << "getTemporalFactor: DMP not found.";
    }
}
 
void
DMPComponent::setTimeStep(double ts, const Ice::Current&)
{
    timestep = ts;
    for (DMPMap::iterator it = dmpPool.begin(); it != dmpPool.end(); it++)
    {
        it->second.first->setTimeStep(ts);
    }
}
 
// convenient getter functions
double
DMPComponent::getAmpl(const std::string& dmpName, int dim, const Ice::Current&)
{
 
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        return dmpPrx->getAmpl(dim);
    }
    else
    {
        ARMARX_ERROR << "getAmpl: DMP not found.";
        return 0;
    }
}
 
cStateVec
DMPComponent::getNextState(const std::string& dmpName,
                           const cStateVec& states,
                           const ::Ice::Current&)
{
    cStateVec nextState;
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        nextState = dmpPrx->getNextState(states);
    }
    else
    {
        ARMARX_ERROR << "getNextState: DMP not found.";
    }
 
    ARMARX_INFO << "Successfully got next state";
 
    return nextState;
}
 
cStateVec
DMPComponent::getCurrentState(const std::string& dmpName, const Ice::Current&)
{
    cStateVec curState;
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        curState = dmpPrx->getCurrentState();
    }
    else
    {
        ARMARX_ERROR << "getCurrentState: DMP not found.";
    }
 
    return curState;
}
 
Ice::DoubleSeq
DMPComponent::getCanonicalValues(const std::string& dmpName, const Ice::Current&)
{
    Ice::DoubleSeq canonicalValues;
 
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        canonicalValues = dmpPrx->getCanonicalValues();
    }
    else
    {
        ARMARX_ERROR << "getCanonicalValues: DMP not found.";
    }
 
    return canonicalValues;
}
 
double
DMPComponent::getTemporalFactor(const std::string& dmpName, const Ice::Current&)
{
    double temporalFactor = 0;
 
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        temporalFactor = dmpPrx->getTemporalFactor();
    }
    else
    {
        ARMARX_ERROR << "getTemporalFactor: DMP not found.";
    }
 
    return temporalFactor;
}
 
double
DMPComponent::getCurrentTime(const Ice::Current&)
{
 
    DMPMap::iterator it = dmpPool.begin();
 
    return it->second.first->getCurrentTime();
}
 
double
DMPComponent::getDampingFactor(const std::string& dmpName, const Ice::Current&)
{
    double dampingFactor = 0;
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        dampingFactor = dmpPrx->getDampingFactor();
    }
    else
    {
        ARMARX_ERROR << "getDampingFactor: DMP not found.";
    }
    return dampingFactor;
}
 
double
DMPComponent::getSpringFactor(const std::string& dmpName, const Ice::Current&)
{
    double springFactor{0};
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        springFactor = dmpPrx->getSpringFactor();
    }
    else
    {
        ARMARX_ERROR << "getSpringFactor: DMP not found.";
    }
    return springFactor;
}
 
int
DMPComponent::getDMPDim(const std::string& dmpName, const Ice::Current&)
{
    int dim = -1;
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        dim = dmpPrx->getDMPDim();
    }
    else
    {
        ARMARX_ERROR << "getDMPDim: DMP not found.";
    }
 
    return dim;
}
 
string
DMPComponent::getDMPType(const std::string& dmpName, const Ice::Current&)
{
    std::string dmpType;
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        dmpType = dmpPrx->getDMPType();
    }
    else
    {
        ARMARX_ERROR << "getDMPType: DMP not found.";
    }
    return dmpType;
}
 
Ice::DoubleSeq
DMPComponent::getStartPosition(const std::string& dmpName, const Ice::Current&)
{
    Ice::DoubleSeq startPositions;
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        startPositions = dmpPrx->getStartPosition();
    }
    else
    {
        ARMARX_ERROR << "getStartPosition: DMP not found.";
    }
    return startPositions;
}
 
double
DMPComponent::getForceTerm(const std::string& dmpName,
                           const Ice::DoubleSeq& canonicalValues,
                           int dim,
                           const Ice::Current&)
{
    double forceterm{0};
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        forceterm = dmpPrx->getForceTerm(canonicalValues, dim);
    }
    else
    {
        ARMARX_ERROR << "getDMPDim: DMP not found.";
    }
 
    return forceterm;
}
 
Vec2D
DMPComponent::calcTrajectory(const std::string& dmpName,
                             double startTime,
                             double timeStep,
                             double endTime,
                             const Ice::DoubleSeq& goal,
                             const cStateVec& states,
                             const Ice::DoubleSeq& canonicalValues,
                             double temporalFactor,
                             const Ice::Current&)
{
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        return dmpPrx->calcTrajectory(
            startTime, timeStep, endTime, goal, states, canonicalValues, temporalFactor);
    }
    else
    {
        ARMARX_ERROR << "getTemporalFactor: DMP not found.";
    }
    return {};
}
 
void
DMPComponent::resetTime(const Ice::Current&)
{
    ctime = 0;
    for (DMPMap::iterator it = dmpPool.begin(); it != dmpPool.end(); it++)
    {
        it->second.first->setCurrentTime(ctime);
    }
}
 
void
DMPComponent::resetCanonicalValues(const Ice::Current&)
{
    for (DMPMap::iterator it = dmpPool.begin(); it != dmpPool.end(); it++)
    {
        ::Ice::DoubleSeq canvals;
        DMPInstancePtr dmpPtr = it->second.first;
        if (dmpPtr->getDMPType() == "PeriodicDMP")
        {
            canvals.push_back(0.0);
            dmpPtr->setCanonicalValues(canvals);
        }
        else
        {
            canvals.push_back(1.0);
            dmpPtr->setCanonicalValues(canvals);
        }
    }
}
 
// Trajectory related
Ice::DoubleSeq
DMPComponent::getTrajGoal(const std::string& dmpName, const Ice::Current&)
{
    Ice::DoubleSeq trajGoal;
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        trajGoal = dmpPrx->getTrajGoal();
    }
    else
    {
        ARMARX_ERROR << "getTrajGoal: DMP not found.";
    }
 
    return trajGoal;
}
 
cStateVec
DMPComponent::getTrajStartState(const std::string& dmpName, const Ice::Current&)
{
    cStateVec trajStartState;
    DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
 
    if (dmpPrx)
    {
        trajStartState = dmpPrx->getTrajStartState();
    }
    else
    {
        ARMARX_ERROR << "getTrajStartState: DMP not found.";
    }
 
    return trajStartState;
}
 
// DMP manager
SVector
DMPComponent::getDMPNameList(const Ice::Current&)
{
    SVector nameList;
 
    for (DMPMap::iterator it = dmpPool.begin(); it != dmpPool.end(); it++)
    {
        nameList.push_back(it->first);
    }
 
    return nameList;
}
 
void
DMPComponent::eraseDMP(const std::string& dmpName, const Ice::Current&)
{
    DMPMap::iterator dmpPoolIt = dmpPool.find(dmpName);
 
    if (dmpPoolIt != dmpPool.end())
    {
        dmpPool.erase(dmpPoolIt);
    }
}
 
bool
DMPComponent::isDMPExist(const std::string& dmpName, const Ice::Current&)
{
    if (dmpPool.find(dmpName) == dmpPool.end())
    {
        return false;
    }
    else
    {
        return true;
    }
}
 
// local functions
DMPInstanceBasePrx
DMPComponent::createDMPInstancePrx(DMPInstancePtr dmpPtr)
{
    Ice::ObjectAdapterPtr adapter = getObjectAdapter();
    Ice::ObjectPrx obj = adapter->addWithUUID(dmpPtr);
 
    return DMPInstanceBasePrx::uncheckedCast(obj);
}
 
//PropertyDefinitionsPtr DMPComponent::createPropertyDefinitions()
//{
//    return PropertyDefinitionsPtr(new DMPComponentPropertyDefinitions(
//                                      getConfigIdentifier()));
//}