ForceTrajectory.cpp

Go to the documentation of this file.

#include <robodyn_controllers/ForceTrajectory.h>

ForceTrajectory::ForceTrajectory()
    : lastTime(0.)
    , logCategory("gov.nasa.controllers.ForceTrajectory")
{
}

void ForceTrajectory::getPose(double time, KDL::JntArrayAcc& pose)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "entered ForceTrajectory::getPose " << time;

    if (time == 0.)
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "initializing..";
        pose = jointsLast = jointsInit;
        lastTime = 0.;
        return;
    }

    if (time < lastTime)
    {
        std::stringstream err;
        err << "ForceTrajectory time moved backwards" << std::endl;
        NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());

        return;
    }

    if (time >= double(trajectory->getDuration()))
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "end of trajectory";
        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();
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "node " << nodeNames[i] << " before: "
                << nodeFrames[i].p.x() << ", " << nodeFrames[i].p.y() << ", " << nodeFrames[i].p.z() << ", "
                << nodeFrames[i].M.GetRot().x() << ", " << nodeFrames[i].M.GetRot().y() << ", " << nodeFrames[i].M.GetRot().z();
    }

    std::map<std::string, KDL::Frame> controlledFrames;
    std::vector<std::string> jointNames;
    treeIk->getJointNames(jointNames);

    treeIk->getFrames(jointsLast.q, controlledFrames);

    for (unsigned int i = 0; i < nodeNames.size(); ++i)
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG <<  "getFrames " << nodeNames[i] << ": "
                << controlledFrames[nodeNames[i]].p.x() << ", " << controlledFrames[nodeNames[i]].p.y() << ", " << controlledFrames[nodeNames[i]].p.z() << ", "
                << controlledFrames[nodeNames[i]].M.GetRot().x() << ", " << controlledFrames[nodeNames[i]].M.GetRot().y() << ", " << controlledFrames[nodeNames[i]].M.GetRot().z();
    }

    double dt = time - lastTime;

    forceController->updateFrames(nodeNames, controlledFrames, nodeFrames, dt);

    for (unsigned int i = 0; i < nodeFrames.size(); ++i)
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "node " << nodeNames[i] << " after: "
                << nodeFrames[i].p.x() << ", " << nodeFrames[i].p.y() << ", " << nodeFrames[i].p.z() << ", "
                << nodeFrames[i].M.GetRot().x() << ", " << nodeFrames[i].M.GetRot().y() << ", " << nodeFrames[i].M.GetRot().z();
    }

    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;
}

void ForceTrajectory::printFrame(std::string name, KDL::Frame frame)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << name << ": "
        << frame.p.x() << ", " << frame.p.y() << ", " << frame.p.z() << ", "
        << frame.M.GetRot().x() << ", " << frame.M.GetRot().y() << ", " << frame.M.GetRot().z();

}