ControlPlatform.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2014-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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     Mirko Waechter ( waechter at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ControlPlatform.h"
 
#include <ArmarXCore/core/time/TimeUtil.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;
 
// DO NOT EDIT NEXT LINE
ControlPlatform::SubClassRegistry ControlPlatform::Registry(ControlPlatform::GetName(),
                                                            &ControlPlatform::CreateInstance);
 
float
signedMin(float newValue, float minAbsValue)
{
    return std::copysign(std::min<float>(fabs(newValue), minAbsValue), newValue);
}
 
ControlPlatform::ControlPlatform(const XMLStateConstructorParams& stateData) :
    XMLStateTemplate<ControlPlatform>(stateData),
    ControlPlatformGeneratedBase<ControlPlatform>(stateData)
{
}
 
void
ControlPlatform::onEnter()
{
 
    // put your user code for the enter-point here
    // execution time should be short (<100ms)
    PlatformContext* context = getContext<PlatformContext>();
 
    setTimeoutEvent(in.getTimeout(), createEventTimeout());
 
    if (in.getUsePlatformNativeController())
    {
        ARMARX_INFO << "Using native Platform Controller";
        Vector3Ptr currentTarget = in.getTargetPose();
        Literal checkX(context->platformUnitObserverName + ".platformPose.positionX",
                       checks::approx,
                       {currentTarget->x, in.getPositionalAccuracy()});
        Literal checkY(context->platformUnitObserverName + ".platformPose.positionY",
                       checks::approx,
                       {currentTarget->y, in.getPositionalAccuracy()});
 
        Literal checkAngle(context->platformUnitObserverName + ".platformPose.rotation",
                           checks::approx,
                           {currentTarget->z, in.getOrientationalAccuracy()});
        // for special case of orientations around 2*PI
        float checkAngleValueOffset;
 
        if (currentTarget->z > M_PI)
        {
            checkAngleValueOffset = -2 * M_PI;
        }
        else
        {
            checkAngleValueOffset = 2 * M_PI;
        }
 
        Literal checkAngle2(
            context->platformUnitObserverName + ".platformPose.rotation",
            checks::approx,
            {(float)(currentTarget->z + checkAngleValueOffset), in.getOrientationalAccuracy()});
 
        installConditionForPoseReached(checkX && checkY && (checkAngle || checkAngle2));
        ARMARX_INFO << "Moving to " << in.getTargetPose()->x << ", " << in.getTargetPose()->y
                    << ", " << in.getTargetPose()->z << ", ";
        context->platformUnitPrx->moveTo(in.getTargetPose()->x,
                                         in.getTargetPose()->y,
                                         in.getTargetPose()->z,
                                         in.getPositionalAccuracy(),
                                         in.getOrientationalAccuracy());
        usleep(1000000);
        context->platformUnitPrx->moveTo(in.getTargetPose()->x,
                                         in.getTargetPose()->y,
                                         in.getTargetPose()->z,
                                         in.getPositionalAccuracy(),
                                         in.getOrientationalAccuracy());
 
        this->setUseRunFunction(false);
    }
    else
    {
        this->setUseRunFunction(true);
    }
}
 
void
ControlPlatform::run()
{
 
    MultiDimPIDController pidTrans(
        in.getPTranslation(), in.getITranslation(), in.getDTranslation());
    //    PIDController pidY(in.getPTranslation(), in.getITranslation(), in.getDTranslation());
    PIDController pidRot(in.getPOrientation(), in.getIOrientation(), in.getDOrientation());
    // 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
    PlatformContext* c = getContext<PlatformContext>();
    DatafieldRefPtr posXRef =
        new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "positionX");
    DatafieldRefPtr posYRef =
        new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "positionY");
    DatafieldRefPtr oriRef =
        new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "rotation");
 
 
    float maxTransVel = in.getMaxTranslationVelocity();
    ARMARX_INFO << "orientation accuracy: " << in.getOrientationalAccuracy();
    ARMARX_INFO << "translational accuracy: " << in.getPositionalAccuracy();
 
 
    float targetX = in.getTargetPose()->x;
    float targetY = in.getTargetPose()->y;
    Eigen::Vector3f target{targetX, targetY, 0};
    float targetOri = in.getTargetPose()->z;
    //    float curOri = oriRef->getDataField()->getFloat();
 
    //    bool reverseZeroCrossing  = (targetOri - curOri > M_PI);
    //    bool ZeroCrossing= (targetOri - curOri < -M_PI);
 
    //    if(reverseZeroCrossing)
    //    {
    //        targetOri -= 2 * M_PI;
    //    }
    //    if(ZeroCrossing)
    //    {
    //        targetOri += 2 * M_PI;
    //    }
 
    float startOri = oriRef->getDataField()->getFloat();
 
 
    ARMARX_INFO << "Platform current pose: " << posXRef->getDataField()->getFloat() << ", "
                << posYRef->getDataField()->getFloat() << ", "
                << oriRef->getDataField()->getFloat();
    ;
    ARMARX_INFO << "Platform target pose: " << *in.getTargetPose();
    ARMARX_INFO << "AdjustPlatform target angle: " << targetOri;
    ARMARX_INFO << VAROUT(maxTransVel);
 
    if (targetOri > M_PI)
    {
        targetOri = -2 * M_PI + targetOri;
    }
 
    if (startOri > M_PI)
    {
        startOri = -2 * M_PI + startOri;
    }
 
    float angleAbsDelta = targetOri - startOri;
 
    // transform alpha to [-pi, pi)
    while (angleAbsDelta < -M_PI)
    {
        angleAbsDelta += 2 * M_PI;
    }
 
    while (angleAbsDelta >= M_PI)
    {
        angleAbsDelta -= 2 * M_PI;
    }
 
 
    Eigen::Vector2f targetOriVec(sin(targetOri), cos(targetOri));
    bool verify = in.getVerifyTargetPose();
    IceUtil::Time targetReachedTime = IceUtil::Time::seconds(0);
    float cycleTime = in.getCycleTime();
 
    while (!isRunningTaskStopped()) // stop run function if returning true
    {
        // do your calculations
        float posx = posXRef->getDataField()->getFloat();
        float posy = posYRef->getDataField()->getFloat();
        float ori = oriRef->getDataField()->getFloat();
        //        if(ZeroCrossing && ori  < M_PI)
        //            ori += 2 * M_PI;
 
        //        if(reverseZeroCrossing && ori > M_PI)
        //            ori -= 2 * M_PI;
 
        Eigen::Vector3f pos{posx, posy, 0.0f};
 
        if (ori > M_PI)
        {
            ori = -2 * M_PI + ori;
        }
        // put your user code for the enter-point here
        // execution time should be short (<100ms)
        //PlatformContext*  context = getContext<PlatformContext>();
 
        float angleDelta = targetOri - ori;
        ARMARX_INFO << deactivateSpam(1) << VAROUT(angleDelta);
 
 
        // transform alpha to [-pi, pi)
 
        while (angleDelta < -M_PI)
        {
            angleDelta += 2 * M_PI;
        }
 
        while (angleDelta >= M_PI)
        {
            angleDelta -= 2 * M_PI;
        }
 
 
        pidTrans.update(pos, target);
        pidRot.update(angleDelta, 0);
 
        Eigen::Vector2f newVel(pidTrans.getControlValue()[0], pidTrans.getControlValue()[1]);
 
 
        float newVelocityRot = -signedMin(pidRot.getControlValue(), in.getMaxRotationalVelocity());
        Eigen::Vector3f globalVel{newVel[0], newVel[1], 0};
        Eigen::Matrix3f m;
        m = Eigen::AngleAxisf(ori, Eigen::Vector3f::UnitZ());
        Eigen::Vector3f localVel = m.inverse() * globalVel;
        if (localVel.head(2).norm() > maxTransVel)
        {
            localVel.head(2) *= maxTransVel / localVel.head(2).norm();
        }
 
        //        Eigen::Vector3f currentVelocity {velXRef->get<float>(), velXRef->get<float>(), 0};
        //        if((localVel-currentVelocity).norm()/(cycleTime*0.001) > maxAcc)
        //           localVel = localVel.norm()*(cycleTime*0.001*maxAcc);
        if (std::isnan(localVel[0]) || std::isnan(localVel[1]) || std::isnan(newVelocityRot))
        {
            throw LocalException("A target velocity is NaN!");
        }
 
 
        //        ARMARX_INFO << deactivateSpam(0.1) << VAROUT(pidRot.getControlValue());
        //        ARMARX_INFO << deactivateSpam(0.1) << VAROUT(newVelocityRot);
 
        c->platformUnitPrx->move(localVel[0], localVel[1], newVelocityRot);
        //ARMARX_IMPORTANT <<  "local x velocity: " << localVel[0] << " local y velocity: " << localVel[1] << " rotation vel: " << newVelocityRot;
 
        if (fabs(pidTrans.previousError) < in.getPositionalAccuracy() &&
            fabs(pidRot.previousError) < in.getOrientationalAccuracy())
        {
            if (!verify)
            {
                emitPoseReached();
            }
            else
            {
                if (targetReachedTime.toSeconds() == 0)
                {
                    targetReachedTime = TimeUtil::GetTime();
                }
 
                ARMARX_INFO << deactivateSpam(0.1) << "error: " << pidTrans.previousError
                            << " x velocity: " << newVel[0] << " y velocity: " << newVel[1];
                ARMARX_INFO << deactivateSpam(0.1) << "rot error: " << pidRot.previousError
                            << " rot velocity: " << newVelocityRot;
 
 
                if ((TimeUtil::GetTime() - targetReachedTime).toMilliSecondsDouble() > 500)
                {
                    emitPoseReached();
                }
            }
        }
        else
        {
            ARMARX_INFO << deactivateSpam(1) << "error: " << pidTrans.previousError
                        << " x velocity: " << newVel[0] << " y velocity: " << newVel[1];
            ARMARX_INFO << deactivateSpam(1) << "rot error: " << pidRot.previousError
                        << " rot velocity: " << newVelocityRot;
        }
 
        usleep(cycleTime * 1000);
    }
 
 
    // Sync trans and rot:
 
    //    float orientationMultiplyer = in.getOrientationMultiplyerForPID();
    //    float maxTransVel = in.getMaxTranslationVelocity();
 
    //    MultiDimPIDController pidTransRot(in.getPTransRot(), in.getITransRot(), in.getDTransRot(), maxTransVel);
 
    //    PlatformContext*  c = getContext<PlatformContext>();
    //    DatafieldRefPtr posXRef = new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "positionX");
    //    DatafieldRefPtr posYRef = new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "positionY");
    //    DatafieldRefPtr oriRef = new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "rotation");
 
 
    //    float targetX = in.getTargetPose()->x;
    //    float targetY = in.getTargetPose()->y;
    //    float targetOri = in.getTargetPose()->z;
 
 
    //    float px = posXRef->getDataField()->getFloat();
    //    float py = posYRef->getDataField()->getFloat();
 
    //    float wholeDistance = std::sqrt(px * px + py * py);
 
 
    //    ARMARX_INFO << "Platform current pose: " << posXRef->getDataField()->getFloat() << ", " << posYRef->getDataField()->getFloat() << ", " << oriRef->getDataField()->getFloat();;
    //    ARMARX_INFO << "Platform target pose: " << *in.getTargetPose();
    //    ARMARX_INFO << "AdjustPlatform target angle: " << targetOri;
    //    ARMARX_INFO << VAROUT(maxTransVel);
 
 
    //    if (targetOri > M_PI)
    //    {
    //        targetOri = - 2  * M_PI + targetOri;
    //    }
 
    //    bool verify = in.getVerifyTargetPose();
    //    IceUtil::Time targetReachedTime = IceUtil::Time::seconds(0);
    //    float cycleTime = in.getCycleTime();
 
    //    while (!isRunningTaskStopped()) // stop run function if returning true
    //    {
    //        // do your calculations
    //        float posx = posXRef->getDataField()->getFloat();
    //        float posy = posYRef->getDataField()->getFloat();
    //        float ori = oriRef->getDataField()->getFloat();
 
    //        float currentDistance = std::sqrt(posx * posx + posy * posy);
    //        float percentageDone = currentDistance <= in.getPositionalAccuracy() ? 0 : currentDistance / wholeDistance;
 
 
    //        if (ori > M_PI)
    //        {
    //            ori = - 2  * M_PI + ori;
    //        }
    //        float angleDelta = targetOri - ori;
 
    //        // transform alpha to [-pi, pi)
    //        while (angleDelta < -M_PI)
    //        {
    //            angleDelta += 2 * M_PI;
    //        }
 
    //        while (angleDelta >= M_PI)
    //        {
    //            angleDelta -= 2 * M_PI;
    //        }
 
 
    //        float posAngle = angleDelta * (-1) * orientationMultiplyer;
    //        float targetAngle = posAngle * percentageDone;
 
 
    //        Eigen::Vector3f pos {posx, posy, posAngle};
    //        ARMARX_INFO << "CurrentPIDValues: " << pos;
    //        Eigen::Vector3f target {targetX, targetY, targetAngle};
    //        ARMARX_INFO << "targetPIDValues: " << target;
    //        pidTransRot.update(pos, target);
 
    //        Eigen::Vector3f newVel(pidTransRot.getControlValue()[0], pidTransRot.getControlValue()[1], pidTransRot.getControlValue()[2]);
 
    //        float newVelRot = signedMin(newVel[2] / (in.getPTransRot() * orientationMultiplyer), in.getMaxRotationalVelocity());
 
    //        Eigen::Vector3f globalVel {newVel[0], newVel[1], 0};
    //        Eigen::Matrix3f m;
    //        m = Eigen::AngleAxisf(ori, Eigen::Vector3f::UnitZ());
    //        Eigen::Vector3f localVel = m.inverse() * globalVel;
    //        if (localVel.norm() > maxTransVel)
    //        {
    //            localVel *= maxTransVel / localVel.norm();
    //        }
 
    //        if (std::isnan(localVel[0]) || std::isnan(localVel[1]) || std::isnan(newVelRot))
    //        {
    //            throw LocalException("A target velocity is NaN!");
    //        }
 
    //        c->platformUnitPrx->move(localVel[0],
    //                                 localVel[1],
    //                                 newVelRot);
 
 
    //        ARMARX_INFO << deactivateSpam(1) << "local x velocity: " << localVel[0] << " local y velocity: " << localVel[1] << " rotation vel: " << newVelRot;
 
    //        if (fabs(pidTransRot.previousError) < in.getPositionalAccuracy() && fabs(pidTransRot.previousError) < in.getOrientationalAccuracy() * orientationMultiplyer)
    //        {
    //            if (!verify)
    //            {
    //                emitPoseReached();
    //            }
    //            else
    //            {
    //                if (targetReachedTime.toSeconds() == 0)
    //                {
    //                    targetReachedTime = TimeUtil::GetTime();
    //                }
 
    //                ARMARX_INFO << deactivateSpam(0.1) << "error: " << pidTransRot.previousError << " x velocity: " << localVel[0] << " y velocity: " << localVel[1] << " rot velocity: " << newVelRot;
 
    //                if ((TimeUtil::GetTime() - targetReachedTime).toMilliSecondsDouble() > 500)
    //                {
    //                    emitPoseReached();
    //                }
    //            }
    //        }
    //        else
    //        {
    //            ARMARX_INFO << deactivateSpam(0.1) << "error: " << pidTransRot.previousError << " x velocity: " << localVel[0] << " y velocity: " << localVel[1] << " rot velocity: " << newVelRot;
    //        }
 
    //        usleep(cycleTime * 1000);
    //    }
}
 
