MMMPlayer.cpp

/*
 * 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    RobotComponents::ArmarXObjects::MMMPlayer
 * @author     Mirko Waechter ( mirko dot waechter at kit dot edu )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */

#include "MMMPlayer.h"
#include <MMM/Motion/MotionReaderXML.h>

#include <ArmarXCore/core/exceptions/local/ExpressionException.h>

#include <ArmarXCore/core/system/ArmarXDataPath.h>

#include <ArmarXCore/observers/variant/Variant.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>

using namespace armarx;
using namespace MMM;


void MMMPlayer::onInitComponent()
{
    usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
    usingProxy("DebugObserver");

    motionFPS = 1.0f;
    desiredFPS = 1.0f;
    frameOffset = 0;
    paused = true;

    usePIDController = getProperty<bool>("UsePIDController").getValue();
    loopPlayback = getProperty<bool>("LoopPlayback").getValue();
    desiredFPS = getProperty<float>("FPS").getValue();
    maxVel = getProperty<float>("absMaximumVelocity").getValue();


    usingTopic(getProperty<std::string>("KinematicTopicName").getValue());
}


void MMMPlayer::onConnectComponent()
{
    kinematicUnit = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());

    debugObserver = getProxy<DebugObserverInterfacePrx>("DebugObserver");
    std::string armarxProjects = getProperty<std::string>("ArmarXProjects").getValue();

    if (!armarxProjects.empty())
    {
        std::vector<std::string> projects;
        boost::split(projects, armarxProjects, boost::is_any_of(",;"), boost::token_compress_on);

        for (std::string & p : projects)
        {
            //            ARMARX_INFO << "Adding to datapaths of " << p;
            armarx::CMakePackageFinder finder(p);

            if (!finder.packageFound())
            {
                ARMARX_WARNING << "ArmarX Package " << p << " has not been found!";
            }
            else
            {
                ARMARX_INFO << "Adding " << p << " to datapaths: " << finder.getDataDir();
                armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
            }
        }
    }

    std::string motionDefault = getProperty<std::string>("MMMFile").getValue();

    if (!motionDefault.empty())
    {
        load(motionDefault, "Armar4");
    }
}

bool MMMPlayer::start()
{
    ScopedRecursiveLock lock(mmmMutex);

    if (!motion)
    {
        return false;
    }

    motionStartTime = IceUtil::Time::now();
    currentFrame = 0;
    frameOffset = 0;
    direction = 1;

    try
    {
        task = new PeriodicTask<MMMPlayer>(this, &MMMPlayer::run, 10, false, "MMMPlayerTask");
        task->start();
        motionStartTime = IceUtil::Time::now();
        paused = false;
        return task->isRunning();
    }
    catch (...)
    {
        ARMARX_WARNING << "Failed to start MMMPLayer task";
    }

    return false;
}

bool MMMPlayer::startMMMPlayer(const Ice::Current&)
{
    ScopedRecursiveLock lock(mmmMutex);
    bool started = MMMPlayer::start();

    if (started)
    {
        ARMARX_INFO << "started MMMPlayer from GUI";
    }

    return started;
}

bool MMMPlayer::pauseMMMPlayer(const Ice::Current&)
{
    ScopedRecursiveLock lock(mmmMutex);

    if (!motion)
    {
        return false;
    }

    if (!paused)
    {
        paused = true;
        kinematicUnit->setJointVelocities(nullVelocities);

        if (direction < 0)
        {
            frameOffset = motion->getNumFrames() - currentFrame;
        }
        else
        {
            frameOffset = currentFrame;
        }

        ARMARX_INFO << "MMMPlayer paused from GUI at frame " << currentFrame;
    }
    else
    {
        motionStartTime = IceUtil::Time::now();
        paused = false;
        ARMARX_INFO << "MMMPlayer resumed from GUI";
    }

    return paused;
}

