TrajectoryPlayer.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    RobotComponents::ArmarXObjects::TrajectoryPlayer
 * @author     zhou ( derekyou dot zhou at gmail dot com )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "TrajectoryPlayer.h"
 
#include <SimoxUtility/math/convert.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
 
using namespace armarx;
 
#define STATE_POSITION 0
#define STATE_VELOCITY 1
#define STATE_ACCELERATION 2
 
bool
TrajectoryPlayer::startTrajectoryPlayer(const Ice::Current&)
{
    direction = 1;
    timeOffset = 0;
 
 
    if (isPreview)
    {
        ARMARX_INFO << "robot file name : " << kinematicUnit->getRobotFilename();
        debugDrawer->setRobotVisu("Preview",
                                  "previewRobot",
                                  kinematicUnit->getRobotFilename(),
                                  armarxProject,
                                  armarx::CollisionModel);
        debugDrawer->updateRobotColor("Preview", "previewRobot", DrawColor{0, 1, 0, 0.5});
    }
 
 
    try
    {
        task = new PeriodicTask<TrajectoryPlayer>(
            this, &TrajectoryPlayer::run, 10, false, "TrajectoryPlayerTask", false);
        paused = false;
        firstRound = true;
        currentTime = 0;
        runningTime = 0;
        startTime = armarx::TimeUtil::GetTime();
 
        task->start();
        return task->isRunning();
    }
    catch (...)
    {
        ARMARX_WARNING << "Failed to start MMMPLayer task";
        return false;
    }
}
 
bool
TrajectoryPlayer::pauseTrajectoryPlayer(const Ice::Current&)
{
    std::unique_lock lock(motionMutex);
 
    if (!paused)
    {
        paused = true;
        timeOffset = runningTime;
        firstRound = true;
        kinematicUnit->setJointVelocities(nullVelocities);
    }
    else
    {
        paused = false;
    }
 
    return paused;
}
 
