MoveJointPlatformTrajectory.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::MotionAndPlatformControlGroup
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "MoveJointPlatformTrajectory.h"
 
#include <algorithm>
#include <chrono>
#include <thread>
 
#include <VirtualRobot/RobotConfig.h>
 
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/libraries/core/RobotStatechartContext.h>
 
using namespace armarx;
using namespace MotionAndPlatformControlGroup;
 
// DO NOT EDIT NEXT LINE
MoveJointPlatformTrajectory::SubClassRegistry
    MoveJointPlatformTrajectory::Registry(MoveJointPlatformTrajectory::GetName(),
                                          &MoveJointPlatformTrajectory::CreateInstance);
 
void
MoveJointPlatformTrajectory::onEnter()
{
    // put your user code for the enter-point here
    // execution time should be short (<100ms)
    timeoutEvent = setTimeoutEvent(in.getTimeoutInMs(), createEventFailure());
 
    t = in.getTrajectory();
    jointNames = t->getDimensionNames();
    if (!validJointNames())
    {
        ARMARX_ERROR_S << "MoveJointPlatformTrajectory expects a trajectory with x, y, alpha as "
                          "its first coordinates!";
        ARMARX_ERROR_S << "given trajectory dimensions: " << jointNames;
        emitFailure();
        return;
    }
    maxAccs.clear();
    if (in.isJointMaxAccelsSet())
    {
        maxAccs = in.getJointMaxAccels();
    }
    jointMaxSpeeds.clear();
    if (in.isJointMaxSpeedsSet())
    {
        jointMaxSpeeds = in.getJointMaxSpeeds();
    }
 
 
    jointTargetTolerance = std::fabs(in.getJointTargetTolerance());
    translationTargetTolerance = std::fabs(in.getTranslationTargetTolerance());
    context = getContext<MotionAndPlatformControlGroupStatechartContext>();
    lastCfgIt = --(t->end());
    currentCfgIt = t->begin();
    ARMARX_DEBUG << "timeout installed" << std::endl;
}
 
void
MoveJointPlatformTrajectory::run()
{
    ARMARX_INFO_S << "trajectory:\n" << in.getTrajectory()->output();
    if (in.getSkipFirstTimepoint() >= in.getTrajectory()->size())
    {
        //nothing to do
        ARMARX_WARNING_S << "The Trajectory did not result in movement!\n"
                         << "skip first = " << in.getSkipFirstTimepoint();
        emitSuccess();
    }
 
    //the platform has no control mode
    //exec
    datafieldsValues = getDataFieldIdentifiers("jointangles");
    if (in.getUseNativePositionControl())
    {
        setControlMode(ePositionControl);
        ctrlNative();
    }
    else
    {
        datafieldsVelocities = getDataFieldIdentifiers("jointvelocities");
        setControlMode(eVelocityControl);
        ctrlInterpol();
    }
}
 
void
MoveJointPlatformTrajectory::onExit()
{
    waitForDone(false);
    ARMARX_INFO_S << "exit MoveJointPlatformTrajectory";
}
 
void
MoveJointPlatformTrajectory::onBreak()
{
    waitForDone(true);
    ARMARX_INFO_S << "break MoveJointPlatformTrajectory";
}
 
void
MoveJointPlatformTrajectory::waitForDone(bool stopAtCurrentPos)
{
    while (!isRunningTaskFinished())
    {
        std::this_thread::sleep_for(std::chrono::milliseconds{5});
    }
    if (in.getSkipFirstTimepoint() >= in.getTrajectory()->size())
    {
        return;
    }
    cacheValues();
 
    if (in.getHoldPositionAfterwards())
    {
        NameValueMap targetJointAngles;
        NameControlModeMap controlModes;
        NameValueMap targetJointVelocities;
 
        for (size_t i = 0; i < jointNames.size(); ++i)
        {
            targetJointAngles[jointNames.at(i)] =
                stopAtCurrentPos ? getValue(i) : lastCfgIt->getPosition(i);
            targetJointVelocities[jointNames.at(i)] = 0.0f;
            controlModes[jointNames.at(i)] = ePositionControl;
        }
        ARMARX_INFO_S << "hold position aftawards is set! (using position contol to hold position)";
        setControlMode(ePositionControl);
        setValues(targetJointAngles);
        setVelocity(targetJointVelocities);
    }
 
    ARMARX_INFO << "Joint target tolerance: " << in.getJointTargetTolerance();
    NameValueMap differences;
    for (size_t i = 0; i < jointNames.size(); ++i)
    {
        differences[jointNames.at(i)] = lastCfgIt->getPosition(i) - getValue(i);
    }
    ARMARX_INFO << VAROUT(differences);
    out.setJointAndPlatformDistancesToTargets(differences);
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
MoveJointPlatformTrajectory::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new MoveJointPlatformTrajectory(stateData));
}
 
