GMRDynamics.cpp

Go to the documentation of this file.

/*
 * GMRDynamics.cpp
 *
 *  Created on: Nov 20, 2011
 *      Author: Seungsu KIM
 */
 
#include "GMRDynamics.h"
 
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
 
 
GMRDynamics::GMRDynamics(int nStates,
                         int nVar,
                         double delta_t,
                         const char* f_mu,
                         const char* f_sigma,
                         const char* f_prio)
{
    this->delta_t = delta_t;
    GMM = new Gaussians(nStates, nVar, f_mu, f_sigma, f_prio);
}
 
GMRDynamics::GMRDynamics(int nStates,
                         int nVar,
                         double delta_t,
                         const vector<double> pri_vec,
                         const vector<double> mu_vec,
                         const vector<double> sig_vec)
{
    this->delta_t = delta_t;
    GMM = new Gaussians(nStates, nVar, pri_vec, mu_vec, sig_vec);
}
 
 
void
GMRDynamics::initGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex)
{
    GMM->InitFastGMR(first_inindex, last_inindex, first_outindex, last_outindex);
 
    gDim = last_inindex - first_inindex + 1;
    if (gDim != static_cast<unsigned int>(last_outindex - first_outindex + 1))
    {
        cout << "dynamics dimension is not matching" << endl;
    }
 
    gXi.Resize(gDim);
    target.Resize(gDim);
 
    gXi.Zero();
    target.Zero();
}
 
void
GMRDynamics::setStateTarget(MathLib::Vector state, MathLib::Vector target)
{
    setTarget(target);
    setState(state);
}
 
void
GMRDynamics::setTarget(MathLib::Vector target, double target_t)
{
    this->target_t = target_t;
 
    //gXi += (this->target - target);
    this->target = target;
}
 
MathLib::Vector
GMRDynamics::getTarget(void)
{
    return target;
}
 
double
GMRDynamics::getTargetT(void)
{
    return target_t;
}
 
void
GMRDynamics::setState(MathLib::Vector state)
{
    gXi = state;
}
 
MathLib::Vector
GMRDynamics::getState(void)
{
    return gXi;
}
 
void
GMRDynamics::setCurrentTime(double current_t)
{
    this->current_t = current_t;
}
 
double
GMRDynamics::getCurrentTime(void)
{
    return current_t;
}
 
MathLib::Vector
GMRDynamics::getVelocity(MathLib::Vector x)
{
    return GMM->Regression(x);
}
 
 
MathLib::Vector
GMRDynamics::getNextState(void)
{
    return getNextState(1.0);
}
 
MathLib::Vector
GMRDynamics::getNextState(double lamda)
{
    // target time
    target_t -= (delta_t * lamda);
 
    gXi += (getVelocity(gXi - target) * (delta_t * lamda));
 
    return gXi;
}
 
double
GMRDynamics::getReachingTime(double lamda)
{
    unsigned int frame = 0;
    unsigned int li = 0;
    MathLib::Vector xi(3);
    xi.Set(gXi);
 
    for (frame = 0; frame < REACHING_ITERATION_MAX; frame++)
    {
        for (li = 0; li < INTEGRATION_L; li++)
        {
            xi += getVelocity(xi - target) * delta_t / (double)INTEGRATION_L * lamda;
 
            if ((xi - target).Norm() < GMR_ERROR_TOLERANCE)
            {
                return (double)(frame * INTEGRATION_L + li) * delta_t / (double)INTEGRATION_L;
            }
        }
    }
    return (double)(frame * INTEGRATION_L + li) * delta_t / (double)INTEGRATION_L;
}