bool
TrajectoryPlayer::stopTrajectoryPlayer(const Ice::Current&)
{
    std::unique_lock lock(motionMutex);
    paused = true;
 
 
    if (isPreview)
    {
        debugDrawer->clearLayer("Preview");
    }
 
    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
TrajectoryPlayer::updateTargetValues()
{
    StringVariantBaseMap debugTargetValues;
    StringVariantBaseMap debugVelocityValues;
 
    {
        std::unique_lock lock(motionMutex);
        targetPositionValues.clear();
        targetVelocityValues.clear();
 
        int maxDerivative = 1;
        //    ARMARX_INFO << deactivateSpam(1) << VAROUT(currentTime);
        std::vector<Ice::DoubleSeq> states = jointTraj->getAllStates(currentTime, maxDerivative);
 
        for (size_t i = 0; i < jointNames.size(); ++i)
        {
            const auto& jointName = jointNames.at(i);
 
            if (jointNamesUsed[jointName])
            {
                // update targetPositionValues
                auto& targetPosValue = targetPositionValues[jointName] = states[i][STATE_POSITION];
                auto it = jointOffets.find(jointName);
                if (it != jointOffets.end())
                {
                    targetPosValue += it->second;
                }
                assert(targetPosValue == targetPositionValues[jointName]);
                debugTargetValues[jointName] = new Variant(targetPosValue);
 
                // update targetVelocityValues
 
                //                    float& targetVel = targetVelocityValues[jointName] = 0;
                //                    if (frozenTime != currentTime)
                //                    {
                //                        targetVel = (states[i][STATE_POSITION] - lastStates[i][STATE_POSITION]) / (currentTime - frozenTime);
                //                    }
 
                //            ARMARX_INFO << "jointName: " << jointName << " targetVel: " << targetVel;
                float& targetVel = targetVelocityValues[jointName] = states[i][STATE_VELOCITY];
                if (isVelocityControl)
                {
 
                    auto pid = PIDs.find(jointName);
 
                    if (pid != PIDs.end())
                    {
 
                        auto cv = pid->second->getControlValue();
                        //ARMARX_INFO << "*" << (jointName) << ": targetPosValue: " << targetPosValue << ", targetVel:" << (targetVel) << ", pid-cv:" << cv << ", result vel:" << (cv + targetVel);
                        targetVel += cv;
                    }
                }
                targetVel = std::min<double>(maxVel / 180.0 * M_PI, targetVel);
                targetVel = std::max<double>(-1 * maxVel / 180.0 * M_PI, targetVel);
                debugVelocityValues[jointName] = new Variant(targetVel);
            }
            else
            {
                targetVelocityValues[jointName] = 0;
            }
        }
 
 
        if (robotPoseUnitEnabled)
        {
            std::vector<Ice::DoubleSeq> pose = basePoseTraj->getAllStates(currentTime, 0);
 
            Eigen::Vector3f position(pose[0][0], pose[1][0], pose[2][0]);
            Eigen::Quaternionf orientation(pose[3][0], pose[4][0], pose[5][0], pose[6][0]);
            Eigen::Matrix4f p = offset * simox::math::pos_quat_to_mat4f(position, orientation);
            if (customRootNode)
            {
                localModel->setJointValues(targetPositionValues);
                localModel->setGlobalPoseForRobotNode(customRootNode, p);
                targetRobotPose = new Pose(localModel->getGlobalPose());
            }
            else
            {
                targetRobotPose = new Pose(p);
            }
        }
    }
 
    debugObserver->setDebugChannel("targetJointAngles", debugTargetValues);
    debugObserver->setDebugChannel("targetVelocity", debugVelocityValues);
}
 
bool
TrajectoryPlayer::resetTrajectoryPlayer(bool moveToFrameZeroPose, const Ice::Current&)
{
    std::unique_lock lock(motionMutex);
    currentTime = 0;
    runningTime = 0;
    if (moveToFrameZeroPose)
    {
        this->setIsVelocityControl(false);
        updateTargetValues();
        kinematicUnit->setJointAngles(targetPositionValues);
    }
    return true;
}
 
bool
TrajectoryPlayer::setJointsInUse(const std::string& jointName, bool inUse, const Ice::Current&)
{
    jointNamesUsed[jointName] = inUse;
    return jointNamesUsed[jointName];
}
 
void
TrajectoryPlayer::loadJointTraj(const TrajectoryBasePtr& trajs, const Ice::Current&)
{
    std::unique_lock lock(motionMutex);
    // get model filename
    if (!armarxProject.empty())
    {
        std::vector<std::string> proj = armarx::Split(armarxProject, ",;", true, true);
 
        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());
            }
        }
    }
 
    ArmarXDataPath::getAbsolutePath(kinematicUnit->getRobotFilename(), modelFileName);
    localModel = VirtualRobot::RobotIO::loadRobot(
        modelFileName, VirtualRobot::RobotIO::RobotDescription::eStructure);
    if (getProperty<std::string>("CustomRootNode").isSet() &&
        !getProperty<std::string>("CustomRootNode").getValue().empty())
    {
        customRootNode =
            localModel->getRobotNode(getProperty<std::string>("CustomRootNode").getValue());
    }
    else
    {
        customRootNode.reset();
    }
 
 
    // load trajectory
    jointTraj = TrajectoryPtr::dynamicCast(trajs);
 
 
    if (!jointTraj)
    {
        ARMARX_ERROR << "Error when loading TrajectoryPlayer: cannot load jointTraj !!!";
        return;
    }
 
    endTime = *jointTraj->getTimestamps().rbegin() - *jointTraj->getTimestamps().begin();
 
    jointTraj = jointTraj->normalize(0, endTime);
    trajEndTime = endTime;
    jointNames = jointTraj->getDimensionNames();
    ARMARX_INFO << VAROUT(jointNames);
 
    if (jointNames.size() != jointTraj->dim())
    {
        ARMARX_ERROR << "Not all trajectory dimensions are named !!! (would cause problems when "
                        "using kinematicUnit)";
        return;
    }
 
    NameControlModeMap modes;
    LimitlessStateSeq limitlessStates;
 
    for (size_t i = 0; i < jointNames.size(); ++i)
    {
        const auto& jointName = jointNames.at(i);
 
        if (isVelocityControl)
        {
            modes[jointName] = eVelocityControl;
        }
        else
        {
            modes[jointName] = ePositionControl;
        }
 
        nullVelocities[jointName] = 0.0;
        jointNamesUsed[jointName] = true;
 
        if (localModel)
        {
            LimitlessState ls;
            ls.enabled = false;
            VirtualRobot::RobotNodePtr rn = localModel->getRobotNode(jointName);
            if (rn)
            {
                ls.enabled = rn->isLimitless();
                ls.limitLo = rn->getJointLimitLo();
                ls.limitHi = rn->getJointLimitHi();
                limitlessMap[jointName] = rn->isLimitless();
            }
            ARMARX_INFO << "limitless status - " << jointName << ": " << rn->isLimitless();
            limitlessStates.push_back(ls);
        }
    }
 
    // setup limitless status of joints
    if (limitlessStates.size() == jointNames.size())
    {
        ARMARX_INFO << "SETTING UP LIMITLESS JOINTS";
        jointTraj->setLimitless(limitlessStates);
    }
    /*else
    {
        ARMARX_IMPORTANT << "NOT SETTING LIMITLESS JOINTS!";
    }*/
 
    ARMARX_INFO << "Setting null velocities: " << nullVelocities;
    kinematicUnit->switchControlMode(modes);
    kinematicUnit->setJointVelocities(nullVelocities);
}
 
