TreeIkTrajectory.cpp

Go to the documentation of this file.

#include <robodyn_controllers/TreeIkTrajectory.h>
#include "nasa_common_logging/Logger.h"

TreeIkTrajectory::TreeIkTrajectory()
    : lastTime(0.)
{

}

void TreeIkTrajectory::getPose(double time, KDL::JntArrayAcc& pose)
{
    if (time == 0.)
    {
        pose = jointsLast = jointsInit;
        lastTime = 0.;
        return;
    }

    if (time < lastTime)
    {
        std::stringstream err;
        err << "TreeIkTrajectory time moved backwards" << std::endl;
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TreeIkTrajectory", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
        return;
    }

    if (time == lastTime)
    {
        pose = jointsLast;
        return;
    }

    // get pos
    trajectory->getPose(time, nodeFramesAcc);
    nodeFrames.resize(nodeFramesAcc.size());

    for (unsigned int i = 0; i < nodeFramesAcc.size(); ++i)
    {
        nodeFrames[i] = nodeFramesAcc[i].GetFrame();
    }

    pose.resize(jointsInit.q.rows());
    treeIk->getJointPositions(jointsLast.q, nodeNames, nodeFrames, pose.q, nodePriorities);

    // get vel & acc
    for (unsigned int i = 0; i < pose.q.rows(); ++i)
    {
        double tstep = time - lastTime;

        if (tstep < treeIk->getTimeStep() / 2.)
        {
            tstep = treeIk->getTimeStep() / 2.;
        }

        pose.qdot(i)    = (pose.q(i) - jointsLast.q(i)) / tstep;
        pose.qdotdot(i) = (pose.qdot(i) - jointsLast.qdot(i)) / tstep;
    }

    lastTime   = time;
    jointsLast = pose;
}