PlayMMMFileState.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    RobotSkillTemplates::PlayMMMFile
 * @author     Philipp Schmidt ( ufedv at student dot kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PlayMMMFileState.h"
 
#include <unordered_set>
 
using namespace armarx;
using namespace PlayMMMFile;
 
// DO NOT EDIT NEXT LINE
PlayMMMFileState::SubClassRegistry PlayMMMFileState::Registry(PlayMMMFileState::GetName(),
                                                              &PlayMMMFileState::CreateInstance);
 
#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
#include <RobotAPI/libraries/core/Trajectory.h>
 
void
PlayMMMFileState::onEnter()
{
    TrajectoryPtr dummyTraj = new Trajectory();
    // put your user code for the enter-point here
    // execution time should be short (<100ms)
    ARMARX_LOG << "onEnter()";
 
    //Load blacklist
    std::unordered_set<std::string> jointsBlacklist;
    if (in.isblacklistSet())
    {
        std::string s = in.getblacklist();
        std::string delimiter = ",";
        size_t pos = 0;
        std::string token;
        while ((pos = s.find(delimiter)) != std::string::npos)
        {
            token = s.substr(0, pos);
            jointsBlacklist.insert(token);
            s.erase(0, pos + delimiter.length());
        }
        jointsBlacklist.insert(s);
    }
 
    //Load motion
    this->getMmmPlayer()->loadMMMFile(in.getmmmFile(), "");
    if (this->getMmmPlayer()->isMotionLoaded())
    {
        this->getTrajectoryPlayer()->setIsVelocityControl(true);
        TrajectoryPtr trajectory = TrajectoryPtr::dynamicCast(this->getMmmPlayer()->getJointTraj());
        float start = in.isStartTimeSet() ? in.getStartTime() : trajectory->begin()->timestamp;
        float end = in.isEndTimeSet() ? in.getEndTime() : trajectory->rbegin()->timestamp;
        ARMARX_INFO << "StartTime of Traj: " << trajectory->begin()->timestamp;
        ARMARX_INFO << "EndTime of Traj: " << trajectory->rbegin()->timestamp;
        ARMARX_INFO << "names: " << trajectory->getDimensionNames();
        TrajectoryPtr trajPart = trajectory->getPart(start, end, 0);
        ARMARX_INFO << "StartTime of Traj: " << trajPart->begin()->timestamp;
        ARMARX_INFO << "EndTime of Traj: " << trajPart->rbegin()->timestamp;
        ARMARX_INFO << " amplitude:  " << trajPart->getAmplitude(0, 0, start, end);
        ARMARX_INFO << "names: " << trajPart->getDimensionNames();
        this->getTrajectoryPlayer()->loadJointTraj(trajPart);
        this->getTrajectoryPlayer()->loadBasePoseTraj(this->getMmmPlayer()->getBasePoseTraj());
 
        //Delete joint names that are not available on robot
        auto map = trajPart->getStatesMap<float>(trajPart->begin()->timestamp);
        for (auto it = map.begin(); it != map.end();)
        {
            if (!this->getKinematicUnitObserver()->existsDataField("jointangles", it->first))
            {
                map.erase(it++);
            }
            else
            {
                ++it;
            }
        }
 
        Ice::StringSeq jointNames = this->getMmmPlayer()->getJointNames();
        for (size_t i = 0; i < jointNames.size(); ++i)
        {
            //Set joints in use
            bool enabled = !jointsBlacklist.count(jointNames.at(i));
            if (enabled)
            {
                ARMARX_LOG << jointNames.at(i) << " enabled";
                this->getTrajectoryPlayer()->setJointsInUse(jointNames.at(i), true);
            }
            else
            {
                ARMARX_LOG << jointNames.at(i) << " disabled";
                this->getTrajectoryPlayer()->setJointsInUse(jointNames.at(i), false);
                map.erase(jointNames.at(i));
            }
        }
 
        //Set speed
        this->getTrajectoryPlayer()->setEndTime(this->getTrajectoryPlayer()->getTrajEndTime() /
                                                in.getspeed());
 
        //Pass joint map
        local.setinitialJointValues(map);
    }
    else
    {
        cancelSubstates();
        this->emitFailure();
        return;
    }
}
 
void
PlayMMMFileState::run()
{
    //    // put your user code for the execution-phase here
    //    // runs in seperate thread, thus can do complex operations
    //    // should check constantly whether isRunningTaskStopped() returns true
    //    while (!isRunningTaskStopped()) // stop run function if returning true
    //    {
 
    //    }
}
 
//void PlayMMMFileState::onBreak()
//{
//    // put your user code for the breaking point here
//    // execution time should be short (<100ms)
//}
 
void
PlayMMMFileState::onExit()
{
    // put your user code for the exit point here
    // execution time should be short (<100ms)
 
    ARMARX_LOG << "onExit()";
    this->getTrajectoryPlayer()->stopTrajectoryPlayer();
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
PlayMMMFileState::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new PlayMMMFileState(stateData));
}