void
TrajectoryPlayer::loadBasePoseTraj(const TrajectoryBasePtr& trajs, const Ice::Current&)
{
    basePoseTraj = TrajectoryPtr::dynamicCast(trajs);
}
 
void
TrajectoryPlayer::setLoopPlayback(bool loop, const Ice::Current&)
{
    loopPlayback = loop;
}
 
void
TrajectoryPlayer::setIsVelocityControl(bool isVelocity, const Ice::Current&)
{
    //    std::unique_lock lock(motionMutex);
 
    isVelocityControl = isVelocity;
    NameControlModeMap modes;
 
    for (size_t i = 0; i < jointNames.size(); ++i)
    {
        const auto& jointName = jointNames.at(i);
 
        if (isVelocity)
        {
            modes[jointName] = eVelocityControl;
        }
        else
        {
            modes[jointName] = ePositionControl;
        }
    }
 
    try
    {
        kinematicUnit->switchControlMode(modes);
    }
    catch (...)
    {
    }
}
 
void
TrajectoryPlayer::reportJointAngles(const NameValueMap& angles,
                                    Ice::Long timestamp,
                                    bool,
                                    const Ice::Current&)
{
    std::unique_lock lock(jointAnglesMutex);
    latestJointAngles = angles;
}
 
void
TrajectoryPlayer::onInitComponent()
{
    offeringTopic("DebugObserver");
    usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
 
    paused = true;
 
    isVelocityControl = getProperty<bool>("isVelocityControl").getValue();
    loopPlayback = getProperty<bool>("LoopPlayback").getValue();
    maxVel = getProperty<float>("absMaximumVelocity").getValue();
 
    usingTopic(getProperty<std::string>("KinematicTopicName").getValue());
 
    offeringTopic("DebugDrawerUpdates");
}
 
void
TrajectoryPlayer::onConnectComponent()
{
    kinematicUnit = getProxy<KinematicUnitInterfacePrx>(
        getProperty<std::string>("KinematicUnitName").getValue());
    debugObserver = getTopic<DebugObserverInterfacePrx>("DebugObserver");
    debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
    armarxProject = getProperty<std::string>("ArmarXProjects").getValue();
    robotPoseUnitEnabled = getProperty<bool>("EnableRobotPoseUnit").getValue();
 
    Eigen::Vector3f position(getProperty<float>("Offset.x").getValue(),
                             getProperty<float>("Offset.y").getValue(),
                             getProperty<float>("Offset.z").getValue());
    Eigen::Vector3f orientation(getProperty<float>("Offset.roll").getValue(),
                                getProperty<float>("Offset.pitch").getValue(),
                                getProperty<float>("Offset.yaw").getValue());
    offset = simox::math::pos_rpy_to_mat4f(position, orientation);
    ARMARX_INFO << "Trajectory Player Offset " << offset;
 
    targetRobotPose = new Pose();
}
 
void
TrajectoryPlayer::onDisconnectComponent()
{
}
 
void
TrajectoryPlayer::onExitComponent()
{
}
 
armarx::PropertyDefinitionsPtr
TrajectoryPlayer::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new TrajectoryPlayerPropertyDefinitions(getConfigIdentifier()));
}
 
