MoveJoints.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::MotionControlGroup
 * @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 "MoveJoints.h"
 
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
 
#include "MotionControlGroupStatechartContext.h"
 
//#include <RobotAPI/libraries/core/RobotStatechartContext.h>
#include <VirtualRobot/RobotConfig.h>
 
using namespace armarx;
using namespace MotionControlGroup;
 
// DO NOT EDIT NEXT LINE
MoveJoints::SubClassRegistry MoveJoints::Registry(MoveJoints::GetName(),
                                                  &MoveJoints::CreateInstance);
 
MoveJoints::MoveJoints(XMLStateConstructorParams stateData) :
    XMLStateTemplate<MoveJoints>(stateData), MoveJointsGeneratedBase<MoveJoints>(stateData)
{
}
 
void
MoveJoints::onEnter()
{
    timeoutEvent = setTimeoutEvent(in.gettimeoutInMs(), createEventEvTimeout());
    ARMARX_DEBUG << "timeout installed" << std::endl;
}
 
void
MoveJoints::run()
{
    // put your user code for the enter-point here
    // execution time should be short (<100ms)
 
    MotionControlGroupStatechartContext* context =
        getContext<MotionControlGroupStatechartContext>();
 
    NameValueMap targetJointAngles;
    NameValueMap targetJointVelocities;
    std::vector<std::string> jointNames;
    std::vector<float> targetJointValues;
 
    if (in.isjointNamesSet() && in.istargetJointValuesSet())
    {
        jointNames = in.getjointNames();
        targetJointValues = in.gettargetJointValues();
    }
    else if (in.isalternativeJointMapSet())
    {
        auto map = in.getalternativeJointMap();
 
        for (const auto& elem : map)
        {
            jointNames.push_back(elem.first);
            targetJointValues.push_back(elem.second);
        }
    }
    else
    {
        throw LocalException("You must either specify the alternativeJointMap param or the "
                             "targetJointValues and jointNames params.");
    }
 
    if (jointNames.size() != targetJointValues.size())
    {
        throw LocalException("Sizes of joint name list and joint value list do not match!");
    }
 
    if (jointNames.size() == 0)
    {
        emitEvJointTargetReached();
        local.setCommandedJoints(NameValueMap());
        return;
    }
 
    ARMARX_VERBOSE << "number of joints that will be set: " << jointNames.size() << flush;
    NameControlModeMap controlModes;
    bool useNativeController = in.getUseNativePositionControl();
    for (size_t i = 0; i < jointNames.size(); i++)
    {
        targetJointAngles[jointNames.at(i)] = targetJointValues.at(i);
        controlModes[jointNames.at(i)] = useNativeController ? ePositionControl : eVelocityControl;
        ARMARX_DEBUG << "setting joint angle for joint '" << jointNames.at(i) << "' to "
                     << targetJointValues.at(i) << std::endl;
    }
 
    // TODO: Set Max Velocities
    if (in.isjointMaxSpeedsSet() &&
        getInput<SingleTypeVariantList>("jointMaxSpeeds")->getSize() == (int)jointNames.size())
    {
        SingleTypeVariantListPtr maxJointValues = getInput<SingleTypeVariantList>("jointMaxSpeeds");
 
        for (size_t i = 0; i < jointNames.size(); i++)
        {
            targetJointVelocities[jointNames.at(i)] = maxJointValues->getVariant(i)->getFloat();
        }
    }
    else
    {
        float maxVel = in.getjointMaxSpeed();
 
        for (size_t i = 0; i < jointNames.size(); i++)
        {
            targetJointVelocities[jointNames.at(i)] = maxVel;
        }
    }
 
    // now install the condition
    Term cond;
    float tolerance = getInput<float>("jointTargetTolerance");
    NameValueMap jointValueMap;
 
    for (size_t i = 0; i < jointNames.size(); i++)
    {
        ARMARX_DEBUG << "creating condition (" << i << " of " << jointNames.size() << ") for value "
                     << targetJointValues.at(i) << flush;
        Literal approx(DataFieldIdentifier(context->getKinematicUnitObserverName(),
                                           "jointangles",
                                           jointNames.at(i)),
                       checks::approx,
                       {targetJointValues.at(i), tolerance});
        cond = cond && approx;
        jointValueMap[jointNames.at(i)] = targetJointValues.at(i);
    }
    local.setCommandedJoints(jointValueMap);
    ARMARX_DEBUG << "installing condition: EvJointTargetReached" << std::endl;
    targetReachedCondition = installCondition<EvJointTargetReached>(cond);
    ARMARX_DEBUG << "condition installed: EvJointTargetReached" << std::endl;
 
    context->getKinematicUnit()->switchControlMode(controlModes);
    if (useNativeController)
    {
        context->getKinematicUnit()->setJointVelocities(targetJointVelocities);
        context->getKinematicUnit()->setJointAngles(targetJointAngles);
        return;
    }
 
    DataFieldIdentifierBaseList jointList;
    for (auto& joint : jointValueMap)
    {
        jointList.push_back(new DataFieldIdentifier(
            context->getKinematicUnitObserverName(), "jointangles", joint.first));
    }
    ARMARX_INFO << "Joint targets: " << jointValueMap;
    auto currentValues = context->getRobot()->getConfig()->getRobotNodeJointValueMap();
 
    ARMARX_INFO << "Joint target tolerance: " << tolerance;
 
 
    NameValueMap currentTargetJointVelocities;
    auto start = TimeUtil::GetTime();
    float accelerationTime = in.getAccelerationTime();
    float maxVel = in.getjointMaxSpeed();
    while (!isRunningTaskStopped())
    {
        TimeUtil::MSSleep(10);
 
        currentValues = context->getRobot()->getConfig()->getRobotNodeJointValueMap();
        ; //context->getKinematicUnitObserver()->getDataFields(jointList);
        float maxDelta = 0;
        size_t i = 0;
        for (auto& value : jointValueMap)
        {
            float delta = fabs((value.second - currentValues[value.first]));
            maxDelta = std::max(delta, maxDelta);
            //            ARMARX_INFO << deactivateSpam(1, value.first) << value.first << ": Difference : " << delta;
            i++;
        }
        if (maxDelta > 0)
        {
            float motionDuration = maxDelta / maxVel;
            ARMARX_INFO << deactivateSpam(1) << VAROUT(maxDelta) << VAROUT(motionDuration);
            targetJointVelocities.clear();
            i = 0;
            for (auto& value : jointValueMap)
            {
                float delta = (value.second - currentValues[value.first]);
                targetJointVelocities[value.first] = delta / motionDuration;
                i++;
            }
            ARMARX_INFO << deactivateSpam(1) << VAROUT(targetJointVelocities);
 
            i = 0;
            NameValueMap currentDeltas;
            for (auto& value : jointValueMap)
            {
                float delta = ((value.second - currentValues[value.first]));
                currentDeltas[value.first] = delta;
                float slowdownFactor =
                    (1 / (1 + exp(-(fabs(delta) - 0.1) / 0.04))); //sigmoid function
                float newVel = std::min(fabs(targetJointVelocities[value.first]),
                                        fabs(delta) * slowdownFactor * 10); // slowdown at end
                if (newVel * delta < 0)
                {
                    newVel *= -1;
                }
                newVel *= std::min(1.0,
                                   (TimeUtil::GetTime() - start).toMilliSecondsDouble() /
                                       accelerationTime); // time to accelerate to fullspeed
                currentTargetJointVelocities[value.first] = newVel;
                i++;
            }
            ARMARX_INFO << deactivateSpam(1)
                        << "Current joint target velocities: " << currentTargetJointVelocities;
            ARMARX_INFO << deactivateSpam(1) << "Current joint deltas to target: " << currentDeltas;
            context->getKinematicUnit()->setJointVelocities(currentTargetJointVelocities);
        }
    }
    if (in.getSetVelocitiesToZeroAtEnd())
    {
        for (auto& joint : jointValueMap)
        {
            targetJointVelocities[joint.first] = 0.0f;
        }
        context->getKinematicUnit()->setJointVelocities(targetJointVelocities);
    }
 
 
    if (in.getHoldPositionAfterwards())
    {
        currentValues = context->getRobot()->getConfig()->getRobotNodeJointValueMap();
        int i = 0;
        for (auto& joint : jointValueMap)
        {
            controlModes[joint.first] = ePositionControl;
            //targetJointAngles[joint.first] = currentValues.at(i)->getFloat();
            i++;
        }
        context->getKinematicUnit()->switchControlMode(controlModes);
        context->getKinematicUnit()->setJointAngles(targetJointAngles);
    }
}
 