void
MoveJointPlatformTrajectory::ctrlInterpol()
{
    //running data per joint used for ctrl
    struct JointData
    {
        //data per joint
        float maxAcc;
        float maxVel;
        float maxDeltaVel;
        //data changed per config
        float targetAng;
        float prevAng;
        float configDeltaAng;
        float allowedPercInaccuracy;
 
        //data changed per iteration
        float ang;
        float vel;
 
        float deltaAng;
        float deltaAngSig;
        float percAng; //percentage of the current angle done
 
        float nextVel;
        float nextPercAng;
        bool nextVelForced;
    };
 
    std::vector<JointData> jointData(jointNames.size());
 
    //init per joint data
    {
        cacheValues();
        for (std::size_t i = 0; i < jointNames.size(); ++i)
        {
            JointData& data = jointData.at(i);
            data.maxAcc = std::fabs(getMaxAcc(i));
            data.maxVel = std::fabs(getMaxVel(i));
            data.maxDeltaVel = data.maxAcc * secondsPerPeriod;
            data.targetAng = getValue(i);
        }
    }
 
    auto updateJoitnDataForNewConfig = [&]
    {
        for (std::size_t i = 0; i < jointNames.size(); ++i)
        {
            JointData& data = jointData.at(i);
            data.prevAng = data.targetAng;
            data.targetAng = currentCfgIt->getPosition(i);
            data.configDeltaAng = data.targetAng - data.prevAng;
            data.allowedPercInaccuracy = getTargetTolerance(i) / std::fabs(data.configDeltaAng);
            ARMARX_INFO_S << jointNames.at(i) << " move " << data.prevAng << " -> "
                          << data.targetAng << " (delta = " << data.configDeltaAng << ")";
        }
    };
 
    ARMARX_INFO_S << "MoveJointPlatformTrajectory start axis sync (acceleration limited) PTP";
    NameValueMap targetJointVel;
    //skip the first target?
    if (in.getSkipFirstTimepoint())
    {
        ARMARX_INFO_S << "skipping first config";
        ++currentCfgIt;
        updateJoitnDataForNewConfig();
    }
    while (!isRunningTaskStopped() && !doneAllConfigs())
    {
        //update per config data
        updateJoitnDataForNewConfig();
 
        //ctrl for current target
        while (!isRunningTaskStopped())
        {
            auto startIter = std::chrono::high_resolution_clock::now();
            //per it
            cacheValues();
            cacheVelocities();
 
            // data to decide whether we go to the next config
            //            float maxAbsDeltaAng = -std::numeric_limits<float>::infinity();
            //            float maxAbsDeltaTrans = -std::numeric_limits<float>::infinity();
            bool dontAdvanceToNextConfig = false;
            //used to determine the major axis
            float minNextPerc = std::numeric_limits<float>::infinity();
 
            //calculate per joint the optimal velocity (getting it as close as possible to the goal without overshooting it)
            //also calculate the axis lagging behind (aka the axis with the lowest % of distance to its current target
            //this will be the major axis and all other axis will try to reach the same % of their current target
            //if an axis has to deaccellerate maximally to stop from overshooting the target, it will do so (and may not try to reach the %)
            //if an axis reached its goal already it will not move
            //no axis will move backwards (they only stop to wait for others)
            for (std::size_t i = 0; i < jointNames.size(); ++i)
            {
                JointData& data = jointData.at(i);
                //init sentinels
                data.nextPercAng = -1;
                data.percAng = -1;
                data.nextVelForced = false;
                //angles
                data.ang = getValue(i);
                data.vel = getVelocity(i);
                const float velSig = sign(data.vel);
 
                data.deltaAng = data.targetAng - data.ang;
                data.deltaAngSig = sign(data.deltaAng);
 
                //                float& maxAbsDelta = (i < 3 ? maxAbsDeltaTrans : maxAbsDeltaAng);
                //                maxAbsDelta = std::max(std::fabs(data.deltaAng), maxAbsDelta);
 
                dontAdvanceToNextConfig |= std::fabs(data.deltaAng) > getConfigChangeTolerance(i);
 
                const float absBrakingDistance = data.vel * data.vel / (2.f * data.maxAcc);
 
                //if we are near the goal (in tolerance)         -> deaccelerate/ move to goal (forced)
                //if this joint should not move (reached target) -> deaccelerate/ move to goal (forced)
                if (((data.deltaAngSig ==
                      velSig) //if we brake we move in the direction of the target
                     && (std::fabs(data.deltaAng) <=
                         absBrakingDistance +
                             getTargetTolerance(i)) //we are near the target and have to brake now
                     ) ||
                    (std::fabs(data.deltaAng) <= getTargetTolerance(i)) //we are at the goal
                )
                {
                    if (std::fabs(data.vel) < data.maxDeltaVel)
                    {
                        //we are slow enough to set the vel to 0
                        data.nextVel = 0;
                    }
                    else
                    {
                        //we are to fast to set the vel to 0 -> brake with max a
                        data.nextVel = (std::fabs(data.vel) - data.maxDeltaVel) * sign(data.vel);
                    }
                    data.nextVelForced = true;
 
 
                    assert(data.nextVel <= data.maxVel);
                    assert(data.nextVel >= -data.maxVel);
                    continue;
                }
 
                //calc opt next vel (vel required to reach target if we will deaccelerate starting at next step)
                //it ignores max a for acceleration (it is only used for deacceleration) and v
                const float p = data.maxDeltaVel;
                const float q = data.deltaAngSig *
                                (data.vel * data.maxDeltaVel - 2 * data.maxAcc * data.deltaAng);
                const float nextVelOpt =
                    data.deltaAngSig * (-p / 2.f + std::sqrt((p / 2.f) * (p / 2.f) - q));
                const float nextVelAccCapped = std::clamp(
                    nextVelOpt, data.vel - data.maxDeltaVel, data.vel + data.maxDeltaVel);
                const float nextVelVelCapped =
                    std::clamp(nextVelAccCapped, -data.maxVel, data.maxVel);
                data.nextVel = nextVelVelCapped;
 
                assert(data.nextVel <= data.maxVel);
                assert(data.nextVel >= -data.maxVel);
                const float nextAngPredict =
                    data.ang + secondsPerPeriod * (data.vel + data.nextVel) / 2.f;
 
                //joint is past target
                data.percAng =
                    1 - data.deltaAng / data.configDeltaAng; // should not be nan, may be  +- inf
                assert(std::isfinite(data.percAng));
                if (data.percAng > 1.f || //past target ->move to target
                    std::fabs(data.configDeltaAng) < jointTargetTolerance
                    // this joint should not move (but it is not near enough) try to move it to the target
                )
                {
                    //we want the current joint to move to the target
                    data.nextVelForced = true;
                    continue;
                }
 
                const float nextAngPredictDelta = data.targetAng - nextAngPredict;
                data.nextPercAng =
                    1 - nextAngPredictDelta / data.configDeltaAng; // should not be nan / +- inf
                assert(std::isfinite(data.nextPercAng));
                minNextPerc = std::min(data.nextPercAng, minNextPerc);
            }
 
            ARMARX_INFO << VAROUT(minNextPerc);
            //calculate final vel (in consideration of the major axis)
            //there are 4 options:
            // 1: this joints velocity is forced -> don't adapt its vel
            // 2: this joint is one of the major axis -> don't adapt its vel
            // 3: this joint is further than the major axis will be -> try to stop
            // 4: this joint is neither of the above -> try to reach the major axis %
            for (std::size_t i = 0; i < jointNames.size(); ++i)
            {
                const auto& name = jointNames.at(i);
                JointData& data = jointData.at(i);
                //case 1+2
                if (data.nextVelForced ||
                    data.nextPercAng <= minNextPerc + data.allowedPercInaccuracy)
                {
 
                    //either this joint has to brake, or it is a major axis
                    targetJointVel[name] = data.nextVel;
                    assert(std::fabs(targetJointVel[name]) <=
                           std::fabs(data.maxVel) + std::numeric_limits<float>::epsilon());
                    assert(std::fabs(targetJointVel[name]) <=
                           std::fabs(data.vel) + std::fabs(data.maxDeltaVel) +
                               std::numeric_limits<float>::epsilon());
 
                    if (data.nextVelForced)
                    {
                        ARMARX_INFO << "for name " << name << "\t" << targetJointVel[name] << "  ("
                                    << data.maxVel << ") delta " << data.deltaAng;
                    }
                    else
                    {
                        ARMARX_INFO << "maj name " << name << "\t" << targetJointVel[name] << "  ("
                                    << data.maxVel << ") delta " << data.deltaAng << "\t p/np"
                                    << data.percAng << "\t/\t" << data.nextPercAng;
                    }
                    continue;
                }
                //case 3
                if (data.percAng >= minNextPerc)
                {
                    // we don't want to move our joint backwards.
                    // => slow down
                    targetJointVel[name] =
                        std::max(0.f, std::fabs(data.vel) - data.maxDeltaVel) * sign(data.vel);
                    assert(std::fabs(targetJointVel[name]) <=
                           std::fabs(data.maxVel) + std::numeric_limits<float>::epsilon());
                    assert(std::fabs(targetJointVel[name]) <=
                           std::fabs(data.vel) + std::fabs(data.maxDeltaVel) +
                               std::numeric_limits<float>::epsilon());
 
                    ARMARX_INFO << "slo name " << name << "\t" << targetJointVel[name] << "  ("
                                << data.maxVel << ") delta " << data.deltaAng << "\t p/np"
                                << data.percAng << "\t/\t" << data.nextPercAng;
                    continue;
                }
                //case 4
                //the delta this joint should moved (is capped by maxAcc/maxVel, since the new vel will be less than the calculated nextVel
                const float deltaAngMoved = data.configDeltaAng - data.deltaAng;
                const float deltaAngMovedNext = minNextPerc * data.configDeltaAng;
                const float delatAngToMoveInIteratiron = deltaAngMovedNext - deltaAngMoved;
                const float newVel = 2.f * delatAngToMoveInIteratiron / secondsPerPeriod - data.vel;
                assert(std::fabs(newVel) <=
                       std::fabs(data.nextVel) + std::numeric_limits<float>::epsilon());
                targetJointVel[name] = newVel;
                assert(std::fabs(targetJointVel[name]) <=
                       std::fabs(data.maxVel) + std::numeric_limits<float>::epsilon());
                assert(std::fabs(targetJointVel[name]) <=
                       std::fabs(data.vel) + std::fabs(data.maxDeltaVel) +
                           std::numeric_limits<float>::epsilon());
                ARMARX_INFO << "var name " << name << "\t" << targetJointVel[name] << "  ("
                            << data.maxVel << ") delta " << data.deltaAng << "\t p/np"
                            << data.percAng << "\t/\t" << data.nextPercAng;
            }
            //set vels
            ARMARX_INFO << "###############################";
            setVelocity(targetJointVel);
            if (!dontAdvanceToNextConfig)
            {
                break;
            }
            //check for speed
            const auto tIter = std::chrono::high_resolution_clock::now() - startIter;
            ARMARX_INFO_S << deactivateSpam() << "Controler iteration took "
                          << std::chrono::duration_cast<std::chrono::milliseconds>(tIter).count()
                          << " ms";
            if (tIter > period)
            {
                ARMARX_WARNING_S
                    << "iteration took long! ("
                    << std::chrono::duration_cast<std::chrono::milliseconds>(tIter).count()
                    << " ms. Period = " << period.count() << " ms)";
            }
            else
            {
                std::this_thread::sleep_until(period + startIter);
            }
        } //ctrl for current target
        ++currentCfgIt;
    } //ctrl for all targets
    if (doneAllConfigs())
    {
        ARMARX_INFO_S
            << "MoveJointPlatformTrajectory end success axis sync (acceleration limited) PTP";
        emitSuccess();
    }
    ARMARX_INFO_S
        << "MoveJointPlatformTrajectory exit failure axis sync (acceleration limited) PTP";
}
 