void
TrajectoryPlayer::run()
{
    if (!jointTraj)
    {
        ARMARX_WARNING << "joint trajectory not found ...";
        return;
    }
 
    if (paused)
    {
        sleep(1);
        startTime = armarx::TimeUtil::GetTime();
    }
    else
    {
        runningTime = (armarx::TimeUtil::GetTime() - startTime).toSecondsDouble() + timeOffset;
 
        if (direction > 0)
        {
            currentTime = runningTime;
        }
        else
        {
            currentTime = endTime - runningTime;
        }
 
        //        if (!firstRound)
        //        {
        //            startCal = armarx::TimeUtil::GetTime();
        //        }
 
        ARMARX_INFO << deactivateSpam(1) << "currentTime: " << currentTime;
        ARMARX_INFO << deactivateSpam(1) << "endTime: " << endTime;
 
        if (runningTime >= endTime) // reset timer.
        {
            runningTime = 0;
            startTime = armarx::TimeUtil::GetTime();
            timeOffset = 0;
        }
 
 
        if (currentTime >= endTime)
        {
            kinematicUnit->setJointVelocities(nullVelocities);
 
            if (loopPlayback)
            {
                direction = -1;
            }
            else if (task)
            {
                task->stop();
                sleep(1);
                return;
            }
        }
 
        if (currentTime <= 0)
        {
            if (loopPlayback)
            {
                direction = 1;
            }
            else if (task)
            {
                task->stop();
                sleep(1);
                return;
            }
        }
 
        StringVariantBaseMap debugTargetValues;
        StringVariantBaseMap debugVelocityValues;
 
        {
            std::unique_lock lock(motionMutex);
            ARMARX_CHECK_EXPRESSION((size_t)jointNames.size() == jointTraj->dim());
            //            updateTargetValues();
            targetPositionValues.clear();
            targetVelocityValues.clear();
 
            int maxDerivative = 1;
            //    ARMARX_INFO << deactivateSpam(1) << VAROUT(currentTime);
            std::vector<Ice::DoubleSeq> states =
                jointTraj->getAllStates(currentTime, maxDerivative);
 
 
            for (size_t i = 0; i < jointNames.size(); ++i)
            {
                const auto& jointName = jointNames.at(i);
 
                if (jointNamesUsed[jointName])
                {
                    // update targetPositionValues
                    auto& targetPosValue = targetPositionValues[jointName] =
                        states[i][STATE_POSITION];
                    auto it = jointOffets.find(jointName);
                    if (it != jointOffets.end())
                    {
                        targetPosValue += it->second;
                    }
                    assert(targetPosValue == targetPositionValues[jointName]);
                    debugTargetValues[jointName] = new Variant(targetPosValue);
 
 
                    // update targetVelocityValues
 
                    //                    float& targetVel = targetVelocityValues[jointName] = 0;
                    //                    if (frozenTime != currentTime)
                    //                    {
                    //                        targetVel = (states[i][STATE_POSITION] - lastStates[i][STATE_POSITION]) / (currentTime - frozenTime);
                    //                    }
 
                    //            ARMARX_INFO << "jointName: " << jointName << " targetVel: " << targetVel;
                    float& targetVel = targetVelocityValues[jointName] = states[i][STATE_VELOCITY];
 
                    if (isVelocityControl)
                    {
 
                        auto pid = PIDs.find(jointName);
 
                        if (pid != PIDs.end())
                        {
                            auto cv = pid->second->getControlValue();
                            /*if (cv > 20)
                            {
                                ARMARX_INFO << "" << (jointName) << ": targetPosValue: " << targetPosValue << ", targetVel:" << (targetVel) << ", pid-cv:" << cv << ", result vel:" << (cv + targetVel);
                            }
                            if (fabs(cv) > 0.2)
                            {
                                ARMARX_IMPORTANT << "|cv| > 1: " << (jointName) << ": targetPosValue: " << targetPosValue << ", actualPosValue: " << latestJointAngles[jointName] << ", targetVel:" << (targetVel) << ", pid-cv:" << cv << ", result vel:" << (cv + targetVel);
                            }
                            if (fabs(targetVel) > 0.2)
                            {
                                ARMARX_IMPORTANT << "|targetVel| > 1: " << (jointName) << ": targetPosValue: " << targetPosValue << ", actualPosValue: " << latestJointAngles[jointName] << ", targetVel:" << (targetVel) << ", pid-cv:" << cv << ", result vel:" << (cv + targetVel);
                            }*/
                            targetVel += cv;
                        }
                    }
                    targetVel = std::min<double>(maxVel / 180.0 * M_PI, targetVel);
                    targetVel = std::max<double>(-1 * maxVel / 180.0 * M_PI, targetVel);
                    targetVel *= direction;
                    debugVelocityValues[jointName] = new Variant(targetVel);
                }
                else
                {
                    targetVelocityValues[jointName] = 0;
                }
            }
 
 
            if (robotPoseUnitEnabled)
            {
                std::vector<Ice::DoubleSeq> pose = basePoseTraj->getAllStates(currentTime, 0);
 
                Eigen::Vector3f position(pose[0][0], pose[1][0], pose[2][0]);
                Eigen::Quaternionf orientation(pose[3][0], pose[4][0], pose[5][0], pose[6][0]);
                Eigen::Matrix4f p = offset * simox::math::pos_quat_to_mat4f(position, orientation);
                if (customRootNode)
                {
                    localModel->setJointValues(targetPositionValues);
                    localModel->setGlobalPoseForRobotNode(customRootNode, p);
                    targetRobotPose = new Pose(localModel->getGlobalPose());
                }
                else
                {
                    targetRobotPose = new Pose(p);
                }
            }
        }
 
        /*{
            ARMARX_INFO << "ELBOW - target pos:" << targetPositionValues["ArmL6_Elb2"] << ", actual pos:" << latestJointAngles["ArmL6_Elb2"] << ",\t targetVel:" << targetVelocityValues["ArmL6_Elb2"] << std::endl;
        }*/
 
 
        if (!isPreview)
        {
            if (!isVelocityControl)
            {
                kinematicUnit->setJointAngles(targetPositionValues);
            }
            else
            {
                kinematicUnit->setJointVelocities(targetVelocityValues);
            }
 
            if (robotPoseUnitEnabled)
            {
                RobotPoseUnitInterfacePrx robotPoseUnitPrx = getProxy<RobotPoseUnitInterfacePrx>(
                    getProperty<std::string>("RobotPoseUnitName").getValue());
 
                robotPoseUnitPrx->moveTo(PoseBasePtr::dynamicCast(targetRobotPose), 0.001f, 0.001f);
            }
        }
        else
        {
            ARMARX_INFO << "Preview ... ";
 
            if (checkJointsLimit())
            {
                debugDrawer->updateRobotConfig("Preview", "previewRobot", targetPositionValues);
 
                if (robotPoseUnitEnabled)
                {
                    debugDrawer->updateRobotPose(
                        "Preview", "previewRobot", PoseBasePtr::dynamicCast(targetRobotPose));
                }
            }
 
            else
            {
                task->stop();
                sleep(1);
                return;
            }
        }
 
        {
            std::unique_lock lock(jointAnglesMutex);
            updatePIDController(latestJointAngles);
        }
 
        debugObserver->setDebugChannel("targetJointAngles", debugTargetValues);
        debugObserver->setDebugChannel("targetVelocity", debugVelocityValues);
    }
}
 
