FunctionApproximator.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    RobotComponents::ArmarXObjects::FunctionApproximator
 * @author     zhou ( you dot zhou at kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "FunctionApproximator.h"
 
 
using namespace armarx;
 
void
FunctionApproximator::onInitComponent()
{
    ARMARX_INFO << "initializing DMP component";
 
 
    ARMARX_INFO << "successfully initialized DMP component";
}
 
void
FunctionApproximator::onConnectComponent()
{
}
 
void
FunctionApproximator::onDisconnectComponent()
{
    ARMARX_INFO << "disconnecting FunctionApproximator component";
}
 
void
FunctionApproximator::onExitComponent()
{
    ARMARX_INFO << "exiting FunctionApproximator component";
}
 
void
FunctionApproximator::learn(const std::string& name,
                            const DVector2d& x,
                            const DVector2d& y,
                            const Ice::Current&)
{
 
    DMP::FunctionApproximationInterfacePtr approximator = funcAppPool.find(name)->second;
    if (approximator)
    {
        if (x.size() != y.size())
        {
            ARMARX_ERROR << "The factors dimension should be the same as the responds dimension.";
        }
 
 
        DMP::DVec2d xx;
        DMP::DVec2d yy;
 
        xx.resize(x.size());
        yy.resize(y.size());
 
        for (size_t i = 0; i < x.size(); ++i)
        {
            DMP::DVec cx;
            DMP::DVec cy;
 
            for (size_t j = 0; j < x[i].size(); ++j)
            {
                cx.push_back(x[i][j]);
                cy.push_back(y[i][j]);
            }
 
            xx[i] = cx;
            yy[i] = cy;
        }
 
        approximator->learn(xx, yy);
    }
    else
    {
        ARMARX_ERROR << "predict: function approximator not found.";
        return;
    }
}
 
Ice::Double
FunctionApproximator::predict(const std::string& name, const Ice::DoubleSeq& x, const Ice::Current&)
{
    DMP::FunctionApproximationInterfacePtr approximator = funcAppPool.find(name)->second;
    if (approximator)
    {
        DMP::DVec xx;
 
        for (size_t i = 0; i < x.size(); ++i)
        {
            xx.push_back(x.at(i));
        }
 
        double res = (*approximator)(xx).at(0);
 
        return res;
    }
    else
    {
        ARMARX_ERROR << "predict: function approximator not found.";
        return 0;
    }
}
 
void
FunctionApproximator::ilearn(const std::string& name,
                             const Ice::DoubleSeq& x,
                             Ice::Double y,
                             const Ice::Current&)
{
    DMP::FunctionApproximationInterfacePtr approximator = funcAppPool.find(name)->second;
    if (approximator)
    {
        DMP::DVec factors;
 
        for (size_t i = 0; i < x.size(); ++i)
        {
            factors.push_back(x.at(i));
        }
 
        approximator->ilearn(factors, y);
    }
    else
    {
        ARMARX_ERROR << "ilearn: function approximator not found.";
        return;
    }
}
 
void
FunctionApproximator::blearn(const std::string& name,
                             const Ice::DoubleSeq& x,
                             const Ice::DoubleSeq& y,
                             const Ice::Current&)
{
    DMP::FunctionApproximationInterfacePtr approximator = funcAppPool.find(name)->second;
    if (approximator)
    {
        DMP::DVec2d factors;
        DMP::DVec2d responds;
 
        factors.push_back(DMP::DVec(x));
        responds.push_back(DMP::DVec(y));
 
        approximator->learn(factors, responds);
    }
    else
    {
        ARMARX_ERROR << "ilearn: function approximator not found.";
        return;
    }
}
 
Ice::DoubleSeq
FunctionApproximator::bpredict(const std::string& name,
                               const Ice::DoubleSeq& x,
                               const Ice::Current&)
{
    DMP::FunctionApproximationInterfacePtr approximator = funcAppPool.find(name)->second;
    if (approximator)
    {
        Ice::DoubleSeq responds;
 
        for (size_t i = 0; i < x.size(); ++i)
        {
            responds.push_back((*approximator)(x.at(i)).at(0));
        }
 
        return responds;
    }
    else
    {
        ARMARX_ERROR << "ilearn: function approximator not found.";
        return std::vector<double>();
    }
}
 
void
FunctionApproximator::reset(const Ice::Current&)
{
 
    for (FuncAppMap::iterator it = funcAppPool.begin(); it != funcAppPool.end(); ++it)
    {
        it->second.reset(new DMP::RBFInterpolator<DMP::GaussRadialBasisFunction>(2));
    }
}
 
//armarx::PropertyDefinitionsPtr FunctionApproximator::createPropertyDefinitions()
//{
//    return armarx::PropertyDefinitionsPtr(new FunctionApproximatorPropertyDefinitions(
//                                      getConfigIdentifier()));
//}
 
 
void
armarx::FunctionApproximator::initialize(const std::string& fappName,
                                         const Ice::DoubleSeq& /*widthFactors*/,
                                         const Ice::Current&)
{
    //int dim = widthFactors.size();
 
    DMP::FunctionApproximationInterfacePtr approximator(new DMP::LWR<DMP::GaussKernel>(100));
 
    funcAppPool.insert(FuncAppPair(fappName, approximator));
}
 
void
armarx::FunctionApproximator::getFunctionApproximatorFromFile(const std::string& funcName,
                                                              const std::string& name,
                                                              const Ice::Current&)
{
    std::ifstream file(name);
 
    DMP::FunctionApproximationInterface* readPtr;
 
    boost::archive::xml_iarchive ar(file);
    ar >> boost::serialization::make_nvp("approximator", readPtr);
 
    file.close();
 
    DMP::FunctionApproximationInterfacePtr approximator(readPtr);
    funcAppPool.insert(FuncAppPair(funcName, approximator));
}
 
void
armarx::FunctionApproximator::getFunctionApproximatorsFromFile(
    const std::vector<std::string>& funcName,
    const std::string& filename,
    const Ice::Current&)
{
    std::ifstream file(filename);
 
    std::vector<DMP::FunctionApproximationInterface*> readPtr;
 
    boost::archive::xml_iarchive ar(file);
    ar >> boost::serialization::make_nvp("approximator", readPtr);
 
    file.close();
 
    for (size_t i = 0; i < readPtr.size(); i++)
    {
 
        DMP::FunctionApproximationInterfacePtr approximator(readPtr[i]);
        funcAppPool.insert(FuncAppPair(funcName[i], approximator));
    }
}
 
void
FunctionApproximator::saveFunctionApproximatorInFile(const std::string& funcName,
                                                     const std::string& name,
                                                     const Ice::Current&)
{
 
    std::cout << "save function approximator in file ... " << std::endl;
    std::ofstream file(name);
 
    boost::archive::xml_oarchive ar(file);
 
    DMP::FunctionApproximationInterface* savedPtr = funcAppPool[funcName].get();
 
    ar << boost::serialization::make_nvp("approximator", savedPtr);
 
    file.close();
}
 
void
armarx::FunctionApproximator::initializeTest(const std::string& fappName,
                                             const Ice::DoubleSeq& widthFactors)
{
    int dim = widthFactors.size();
 
    DMP::FunctionApproximationInterfacePtr approximator(
        new DMP::RBFInterpolator<DMP::GaussRadialBasisFunction>(dim, widthFactors));
 
    funcAppPool.insert(FuncAppPair(fappName, approximator));
}
 
void
FunctionApproximator::saveFunctionApproximatorInFileTest(const std::string& funcName,
                                                         const std::string& name)
{
 
    std::cout << "save function approximator in file ... " << std::endl;
    std::ofstream file(name);
    DMP::FunctionApproximationInterfacePtr approximator = funcAppPool[funcName];
 
    boost::archive::text_oarchive ar(file);
    DMP::FunctionApproximationInterface* savedPtr = approximator.get();
    ar << boost::serialization::make_nvp("approximator", savedPtr);
 
    DMP::FunctionApproximationInterface* appPtr = approximator.get();
    std::stringstream str;
    {
        boost::archive::xml_oarchive ar1(str);
        ar1 << boost::serialization::make_nvp("approximator", appPtr);
    }
    std::cout << str.str() << std::endl;
 
    boost::archive::xml_iarchive ari(str);
    DMP::FunctionApproximationInterface* newPtr;
    ari >> boost::serialization::make_nvp("approximator", newPtr);
 
 
    file.close();
}