ShapeHand.cpp

Go to the documentation of this file.

#include "ShapeHand.h"
 
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
 
namespace armarx::control::skills::skills
{
    ShapeHand::ShapeHand(const HandUnitServices& srv) :
        ::armarx::skills::SpecializedSkill<ParamType>(GetSkillDescription()), srv_(srv)
    {
    }
 
    ::armarx::skills::Skill::MainResult
    ShapeHand::main()
    {
        // verify exactly one of the parameters 'shapeName' and 'jointAngles' is set
        if ((not getParameters().shapeName) and (not getParameters().jointAngles))
        {
            ARMARX_ERROR << "Exactly one of the parameters 'shapeName' and 'jointAngles' must be "
                            "set. However, neither has been set.";
            return MakeFailedResult();
        }
        else if (getParameters().shapeName and getParameters().jointAngles)
        {
            ARMARX_ERROR << "Exactly one of the parameters 'shapeName' and 'jointAngles' must be "
                            "set. However, both have been set.";
            return MakeFailedResult();
        }
 
        // set shape/joinAngles
 
        auto& handUnit = srv_.getHandUnit(getParameters().hand);
 
        if (getParameters().shapeName)
        {
            const auto shape = getParameters().shapeName.value();
            ARMARX_INFO << VAROUT(shape);
 
            // check that the shape is defined
            const auto defShapes =
                ::armarx::SingleTypeVariantListPtr::dynamicCast(handUnit->getShapeNames())
                    ->toStdVector<std::string>();
 
            if (not std::any_of(defShapes.begin(),
                                defShapes.end(),
                                [&shape](const auto& defShape) { return shape == defShape; }))
            {
                ARMARX_ERROR << "Shape with name '" << shape
                             << "' not defined. Defined shapes: " << defShapes;
                return MakeFailedResult();
            }
 
            // set shape
            ARMARX_INFO << "Setting shape of hand '" << handUnit->getHandName() << "' to '" << shape
                        << "'.";
 
            handUnit->setShape(shape);
 
            if (getParameters().waitUntilFinished)
            {
                return waitUntilFinished(handUnit->getShapeJointValues(shape));
            }
        }
 
        else // getParameters().jointAngles
        {
            const auto jointAngles = getParameters().jointAngles.value();
 
            ARMARX_INFO << "Setting joint angles of hand '" << handUnit->getHandName()
                        << "' to: " << jointAngles;
 
            handUnit->setJointAngles(jointAngles);
 
            if (getParameters().waitUntilFinished)
            {
                return waitUntilFinished(jointAngles);
            }
        }
        return MakeSucceededResult();
    }
 
    armarx::skills::Skill::MainResult
    ShapeHand::waitUntilFinished(const NameValueMap& targetAngles)
    {
        auto& handUnit = srv_.getHandUnit(getParameters().hand);
        const auto movementTimeout = getParameters().waitUntilFinished->movementTimeout;
 
        ARMARX_INFO << "Waiting until target position is reached"
                    << (movementTimeout ? " or movement times out" : "");
 
        struct
        {
            NameValueMap angles;
            ::armarx::DateTime time;
        } pastJointAngles{handUnit->getCurrentJointValues(), DateTime::Now()};
 
        const Clock clock;
        const auto sleepTime = Duration::MilliSeconds(20);
 
        bool reached = false;
        bool movementTimedOut = false;
 
        while (not shouldSkillTerminate() and not reached and not movementTimedOut)
        {
            const auto currentAngles = handUnit->getCurrentJointValues();
 
            // check if target reached
            reached = true;
            for (const auto& [jointName, targetAngle] : targetAngles)
            {
                if (std::abs(targetAngle - currentAngles.at(jointName)) > getAccuracy(jointName))
                {
                    reached = false;
                    ARMARX_VERBOSE
                        << deactivateSpam(1) << "Delta for joint '" << jointName
                        << "' is: " << std::abs(targetAngle - currentAngles.at(jointName)) << " > "
                        << getAccuracy(jointName) << "(accuracy)";
                    break;
                }
            }
 
            // check if movement timed out
            if (movementTimeout and (DateTime::Now() - pastJointAngles.time).toSecondsDouble() >=
                                        movementTimeout.value())
            {
                movementTimedOut = true;
                for (const auto& [jointName, targetAngle] : targetAngles)
                {
                    if (std::abs(currentAngles.at(jointName) -
                                 pastJointAngles.angles.at(jointName)) > getAccuracy(jointName))
                    {
                        movementTimedOut = false;
                        ARMARX_VERBOSE
                            << deactivateSpam(1) << "Joint '" << jointName << "' still moving: "
                            << std::abs(currentAngles.at(jointName) -
                                        pastJointAngles.angles.at(jointName))
                            << " > " << getAccuracy(jointName) << "(accuracy)"
                            << "in the last " << movementTimeout.value() << "s (movement timeout)";
                        break;
                    }
                }
                pastJointAngles = {currentAngles, DateTime::Now()};
            }
 
            clock.waitFor(sleepTime);
        }
 
        if (reached)
        {
            ARMARX_INFO << "Target position reached!";
        }
        else if (movementTimedOut)
        {
            ARMARX_INFO << "Movement timed out! Joints stopped moving. Not waiting anymore until "
                           "target postion is reached";
        }
        else if (shouldSkillTerminate())
        {
            ARMARX_IMPORTANT
                << "Skill aborted. Not waiting anymore until target position is reached";
            return MakeAbortedResult();
        }
 
        return MakeSucceededResult();
    }
 
    float
    ShapeHand::getAccuracy(const std::string& jointName)
    {
        const auto accuracy = getParameters().waitUntilFinished->accuracy;
        const auto accuracyOverride = getParameters().waitUntilFinished->accuracyOverride;
 
        return accuracyOverride.count(jointName) > 0 ? accuracyOverride.at(jointName) : accuracy;
    }
 
    OpenHand::OpenHand() : armarx::skills::SpecializedSkill<ParamType>(GetSkillDescription())
    {
    }
 
    ::armarx::skills::Skill::MainResult
    OpenHand::main()
    {
        // call ShapeHand-skill
        ::armarx::skills::SkillID id;
        id.skillName = constants::SHAPE_HAND_SKILL_NAME;
        id.providerId = getSkillId().providerId;
 
        arondto::ShapeHandParams params;
        params.hand = getParameters().hand;
        params.shapeName = "Open";
        params.waitUntilFinished = getParameters().waitUntilFinished;
 
        return {(*callSubskill(::armarx::skills::SkillProxy(manager, id), params.toAron())).status};
    }
 
    CloseHand::CloseHand() : armarx::skills::SpecializedSkill<ParamType>(GetSkillDescription())
    {
    }
 
    ::armarx::skills::Skill::MainResult
    CloseHand::main()
    {
        // call ShapeHand-skill
        ::armarx::skills::SkillID id;
        id.skillName = constants::SHAPE_HAND_SKILL_NAME;
        id.providerId = getSkillId().providerId;
 
        arondto::ShapeHandParams params;
        params.hand = getParameters().hand;
        params.shapeName = "Close";
        params.waitUntilFinished = getParameters().waitUntilFinished;
 
        return {(*callSubskill(::armarx::skills::SkillProxy(manager, id), params.toAron())).status};
    }
 
} // namespace armarx::control::skills::skills