void
MoveJoints::onExit()
{
    // put your user code for the exit point here
    // execution time should be short (<100ms)
    while (!isRunningTaskFinished())
    {
        TimeUtil::MSSleep(5);
    }
    MotionControlGroupStatechartContext* context =
        getContext<MotionControlGroupStatechartContext>();
    DataFieldIdentifierBaseList jointList;
    auto jointValueMap = local.getCommandedJoints();
    NameValueMap targetJointVelocities;
    for (auto& joint : jointValueMap)
    {
        jointList.push_back(new DataFieldIdentifier(
            context->getKinematicUnitObserverName(), "jointangles", joint.first));
        targetJointVelocities[joint.first] = 0.0f;
    }
 
    auto currentValues = context->getRobot()->getConfig()->getRobotNodeJointValueMap();
    size_t i = 0;
 
    float tolerance = getInput<float>("jointTargetTolerance");
    ARMARX_INFO << "Joint target tolerance: " << tolerance;
    NameValueMap differences;
    for (auto& value : jointValueMap)
    {
        differences[value.first] = (value.second - currentValues[value.first]);
        i++;
    }
    ARMARX_VERBOSE << VAROUT(differences);
}
 
// DO NOT EDIT NEXT FUNCTION
std::string
MoveJoints::GetName()
{
    return "MoveJoints";
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
MoveJoints::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new MoveJoints(stateData));
}