DMPInstance.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    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 "DMPInstance.h"
#include <dmp/io/MMMConverter.h>
#include <Ice/ObjectAdapter.h>
 
using namespace armarx;
 
// tool functions
DMP::Vec<DMP::DMPState> getDMPStateFromcStateVec(const cStateVec& state)
{
    DMP::Vec<DMP::DMPState> res;
    res.resize(state.size());
 
    for (size_t i = 0; i < state.size(); i++)
    {
        res[i].pos = state[i].pos;
        res[i].vel = state[i].vel;
    }
 
    return res;
}
 
DMP::DVec2d getDVec2dStateFromcStateVec(const cStateVec& state)
{
    DMP::DVec2d res;
    res.resize(state.size());
    for (size_t i = 0; i < state.size(); i++)
    {
        res[i].push_back(state[i].pos);
        res[i].push_back(state[i].vel);
    }
 
    return res;
}
 
armarx::cStateVec getcStateVec(const DMP::Vec<DMP::DMPState>& dmpstate)
{
    armarx::cStateVec sv;
    sv.resize(dmpstate.size());
 
    for (size_t i = 0; i < dmpstate.size(); i++)
    {
        sv[i].pos = dmpstate[i].pos;
        sv[i].vel = dmpstate[i].vel;
    }
 
    return sv;
}
 
armarx::cStateVec getcStateVec(const DMP::DVec2d& dmpstate)
{
    armarx::cStateVec sv;
    sv.resize(dmpstate.size());
 
    for (size_t i = 0; i < dmpstate.size(); i++)
    {
        sv[i].pos = dmpstate[i].at(0);
        sv[i].vel = dmpstate[i].at(1);
    }
 
    return sv;
}
 
 
DMPInstance::DMPInstance(DMP::DMPInterfacePtr dmpInter, std::string DMPType)
{
    dmp = dmpInter;
    dmpType = DMPType;
    ctime = 0;
    timestep = 0.001;
}
 
// state calculation
 
cStateVec DMPInstance::getNextState(const cStateVec& states, const Ice::Current&)
{
    if (canonicalValues.size() == 0)
    {
        ARMARX_WARNING << "Canonical value is not specified. It will be set 1.0.";
        canonicalValues.push_back(1.0);
    }
 
    DMP::DVec2d currentDMPState = getDVec2dStateFromcStateVec(states);
    if (currentDMPState.size() == 0)
    {
        ARMARX_ERROR << "The current state is not available. Please specify current state with setDMPState().";
    }
 
    DMP::DVec goal = dmp->getGoals();
 
 
    ARMARX_INFO << "CanonicalValues: " << canonicalValues[0]
                << " ctime: " << ctime
                << " Timestep: " << timestep;
    double temporalFactor = dmp->getTemporalFactor();
    currentDMPState = dmp->calculateTrajectoryPointBase(ctime + timestep, goal, ctime,
                      currentDMPState, canonicalValues, temporalFactor);
 
    ctime += timestep;
    cStateVec nextState =  getcStateVec(currentDMPState);
    setDMPState(nextState);
    ARMARX_INFO << "Got next state";
 
    return nextState;
}
 
armarx::nStateValues DMPInstance::calcNextState(double t, const Ice::DoubleSeq& goal, double currentT, const Vec2D& currentStates, const Ice::DoubleSeq& canonicalValues, double temporalFactor, const ::Ice::Current&)
{
    if (canonicalValues.size() == 0)
    {
        ARMARX_ERROR << "Canonical value is not specified.";
        //        canonicalValues.push_back(1.0);
    }
 
    if (currentStates.size() == 0)
    {
        ARMARX_ERROR << "Current DMP State has to be specified";
    }
 
    DMP::DVec goal_v = DMP::DVec(goal);
 
    if (goal.size() == 0)
    {
        ARMARX_WARNING << "Using goal specified in configs";
        goal_v = getGoal();
 
    }
 
    DMP::DVec2d curStates;
 
    for (std::size_t i = 0; i < currentStates.size() ; ++i)
    {
        curStates.emplace_back(DMP::DVec(currentStates.at(i)));
    }
 
    DMP::DVec canValues = DMP::DVec(canonicalValues);
    ARMARX_INFO_S << VAROUT(temporalFactor);
    DMP::DVec2d tmp_result = dmp->calculateTrajectoryPointBase(t, goal_v, currentT, curStates, canValues, temporalFactor);
 
    Ice::DoubleSeq   tmpCanVal;
    for (std::size_t i = 0; i < canValues.size(); i++)
    {
        tmpCanVal.emplace_back(canValues.at(i));
    }
 
    nStateValues result;
 
    result.canonicalValues = tmpCanVal;
 
    Vec2D nState;
    for (std::size_t i = 0; i < tmp_result.size() ; ++i)
    {
        ::Ice::DoubleSeq dblseq = ::Ice::DoubleSeq(tmp_result.at(i));
 
        nState.emplace_back(dblseq);
    }
 
    result.nextState = nState;
 
    return result;
}
 