bool MMMPlayer::stopMMMPlayer(const Ice::Current&)
{
    ScopedRecursiveLock lock(mmmMutex);

    paused = true;

    try
    {
        if (task)
        {
            task->stop();
        }

        if (task->isRunning())
        {
            ARMARX_WARNING << "Failed to stop MMMPlayer";
        }
        else
        {
            ARMARX_INFO << "stopped MMMPlayer task from GUI";
        }

        kinematicUnit->setJointVelocities(nullVelocities);

        return !(task->isRunning());
    }
    catch (...)
    {
        ARMARX_WARNING << "Failed to stop MMMPlayer";
    }

    return false;
}


void MMMPlayer::load(const std::string& MMMFile, const std::string& projects)
{
    ScopedRecursiveLock lock(mmmMutex);
    currentFrame = 0;
    direction = 1;

    //    start = 0.3;
    //    end = 1.26;

    //    jointOffets["Right Arm Shoulder2"] = 20.0/180.0*M_PI;
    //    jointOffets["Left Arm Shoulder2"] = 20.0/180.0*M_PI;

    if (!projects.empty())
    {
        std::vector<std::string> proj;
        boost::split(proj, projects, boost::is_any_of(",;"), boost::token_compress_on);

        for (std::string & p : proj)
        {
            ARMARX_INFO << "Adding to datapaths of " << p;
            armarx::CMakePackageFinder finder(p);

            if (!finder.packageFound())
            {
                ARMARX_WARNING << "ArmarX Package " << p << " has not been found!";
            }
            else
            {
                ARMARX_INFO << "Adding to datapaths: " << finder.getDataDir();
                armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
            }
        }
    }

    MotionReaderXML reader;

    ArmarXDataPath::getAbsolutePath(MMMFile, motionPath);
    ARMARX_VERBOSE << "Set motionPath to " + motionPath ;
    motion = reader.loadMotion(motionPath);

    if (!motion)
    {
        terminate();
        return;
    }

    if (motion->getNumFrames() > 1)
    {
        float frameDuration = motion->getMotionFrame(1)->timestep - motion->getMotionFrame(0)->timestep;
        ARMARX_CHECK_EXPRESSION(frameDuration != 0);
        ARMARX_CHECK_EXPRESSION(frameDuration > 0)
        motionFPS = 1.0f / frameDuration;
        ARMARX_VERBOSE << VAROUT(motionFPS);
    }

    jointNames = motion->getJointNames();

    /* Debug: Clamp to joint limits
        unsigned int left_hip1_index = 0;
        unsigned int left_hip2_index = 0;
        unsigned int left_hip3_index = 0;
        unsigned int left_knee_index = 0;*/
    unsigned int left_ankle1_index = 0;
    unsigned int left_ankle2_index = 0;

    /*unsigned int right_hip1_index = 0;
    unsigned int right_hip2_index = 0;
    unsigned int right_hip3_index = 0;
    unsigned int right_knee_index = 0;*/
    unsigned int right_ankle1_index = 0;
    unsigned int right_ankle2_index = 0;



    for (unsigned int i = 0; i < jointNames.size(); i++)
    {
        /*if (jointNames[i] == "LegL_Hip1_joint")
        {
            left_hip1_index = i;
        }

        if (jointNames[i] == "LegL_Hip2_joint")
        {
            left_hip2_index = i;
        }

        if (jointNames[i] == "LegL_Hip3_joint")
        {
            left_hip3_index = i;
        }

        if (jointNames[i] == "LegL_Knee_joint")
        {
            left_knee_index = i;
        }
        */
        if (jointNames[i] == "LegL_Ank1_joint")
        {
            left_ankle1_index = i;
        }

        if (jointNames[i] == "LegL_Ank2_joint")
        {
            left_ankle2_index = i;
        }

        /*
                if (jointNames[i] == "LegR_Hip1_joint")
                {
                    right_hip1_index = i;
                }

                if (jointNames[i] == "LegR_Hip2_joint")
                {
                    right_hip2_index = i;
                }

                if (jointNames[i] == "LegR_Hip3_joint")
                {
                    right_hip3_index = i;
                }

                if (jointNames[i] == "LegR_Knee_joint")
                {
                    right_knee_index = i;
                }
        */
        if (jointNames[i] == "LegR_Ank1_joint")
        {
            right_ankle1_index = i;
        }

        if (jointNames[i] == "LegR_Ank2_joint")
        {
            right_ankle2_index = i;
        }



    }

    for (unsigned int i = 0; i < motion->getNumFrames(); i++)
    {
        //ARMARX_INFO << "Trajectory clamped";
        /*motion->getMotionFrame(i)->joint[left_hip1_index] = std::max(-0.44f, motion->getMotionFrame(i)->joint[left_hip1_index]);
        motion->getMotionFrame(i)->joint[left_hip1_index] = std::min(1.53f, motion->getMotionFrame(i)->joint[left_hip1_index]);
        motion->getMotionFrame(i)->joint[left_hip2_index] = std::max(-0.13f, motion->getMotionFrame(i)->joint[left_hip2_index]);
        motion->getMotionFrame(i)->joint[left_hip2_index] = std::min(0.60f, motion->getMotionFrame(i)->joint[left_hip2_index]);
        motion->getMotionFrame(i)->joint[left_hip3_index] = std::max(-0.60f, motion->getMotionFrame(i)->joint[left_hip3_index]);
        motion->getMotionFrame(i)->joint[left_hip3_index] = std::min(0.39f, motion->getMotionFrame(i)->joint[left_hip3_index]);
        motion->getMotionFrame(i)->joint[left_knee_index] = std::max(0.1f, motion->getMotionFrame(i)->joint[left_knee_index]);
        motion->getMotionFrame(i)->joint[left_knee_index] = std::min(1.61f, motion->getMotionFrame(i)->joint[left_knee_index]);*/
        motion->getMotionFrame(i)->joint[left_ankle1_index] = std::max(-0.45f, motion->getMotionFrame(i)->joint[left_ankle1_index]);
        motion->getMotionFrame(i)->joint[left_ankle1_index] = std::min(0.45f, motion->getMotionFrame(i)->joint[left_ankle1_index]);
        motion->getMotionFrame(i)->joint[left_ankle2_index] = std::max(-0.20f, motion->getMotionFrame(i)->joint[left_ankle2_index]);
        motion->getMotionFrame(i)->joint[left_ankle2_index] = std::min(0.20f, motion->getMotionFrame(i)->joint[left_ankle2_index]);


        /* motion->getMotionFrame(i)->joint[right_hip1_index] = std::max(-0.47f, motion->getMotionFrame(i)->joint[right_hip1_index]);
         motion->getMotionFrame(i)->joint[right_hip1_index] = std::min(1.56f, motion->getMotionFrame(i)->joint[right_hip1_index]);
         motion->getMotionFrame(i)->joint[right_hip2_index] = std::max(-0.16f, motion->getMotionFrame(i)->joint[right_hip2_index]);
         motion->getMotionFrame(i)->joint[right_hip2_index] = std::min(0.63f, motion->getMotionFrame(i)->joint[right_hip2_index]);
         motion->getMotionFrame(i)->joint[right_hip3_index] = std::max(-0.63f, motion->getMotionFrame(i)->joint[right_hip3_index]);
         motion->getMotionFrame(i)->joint[right_hip3_index] = std::min(0.39f, motion->getMotionFrame(i)->joint[right_hip3_index]);
         motion->getMotionFrame(i)->joint[right_knee_index] = std::max(0.1f, motion->getMotionFrame(i)->joint[right_knee_index]);
         motion->getMotionFrame(i)->joint[right_knee_index] = std::min(1.61f, motion->getMotionFrame(i)->joint[right_knee_index]);*/
        motion->getMotionFrame(i)->joint[right_ankle1_index] = std::max(-0.45f, motion->getMotionFrame(i)->joint[right_ankle1_index]);
        motion->getMotionFrame(i)->joint[right_ankle1_index] = std::min(0.45f, motion->getMotionFrame(i)->joint[right_ankle1_index]);
        motion->getMotionFrame(i)->joint[right_ankle2_index] = std::max(-0.20f, motion->getMotionFrame(i)->joint[right_ankle2_index]);
        motion->getMotionFrame(i)->joint[right_ankle2_index] = std::min(0.20f, motion->getMotionFrame(i)->joint[right_ankle2_index]);

    }

    NameControlModeMap modes;

    for (size_t i = 0; i < jointNames.size(); ++i)
    {
        const auto& jointName = jointNames.at(i);

        if (usePIDController)
        {
            modes[jointName] = eVelocityControl;
        }
        else
        {
            modes[jointName] = ePositionControl;
        }

        nullVelocities[jointName] = 0.0;
        jointNamesUsed[jointName] = true;
    }

    kinematicUnit->switchControlMode(modes);
    kinematicUnit->setJointVelocities(nullVelocities);
}