void
ControlPlatform::cleanUp()
{
    // put your user code for the exit point here
    // execution time should be short (<100ms)
    PlatformContext* c = getContext<PlatformContext>();
    DatafieldRefPtr posXRef =
        new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "positionX");
    DatafieldRefPtr posYRef =
        new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "positionY");
    DatafieldRefPtr oriRef =
        new DatafieldRef(c->platformUnitObserverPrx, "platformPose", "rotation");
    float posX = posXRef->getDataField()->getFloat();
    float posY = posYRef->getDataField()->getFloat();
    float ori = oriRef->getDataField()->getFloat();
    Vector3Ptr error = new Vector3(fabs(in.getTargetPose()->x - posX),
                                   fabs(in.getTargetPose()->y - posY),
                                   fabs(in.getTargetPose()->z - ori));
    ARMARX_INFO << "Remaining platform error: " << *error;
    out.setRemainingError(error);
}
 
void
ControlPlatform::onBreak()
{
    while (!isRunningTaskFinished())
    {
        ARMARX_INFO << deactivateSpam(1) << "Waiting for runfunction";
        usleep(1000);
    }
    ARMARX_INFO << "Breaking state";
    PlatformContext* c = getContext<PlatformContext>();
    cleanUp();
    if (in.getStopPlatformOnBreak())
    {
        c->platformUnitPrx->stopPlatform();
    }
}
 
void
ControlPlatform::onExit()
{
    while (!isRunningTaskFinished())
    {
        ARMARX_INFO << deactivateSpam(1) << "Waiting for runfunction";
        usleep(1000);
    }
    ARMARX_INFO << "Exiting state";
 
    cleanUp();
    PlatformContext* c = getContext<PlatformContext>();
    if (in.getStopPlatformOnExit())
    {
        c->platformUnitPrx->stopPlatform();
    }
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
ControlPlatform::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new ControlPlatform(stateData));
}