armarx::nStateValues DMPInstance::calcNextStateFromConfig(double t, double currentT, const Vec2D& currentStates, const Ice::DoubleSeq& canonicalValues, const Ice::Current&)
{
    DMP::DVec goal = getGoal();
    double temporalFactor = getTemporalFactor();
    return calcNextState(t, goal, currentT, currentStates, canonicalValues, temporalFactor);
}
 
 
 
// configuration & parameters
void DMPInstance::setConfigurationMap(const DMPConfigMap& conf, const Ice::Current&)
{
    std::map<std::string, paraType > configMap;
    for (::std::map< ::std::string, ::Ice::DoubleSeq>::const_iterator it = conf.begin(); it != conf.end(); ++it)
    {
        configMap[it->first] = (DMP::DVec) it->second;
        dmp->setConfiguration(it->first, it->second);
    }
 
    configs = configMap;
}
 
configMap DMPInstance::createConfigMap(DMP::Vec<std::string> paraIDs, DMP::Vec<paraType> paraVals)
{
    if (paraIDs.size() != paraVals.size())
    {
        ARMARX_WARNING << "ID list and value list have different sizes, which may cause error.";
    }
 
    configMap configs;
    for (size_t i = 0; i < paraIDs.size(); i++)
    {
        if (configs.find(paraIDs[i]) == configs.end())
        {
            configs.insert(configPair(paraIDs[i], paraVals[i]));
        }
        else
        {
            configs[paraIDs[i]] = paraVals[i];
        }
    }
 
    return configs;
}
 
void DMPInstance::setParameter(const std::string& paraID, const Ice::DoubleSeq& value, const ::Ice::Current&)
{
    if (configs.find(paraID) == configs.end())
    {
        configs.insert(configPair(paraID, value));
    }
    else
    {
        configs[paraID] = value;
    }
    dmp->setConfiguration(paraID, DMP::DVec(value));
}
 
// setter functions
 
void DMPInstance::setCanonicalValues(const Ice::DoubleSeq& value, const Ice::Current&)
{
    canonicalValues = value;
}
 
void DMPInstance::setAmpl(int dim, double value, const Ice::Current&)
{
    dmp->setAmpl(dim, value);
}
 
void DMPInstance::setTemporalFactor(double value, const Ice::Current&)
{
    dmp->setTemporalFactor(value);
}
 
void DMPInstance::setGoal(const Ice::DoubleSeq& value, const Ice::Current&)
{
    if (configs.find(DMP::Goal) == configs.end())
    {
        configs.insert(configPair(DMP::Goal, DMP::DVec(value)));
    }
    else
    {
        configs[DMP::Goal] = DMP::DVec(value);
    }
    dmp->setConfiguration(DMP::Goal, DMP::DVec(value));
}
 
void DMPInstance::setStartPosition(const Ice::DoubleSeq& value, const Ice::Current&)
{
    if (configs.find(DMP::StartPosition) == configs.end())
    {
        configs.insert(configPair(DMP::StartPosition, DMP::DVec(value)));
    }
    else
    {
        configs[DMP::StartPosition] = DMP::DVec(value);
    }
    dmp->setConfiguration(DMP::StartPosition, DMP::DVec(value));
}
 
void DMPInstance::setDMPState(const cStateVec& state, const Ice::Current&)
{
    dmpState = state;
}
 
 
// getter function
 
double DMPInstance::getAmpl(int dim, const Ice::Current&)
{
    return dmp->getAmpl(dim);
}
 
double DMPInstance::getTemporalFactor(const Ice::Current&)
{
    return dmp->getTemporalFactor();
}
 
double DMPInstance::getForceTerm(const Ice::DoubleSeq& canVal, int dim, const Ice::Current&)
{
    return dmp->_getPerturbationForce(dim, canVal[0]);
}
 
Ice::DoubleSeq DMPInstance::getGoal(const Ice::Current&)
{
    DMP::DVec goals = dmp->getGoals();
    Ice::DoubleSeq res;
 
    for (size_t i = 0; i < goals.size(); ++i)
    {
        res.push_back(goals.at(i));
    }
 
    return res;
}
 
// trajectory related
 