bool MMMPlayer::loadTrajectory(const std::string& MMMFile, const std::string& projects, const Ice::Current&)
{
    ARMARX_VERBOSE << "loaded trajectory " << MMMFile;
    load(MMMFile, projects);

    if (!motion)
    {
        return false;
    }
    else
    {
        return true;
    }
}

int MMMPlayer::getNumberOfFrames(const Ice::Current&)
{

    if (!motion)
    {
        return 0;
    }

    return motion->getNumFrames();
}

int MMMPlayer::getCurrentFrame(const Ice::Current&)
{
    return currentFrame;
}

bool MMMPlayer::setLoopPlayback(bool loop, const Ice::Current&)
{

    loopPlayback = loop;
    return loopPlayback;
}

std::string MMMPlayer::getMotionPath(const Ice::Current&)
{
    return motionPath;
}

StringList MMMPlayer::getJointNames(const Ice::Current&)
{
    return jointNames;
}

int MMMPlayer::setUsePID(bool usePID, const Ice::Current&)
{

    ScopedRecursiveLock lock(mmmMutex);

    usePIDController = usePID;
    NameControlModeMap modes;

    for (size_t i = 0; i < jointNames.size(); ++i)
    {
        const auto& jointName = jointNames.at(i);

        if (usePIDController)
        {
            modes[jointName] = eVelocityControl;
        }
        else
        {
            modes[jointName] = ePositionControl;
        }
    }

    try
    {
        kinematicUnit->switchControlMode(modes);
    }
    catch (...) { }

    return usePIDController;
}