void
TrajectoryPlayer::updatePIDController(const NameValueMap& angles)
{
    if (!isVelocityControl)
    {
        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(),
                                                            std::numeric_limits<double>::max(),
                                                            std::numeric_limits<double>::max(),
                                                            limitlessMap[name]));
            ARMARX_INFO << "Creating PID for " << name << " is limitless:" << limitlessMap[name];
            it = PIDs.find(name);
        }
 
        PIDControllerPtr pid = it->second;
        pid->update(joint.second, targetPositionValues[name]);
        //ARMARX_INFO << "PID update:" << name << ", measured:"  << joint.second << ", target:" << targetPositionValues[name];
    }
}
 
bool
TrajectoryPlayer::checkJointsLimit()
{
 
    if (!localModel)
    {
        ARMARX_WARNING << "No local model found !!! (No joints limit checked)";
        return false;
    }
 
    std::unique_lock lock(jointAnglesMutex);
 
 
    for (NameValueMap::iterator it = latestJointAngles.begin(); it != latestJointAngles.end(); it++)
    {
        std::string jointName = it->first;
        float jointValue = it->second;
 
        float lowLimit = localModel->getRobotNode(jointName)->getJointLimitLow();
        float highLimit = localModel->getRobotNode(jointName)->getJointLimitHigh();
        DrawColor errColor = {1, 0, 0, 1};
        DrawColor warnColor = {1, 1, 0, 1};
        DrawColor normColor = {0, 1, 0, 1};
        if (jointValue < lowLimit || jointValue > highLimit)
        {
            debugDrawer->updateRobotNodeColor("Preview", "previewRobot", jointName, errColor);
            return false;
        }
 
        float dist = highLimit - lowLimit;
 
        bool isWarning = ((jointValue < (lowLimit + 0.1 * dist)) && (jointValue >= lowLimit)) ||
                         ((jointValue > (highLimit - 0.1 * dist)) && (jointValue <= highLimit));
 
        if (isWarning)
        {
            debugDrawer->updateRobotNodeColor("Preview", "previewRobot", jointName, warnColor);
        }
        else
        {
            debugDrawer->updateRobotNodeColor("Preview", "previewRobot", jointName, normColor);
        }
    }
 
    return true;
}
 
bool
TrajectoryPlayer::checkSelfCollision()
{
    if (!localModel)
    {
        ARMARX_WARNING << "No local model found !!! (No joints limit checked)";
        return false;
    }
 
    std::unique_lock lock(jointAnglesMutex);
 
    localModel->setJointValues(latestJointAngles);
 
    return true;
}