Ice::DoubleSeq DMPInstance::getTrajGoal(const ::Ice::Current&)
{
    Ice::DoubleSeq trajGoal;
    for (size_t i = 0; i < trajs[0].dim(); i++)
    {
        trajGoal.push_back(trajs[0].rbegin()->getPosition(i));
    }
 
    return trajGoal;
 
}
 
cStateVec DMPInstance::getTrajStartState(const ::Ice::Current&)
{
    cStateVec startState;
 
    for (size_t i = 0; i < trajs[0].dim(); i++)
    {
        cState state;
        state.pos = trajs[0].begin()->getPosition(i);
        state.vel = trajs[0].begin()->getDeriv(i, 1);
        state.acc = trajs[0].begin()->getDeriv(i, 2);
 
        startState.push_back(state);
    }
 
    return startState;
}
 
void DMPInstance::readTrajectoryFromFile(const std::string& file, double times, const Ice::Current&)
{
    std::string ext = file.rfind(".") == file.npos ? file : file.substr(file.rfind(".") + 1);
 
    if (ext == "xml")
    {
 
 
        ARMARX_INFO << "xml file loaded";
    }
    else if (ext == "csv")
    {
        DMP::SampledTrajectoryV2 traj;
        traj.readFromCSVFile(file);
        traj.gaussianFilter(0.01);
 
        double startTime = 0;
        double endTime = times;
        traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, startTime, endTime);
 
 
        trajs.push_back(traj);
        dmp->setDim(traj.dim());
 
        ARMARX_INFO << "csv file loaded";
 
    }
    else if (ext == "vsg")
    {
 
    }
    else
    {
        ARMARX_ERROR << "Error: The file is not valid ";
    }
}
 
// DMP Trainer
void DMPInstance::trainDMP(const Ice::Current&)
{
    dmp->learnFromTrajectories(trajs);
}
 
 
Vec2D DMPInstance::calcTrajectory(double startTime, double timeStep, double endTime,
                                  const ::Ice::DoubleSeq& goal,
                                  const cStateVec& states,
                                  const ::Ice::DoubleSeq& canonicalValues, double temporalFactor, const Ice::Current&)
{
    setDMPState(states);
    setGoal(goal);
    setCanonicalValues(canonicalValues);
    setTemporalFactor(temporalFactor);
    setCurrentTime(startTime);
    setTimeStep(timeStep);
 
    Vec2D resultingTrajectory;
 
    cStateVec nextStates = states;
    while (ctime < endTime)
    {
        nextStates = getNextState(nextStates);
        ::Ice::DoubleSeq positions;
 
        positions.push_back(ctime);
        for (unsigned int d = 0; d < states.size(); d++)
        {
            positions.push_back(nextStates.at(d).pos);
        }
 
        resultingTrajectory.push_back(positions);
    }
 
    setCurrentTime(0);
 
    return resultingTrajectory;
 
}
 
 
Vec2D DMPInstance::calcTrajectoryV2(const Ice::DoubleSeq& timestamps,
                                    const Ice::DoubleSeq& goal,
                                    const Vec2D& initStates,
                                    const Ice::DoubleSeq& canonicalValues, double temporalFactor, const Ice::Current&)
{
    DMP::DVec timestampsLocal = timestamps;
 
    if (timestampsLocal.size() == 0)
    {
        ARMARX_ERROR << "No Timestamps specified";
    }
 
    DMP::DVec::const_iterator it = timestamps.begin();
    Vec2D previousState = initStates;
    Vec2D resultingTrajectory;
    //    std::map<double, DMP::DVec > resultingSystemStatesMap;
 
    //DMP::DVec canonicalValues = initialCanonicalValues;
    double prevTimestamp = *it;
 
    Ice::DoubleSeq canValues = canonicalValues;
 
    for (; it != timestamps.end(); it++)
    {
        nStateValues nState = calcNextState(*it, goal, prevTimestamp, previousState, canValues, temporalFactor);
        canValues = nState.canonicalValues;
        previousState = nState.nextState;
 
        prevTimestamp = *it;
        ::Ice::DoubleSeq positions;
        for (unsigned int d = 0; d < previousState.size(); d++)
        {
            positions.push_back(previousState.at(d)[0]);
        }
        resultingTrajectory.push_back(positions);
    }
 
    return resultingTrajectory;
}
 
Vec2D DMPInstance::calcTrajectoryFromConfig(const Ice::DoubleSeq& timestamps,
        const Vec2D& initStates,
        const Ice::DoubleSeq& canonicalValues, const Ice::Current&)
{
    DMP::DVec goal = getGoal();
    double temporalFactor = getTemporalFactor();
    return calcTrajectoryV2(timestamps, goal, initStates, canonicalValues, temporalFactor);
}