void MMMPlayer::setFPS(float FPS, const Ice::Current&)
{
    desiredFPS = FPS;
    ARMARX_VERBOSE << "set FPS to " << FPS;
    return;
}

bool MMMPlayer::setJointsInUse(const std::string& jointName, bool inUse, const Ice::Current&)
{
    jointNamesUsed[jointName] = inUse;
    return jointNamesUsed[jointName];
}

void MMMPlayer::setCurrentFrame(int frameNumber, const Ice::Current&)
{

    ScopedRecursiveLock lock(mmmMutex);

    if (!motion)
    {
        return;
    }

    currentFrame = frameNumber;

    if (direction < 0)
    {
        frameOffset = motion->getNumFrames() - frameNumber;
    }
    else
    {
        frameOffset = frameNumber;
    }

    auto frame = motion->getMotionFrame(frameNumber);

    targetPositionValues.clear();
    StringVariantBaseMap debugTargetValues;
    ARMARX_CHECK_EXPRESSION((int)jointNames.size() == frame->joint.rows());

    for (size_t i = 0; i < jointNames.size(); ++i)
    {
        const auto& jointName = jointNames.at(i);

        if (jointNamesUsed[jointName])
        {
            auto& targetPosValue = targetPositionValues[jointName] = frame->joint[i];
            auto it = jointOffets.find(jointName);

            if (it != jointOffets.end())
            {
                targetPosValue += it->second;
            }

            assert(targetPosValue == targetPositionValues[jointName]);
            debugTargetValues[jointName] = new Variant(targetPosValue);
        }
    }

    debugObserver->setDebugChannel("targetJointAngles", debugTargetValues);
    kinematicUnit->setJointVelocities(nullVelocities);
    kinematicUnit->setJointAngles(targetPositionValues);

    ARMARX_INFO << "MMMPlayer: Current frame set to " << frameNumber;
}

