PlayPlatformTrajectory.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::PlatformGroup
 * @author     Stefan Reither ( stef dot reither at web dot de )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PlayPlatformTrajectory.h"
 
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
 
#include <RobotAPI/libraries/core/PIDController.h>
 
#include <RobotSkillTemplates/statecharts/PlatformGroup/PlatformContext.h>
 
using namespace armarx;
using namespace PlatformGroup;
 
#define STATE_POSITION 0
#define STATE_VELOCITY 1
 
#define MAX_TRANSLATION_ACCEL 0.5 // m/s²
#define MAX_ROTATION_ACCEL 0.75 // rad/s²
 
#define FACTOR_BETWEEN_TRANS_AND_ROT 500.0
 
// DO NOT EDIT NEXT LINE
PlayPlatformTrajectory::SubClassRegistry
    PlayPlatformTrajectory::Registry(PlayPlatformTrajectory::GetName(),
                                     &PlayPlatformTrajectory::CreateInstance);
 
TrajectoryPtr
balanceTimestamps(TrajectoryPtr t)
{
    ARMARX_INFO << t->output();
    double length = t->getLength(0);
    ARMARX_INFO << VAROUT(length);
    double timelength = t->getTimeLength();
    ARMARX_INFO << VAROUT(timelength);
    auto timestamps = t->getTimestamps();
    ARMARX_INFO << VAROUT(timestamps);
    Ice::DoubleSeq newTimestamps;
    newTimestamps.push_back(0);
    for (size_t var = 0; var < timestamps.size() - 1; ++var)
    {
        double tBefore = timestamps.at(var);
        double tAfter = (timestamps.at(var + 1));
        ARMARX_INFO << VAROUT(tBefore) << VAROUT(tAfter);
        double partLength = t->getLength(0, tBefore, tAfter);
        double lengthPortion = partLength / length;
        ARMARX_INFO << VAROUT(partLength) << VAROUT(lengthPortion);
        newTimestamps.push_back(*newTimestamps.rbegin() + timelength * lengthPortion);
    }
    ARMARX_INFO << VAROUT(newTimestamps);
    TrajectoryPtr newTraj = new Trajectory();
    for (size_t d = 0; d < t->dim(); ++d)
    {
        newTraj->addDimension(t->getDimensionData(d), newTimestamps, t->getDimensionName(d));
    }
    ARMARX_INFO << newTraj->output();
 
    return newTraj;
}
 
TrajectoryPtr
normalizeTimestamps(TrajectoryPtr t, float maxTransVel, float maxRotVel)
{
    double totalTime = 0.0;
    auto timestamps = t->getTimestamps();
    auto dimNames = t->getDimensionNames();
 
    for (size_t var = 0; var < timestamps.size() - 1; ++var)
    {
        double xLength =
            t->getLength(std::find(dimNames.begin(), dimNames.end(), "x") - dimNames.begin(),
                         0,
                         timestamps.at(var),
                         timestamps.at(var + 1)) /
            1000; // conversion mm to m
        ARMARX_INFO << VAROUT(xLength);
        double yLength =
            t->getLength(std::find(dimNames.begin(), dimNames.end(), "y") - dimNames.begin(),
                         0,
                         timestamps.at(var),
                         timestamps.at(var + 1)) /
            1000; // conversion mm to m
        ARMARX_INFO << VAROUT(yLength);
        double alphaLength =
            t->getLength(std::find(dimNames.begin(), dimNames.end(), "alpha") - dimNames.begin(),
                         0,
                         timestamps.at(var),
                         timestamps.at(var + 1));
        ARMARX_INFO << VAROUT(alphaLength);
 
        double xyLength = std::sqrt(fabs(xLength * xLength + yLength * yLength));
        ARMARX_INFO << VAROUT(xyLength);
 
        double sAcc = 0.5 * maxTransVel * maxTransVel / MAX_TRANSLATION_ACCEL;
 
        double xyTime = 0.0;
 
        if (xyLength <= 2 * sAcc)
        {
            xyTime = 2 * std::sqrt(xyLength / MAX_TRANSLATION_ACCEL);
        }
        else
        {
            xyTime = 2 * maxTransVel / MAX_TRANSLATION_ACCEL + (xyLength - 2 * sAcc) / maxTransVel;
        }
 
        ARMARX_INFO << VAROUT(xyTime);
        double rotTime =
            2 * maxRotVel / MAX_ROTATION_ACCEL +
            (alphaLength - 0.5 * maxRotVel * maxRotVel / MAX_ROTATION_ACCEL) / maxRotVel;
        ARMARX_INFO << VAROUT(rotTime);
 
        double dtime = (xyTime > rotTime) ? xyTime : rotTime;
        totalTime += dtime;
    }
 
    t = t->normalize(0.0, totalTime);
    ARMARX_INFO << t->output();
 
    return t;
}
 
TrajectoryPtr
scaleRotationLengths(TrajectoryPtr t, float factor)
{
    Ice::DoubleSeq factors = {1.0, 1.0, factor};
    ARMARX_INFO << VAROUT(factors);
    t->scaleValue(factors);
    return t;
}
 
void
PlayPlatformTrajectory::onEnter()
{
    TrajectoryPtr platformTraj = in.getTrajectory();
    auto dimNames = platformTraj->getDimensionNames();
    if (dimNames.at(0) != "x" || dimNames.at(1) != "y" || dimNames.at(2) != "alpha")
    {
        ARMARX_WARNING << "Trajectory misses at least one of the following needed dimensions: 'x', "
                          "'y' and 'alpha'.";
        emitFailure();
    }
}
 