void
MoveJointPlatformTrajectory::ctrlNative()
{
    ARMARX_INFO_S << "MoveJointPlatformTrajectory native PTP";
    {
        NameValueMap maxVelsMap;
        for (std::size_t i = 0; i < jointNames.size(); ++i)
        {
            maxVelsMap[jointNames.at(i)] = getMaxVel(i);
        }
        setMaxVelocity(maxVelsMap);
    }
    NameValueMap jointTargetMap;
    while (!doneAllConfigs())
    {
        if (isRunningTaskStopped())
        {
            return;
        }
        currentCfgIt->writePositionsToNameValueMap(jointTargetMap);
        setValues(jointTargetMap);
        //wait for joint reached
        while (true)
        {
            setValues(jointTargetMap);
            if (isRunningTaskStopped())
            {
                return;
            }
            //calc max delta
            cacheValues();
            float maxDelta = 0;
            float maxTranslationDelta = std::max(std::fabs(getValue(0) - jointTargetMap.at("x")),
                                                 std::fabs(getValue(1) - jointTargetMap.at("y")));
            //alpha is considered a rotation joint
            for (std::size_t i = 2; i < jointNames.size(); ++i)
            {
                maxDelta = std::max(maxDelta,
                                    std::fabs(getValue(i) - jointTargetMap.at(jointNames.at(i))));
            }
            //break if delta is small
 
            if (maxDelta < jointTargetTolerance && maxTranslationDelta < translationTargetTolerance)
            {
                break;
            }
            std::this_thread::sleep_for(period);
        }
        ++currentCfgIt;
    }
    ARMARX_INFO_S << "MoveJointPlatformTrajectory exit native PTP";
    emitSuccess();
}
 
float
MoveJointPlatformTrajectory::getValue(std::size_t i) const
{
    return getFromList(cachedValues, i);
}
 
float
MoveJointPlatformTrajectory::getVelocity(std::size_t i) const
{
    return getFromList(cachedVelocities, i);
}
 
int
MoveJointPlatformTrajectory::sign(double val)
{
    return (val < 0) ? -1 : 1;
}