void MMMPlayer::run()
{
    NameValueMap targetVelocities;

    if (!motion)
    {
        return;
    }

    if (paused)
    {
        sleep(1);
        //        ARMARX_INFO << deatargetVel  = std::min<double>(maxVel / 180.0 * M_PI, targetVel);
        //deactivateSpam(3) << " pausing MMMPlayer at frame " << currentFrame;
        motionStartTime = IceUtil::Time::now();
    }
    else
    {
        currentMotionTime = ((IceUtil::Time::now() - motionStartTime).toSecondsDouble() * 100 * desiredFPS / motionFPS) + frameOffset;

        if (direction > 0)
        {
            currentFrame = currentMotionTime;
        }
        else
        {
            currentFrame = motion->getNumFrames() - (int)currentMotionTime;
        }

        if (currentFrame >= (int)motion->getNumFrames() || currentFrame < 0)
        {
            ARMARX_VERBOSE << "current frame " << currentFrame;

            kinematicUnit->setJointVelocities(nullVelocities);

            if (loopPlayback)
            {
                direction *= -1;
                ARMARX_VERBOSE << "direction " << direction;
                motionStartTime = IceUtil::Time::now();
                frameOffset = 1;
                return;
            }
            else if (task)
            {
                task->stop();
                sleep(1);
                //                ARMARX_VERBOSE << "after stop task running:" << task->isRunning();
                return;
            }
        }

        double interpolationValue = currentMotionTime - (int)currentMotionTime;
        auto frame = motion->getMotionFrame(currentFrame);
        MMM::MotionFramePtr nextFrame;

        if (currentFrame + 1 < (int)motion->getNumFrames())
        {
            nextFrame = motion->getMotionFrame(currentFrame + 1);
        }
        NameValueMap tmpTargetValues;
        StringVariantBaseMap debugTargetValues;
        {
            ScopedRecursiveLock lock(mmmMutex);
            targetPositionValues.clear();
            ARMARX_INFO << deactivateSpam(1) << "frame: " << currentFrame << " " << VAROUT(interpolationValue);
            ARMARX_CHECK_EXPRESSION((int)jointNames.size() == frame->joint.rows());


            for (size_t i = 0; i < jointNames.size(); ++i)
            {
                const auto& jointName = jointNames.at(i);

                if (jointNamesUsed[jointName])
                {
                    auto& targetPosValue = targetPositionValues[jointName] = frame->joint[i];

                    /*if (nextFrame)
                    {
                        targetPosValue += interpolationValue * (nextFrame->joint[i] - frame->joint[i]);
                    }*/

                    auto it = jointOffets.find(jointName);

                    if (it != jointOffets.end())
                    {
                        targetPosValue += it->second;
                    }

                    //                    assert(targetPosValue == targetPositionValues[jointName]);
                    debugTargetValues[jointName] = new Variant(targetPosValue);
                    float& targetVel = targetVelocities[jointName] =  0.0;
                    auto vel = frame->joint_vel[i];

                    if (vel != 0.0f)
                    {
                        targetVel = vel;
                    }
                    else if (nextFrame)
                    {
                        if (nextFrame->timestep - frame->timestep == 0)
                        {
                            ARMARX_INFO << deactivateSpam() << "timestep delta is zero at frame " << currentFrame << " - cannot calculate velocity";
                        }

                        targetVel = targetVelocities[jointName] = (nextFrame->joint[i] - frame->joint[i]) / (nextFrame->timestep - frame->timestep);
                    }
                    targetVel *= 0.4;
                    MMM::MotionFramePtr nextNextFrame;

                    if (currentFrame + 2 < (int)motion->getNumFrames())
                    {
                        nextNextFrame = motion->getMotionFrame(currentFrame + 2);
                    }

                    targetVel *= desiredFPS / motionFPS * direction;

                    //            debugObserver->setDebugDatafield(jointName, "p", new Variant(pid->second->previousError * pid->second->Kp));
                    //            debugObserver->setDebugDatafield(jointName, "i", new Variant(pid->second->integral* pid->second->Ki));
                    //            debugObserver->setDebugDatafield(jointName, "d", new Variant(pid->second->derivative * pid->second->Kd));
                    //            debugObserver->setDebugDatafield(jointName, "velocity", new Variant(targetVelocities[jointName]));
                    //}

                    //        targetVelocities[jointName] =  0.0;

                    auto pid = PIDs.find(jointName);

                    if (usePIDController)
                    {
                        if (pid != PIDs.end())
                        {
                            auto cv = pid->second->getControlValue();
                            targetVel += cv;

                            ARMARX_INFO << deactivateSpam(0.5, jointName) << jointName << " ControlValue   " << cv;


                            if (cv > 20)
                            {
                                ARMARX_INFO << "" << (jointName) << ": v:" << (targetVel) << " pos: " << targetPosValue;
                            }

                            //                    debugObserver->setDebugDatafield(jointName, "p", new Variant(pid->second->previousError * pid->second->Kp));
                            //                    debugObserver->setDebugDatafield(jointName, "i", new Variant(pid->second->integral * pid->second->Ki));
                            //                    debugObserver->setDebugDatafield(jointName, "d", new Variant(pid->second->derivative * pid->second->Kd));
                            //                    debugObserver->setDebugDatafield(jointName, "velocity", new Variant(targetVelocities[jointName]));
                        }
                    }

                    targetVel  = std::min<double>(maxVel / 180.0 * M_PI, targetVel);
                    targetVel  = std::max<double>(-1 * maxVel / 180.0 * M_PI, targetVel);
                }
                else
                {
                    targetVelocities[jointName] = 0.0;
                }
            }
            tmpTargetValues = targetPositionValues;
        }

        {
            ScopedLock lock(jointAnglesMutex);
            updatePIDController(latestJointAngles);
        }

        debugObserver->setDebugChannel("targetJointAngles", debugTargetValues);
        kinematicUnit->setJointVelocities(targetVelocities);

        if (!usePIDController)
        {
            kinematicUnit->setJointAngles(tmpTargetValues);
        }

        ARMARX_VERBOSE << deactivateSpam(1) << "Playing frame " << currentFrame;
    }
}