void
PlayPlatformTrajectory::run()
{
    PlatformContext* c = getContext<PlatformContext>();
    PlatformUnitInterfacePrx platformUnit = c->platformUnitPrx;
    TrajectoryPtr platformTraj = in.getTrajectory();
    auto dimNames = platformTraj->getDimensionNames();
    float maxTransVel = in.getMaxTranslationVelocity();
    float maxRotVel = in.getMaxRotationVelocity();
 
    if (in.getBalanceTrajectory())
    {
        // Scaling rotation dimension of the trajectory to fit to the translation trajectory
 
        platformTraj = scaleRotationLengths(platformTraj, FACTOR_BETWEEN_TRANS_AND_ROT);
 
        // Balancing timestamps of the trajetory
 
        platformTraj = balanceTimestamps(platformTraj);
 
        // Rescaling rotation dimension of the trajectory
 
        platformTraj =
            scaleRotationLengths(platformTraj, (float)(1 / FACTOR_BETWEEN_TRANS_AND_ROT));
 
        // calculate approximation of duration of the movement given a maximal velocity and a maximal acceleration
 
        platformTraj = normalizeTimestamps(platformTraj, maxTransVel, maxRotVel);
    }
 
    // Executing trajectory
 
 
    MultiDimPIDController pidTrans(in.getPTranslation(),
                                   in.getITranslation(),
                                   in.getDTranslation(),
                                   maxTransVel,
                                   MAX_TRANSLATION_ACCEL * 1000);
    PIDController pidRot(in.getPOrientation(),
                         in.getIOrientation(),
                         in.getDOrientation(),
                         maxRotVel,
                         MAX_ROTATION_ACCEL);
    DatafieldRefPtr posXRef =
        new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "positionX");
    DatafieldRefPtr posYRef =
        new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "positionY");
    DatafieldRefPtr oriRef =
        new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "rotation");
 
    NameValueMap targetValues;
    IceUtil::Time startTime = armarx::TimeUtil::GetTime();
    double currentTime = 0.0;
    bool finalPoseReached = false;
    int cycleTime = in.getCycleTimeMS();
    CycleUtil cycle(cycleTime);
 
    c->debugDrawer->removeLayer("ActualRobotPath");
 
    //    Vector3Ptr positionOld;
 
    IceUtil::Time cycleStart;
    while (!isRunningTaskStopped()) // stop run function if returning true
    {
        cycleStart = armarx::TimeUtil::GetTime();
 
        currentTime = (armarx::TimeUtil::GetTime() - startTime).toSecondsDouble();
        if (currentTime > platformTraj->getTimeLength())
        {
            currentTime = platformTraj->getTimeLength();
            finalPoseReached = true;
        }
 
        float posx = posXRef->getDataField()->getFloat();
        float posy = posYRef->getDataField()->getFloat();
        float ori = oriRef->getDataField()->getFloat();
 
        //        Vector3Ptr v = new Vector3 {posx, posy, 50.f};
        //        if (positionOld)
        //        {
        //            c->debugDrawer->setLineVisu("ActualRobotPath", boost::lexical_cast<std::string>(currentTime), positionOld, v, 2.0, DrawColor {0, 1, 0, 1});
        //        }
        //        positionOld = v;
 
        std::vector<Ice::DoubleSeq> allStates = platformTraj->getAllStates(currentTime, 1);
        for (size_t i = 0; i < platformTraj->dim(); ++i)
        {
            targetValues[dimNames.at(i)] = allStates[i][STATE_POSITION];
        }
 
        Eigen::Vector3f pos{posx, posy, 0.0f};
        Eigen::Vector3f targetPos{targetValues["x"], targetValues["y"], 0.0f};
 
        pidTrans.update(pos, targetPos);
        pidRot.update(ori, targetValues["alpha"]);
        float newRotVel = pidRot.getControlValue();
        Eigen::Vector3f globalNewTransVel = {
            pidTrans.getControlValue()[0], pidTrans.getControlValue()[1], 0};
        Eigen::Matrix3f m;
        m = Eigen::AngleAxisf(ori, Eigen::Vector3f::UnitZ());
        Eigen::Vector3f localVel = m.inverse() * globalNewTransVel;
 
 
        ARMARX_INFO << "New values: x: " << localVel[0] << " , y= " << localVel[1]
                    << " , alpha= " << newRotVel;
 
        platformUnit->move(localVel[0], localVel[1], newRotVel);
 
        if (finalPoseReached && fabs(pidTrans.previousError) < in.getPositionalAccuracy() &&
            fabs(pidRot.previousError) < in.getOrientationalAccuracy())
        {
            disableRunFunction();
            platformUnit->move(0, 0, 0);
            ARMARX_INFO << "Success";
            emitSuccess();
        }
        else
        {
            cycle.waitForCycleDuration();
        }
    }
}
 
void
PlayPlatformTrajectory::onExit()
{
    // Setting velocities to zero
    PlatformContext* c = getContext<PlatformContext>();
    PlatformUnitInterfacePrx platformUnit = c->platformUnitPrx;
    platformUnit->move(0.0, 0.0, 0.0);
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
PlayPlatformTrajectory::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new PlayPlatformTrajectory(stateData));
}