void MMMPlayer::reportJointAngles(const NameValueMap& angles, bool, const Ice::Current&)
{
    ScopedLock lock(jointAnglesMutex);
    latestJointAngles = angles;
}

void MMMPlayer::updatePIDController(const NameValueMap& angles)
{
    if (!usePIDController)
    {
        ARMARX_INFO << deactivateSpam() << "jointangles reporting DISABLED";
        return;
    }

    for (const auto & joint : angles)
    {
        const std::string& name = joint.first;

        if (targetPositionValues.find(name) == targetPositionValues.end())
        {
            continue;
        }

        auto it = PIDs.find(name);

        if (it == PIDs.end())
        {
            PIDs[name] = PIDControllerPtr(new PIDController(getProperty<float>("Kp").getValue(),
                                          getProperty<float>("Ki").getValue(),
                                          getProperty<float>("Kd").getValue()));
            it = PIDs.find(name);
        }

        PIDControllerPtr pid = it->second;
        pid->update(joint.second, targetPositionValues[name]);
    }
}


void MMMPlayer::onDisconnectComponent()
{
    ScopedRecursiveLock lock(mmmMutex);

    if (task)
    {
        task->stop();
    }

    kinematicUnit->stop();
}


void MMMPlayer::onExitComponent()
{

}

PropertyDefinitionsPtr MMMPlayer::createPropertyDefinitions()
{
    return PropertyDefinitionsPtr(new MMMPlayerPropertyDefinitions(
                                      getConfigIdentifier()));
}