ShapeHand.cpp

Go to the documentation of this file.

#include "ShapeHand.h"
 
#include <cstdlib>
#include <string>
 
#include <range/v3/algorithm/contains.hpp>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
 
#include <RobotAPI/interface/core/NameValueMap.h>
#include <RobotAPI/libraries/skills/core/Skill.h>
#include <RobotAPI/libraries/skills/core/SkillID.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
#include <RobotAPI/libraries/skills/provider/SpecializedSkill.h>
 
#include <armarx/control/skills/aron/HandParam.aron.generated.h>
#include <armarx/control/skills/aron/ShapeHandParams.aron.generated.h>
#include <armarx/control/skills/constants/constants.h>
#include <armarx/control/skills/skills/HandUnitServices.h>
 
namespace armarx::control::skills::skills
{
    namespace
    {
        NameValueMap
        removePrefixFromKeys(const NameValueMap& map, const std::string& prefix)
        {
            NameValueMap result;
            for (const auto& [key, value] : map)
            {
                if (simox::alg::starts_with(key, prefix))
                {
                    result[key.substr(prefix.size())] = value;
                }
                else
                {
                    result[key] = value;
                }
            }
            return result;
        }
    } // namespace
 
    ShapeHand::ShapeHand(const HandUnitServices& srv, Properties p) :
        ::armarx::skills::SpecializedSkill<ParamType>(GetSkillDescription()), srv_(srv), p_(p)
    {
    }
 
    ::armarx::skills::Skill::MainResult
    ShapeHand::main()
    {
        const arondto::ShapeHandParams params = getParameters();
 
        // verify exactly one of the parameters 'shapeName' and 'jointAngles' is set
        if ((not params.shapeName) and (not params.jointAngles))
        {
            ARMARX_ERROR << "Exactly one of the parameters 'shapeName' and 'jointAngles' must be "
                            "set. However, neither has been set.";
            return MakeFailedResult();
        }
 
        if (params.shapeName and params.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(params.hand);
 
        if (params.shapeName)
        {
            const auto shape = params.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 ranges::contains(defShapes, shape))
            // {
            //     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 (params.waitUntilFinished)
            {
                return waitUntilFinished(handUnit->getShapeJointValues(shape));
            }
        }
 
        else // params.jointAngles
        {
            const auto jointAngles = params.jointAngles.value();
 
            ARMARX_INFO << "Setting joint angles of hand '" << handUnit->getHandName()
                        << "' to: " << jointAngles;
 
            handUnit->setJointAngles(jointAngles);
 
            if (params.waitUntilFinished)
            {
                return waitUntilFinished(jointAngles);
            }
        }
        return MakeSucceededResult();
    }
 
    armarx::skills::Skill::MainResult
    ShapeHand::waitUntilFinished(const NameValueMap& targetAnglesRaw)
    {
        if (p_.disableWaitUntilFinished)
        {
            return MakeSucceededResult();
        }
 
        const arondto::ShapeHandParams params = getParameters();
 
        auto& handUnit = srv_.getHandUnit(params.hand);
        const auto movementTimeout = params.waitUntilFinished->movementTimeout;
 
        ARMARX_INFO << "Waiting until target position is reached"
                    << (movementTimeout ? " or movement times out" : "");
 
        const std::string handPrefix = params.hand == ::armarx::control::skills::arondto::Hand::Left
                                           ? "LeftHand"
                                           : "RightHand";
 
        const NameValueMap targetAngles = removePrefixFromKeys(targetAnglesRaw, handPrefix);
 
        struct
        {
            NameValueMap angles;
            ::armarx::DateTime time;
        } pastJointAngles{.angles = handUnit->getCurrentJointValues(), .time = 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)
        {
            // get current angles without hand prefix, e.g. "LeftHand" (depending on simulation or real robot, this might be inconsistent)
            const auto currentAngles =
                removePrefixFromKeys(handUnit->getCurrentJointValues(), handPrefix);
 
            // check if target reached
            reached = true;
            for (const auto& [jointName, targetAngle] : targetAngles)
            {
                ARMARX_CHECK(currentAngles.count(jointName) > 0)
                    << "Joint '" << jointName << "' not found in current joint angles.";
                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 = {.angles = currentAngles, .time = 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 arondto::ShapeHandParams params = getParameters();
 
        const auto accuracy = params.waitUntilFinished->accuracy;
        const auto accuracyOverride = params.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;
 
        using Parameters = arondto::ShapeHandParams;
        const auto inParams = getParameters();
 
 
        auto status = callSubskill<Parameters>(id,
                                               [inParams](Parameters& params)
                                               {
                                                   params.hand = inParams.hand;
                                                   params.shapeName = "Open";
                                                   params.waitUntilFinished =
                                                       inParams.waitUntilFinished;
                                               });
 
        if (armarx::skills::skillExecutionFailed(status))
        {
            return MakeFailedResult();
        }
 
        return MakeSucceededResult();
    }
 
    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;
 
        using Parameters = arondto::ShapeHandParams;
 
        const auto inParams = getParameters();
 
        auto status = callSubskill<Parameters>(id,
                                               [&inParams](Parameters& params)
                                               {
                                                   params.hand = inParams.hand;
                                                   params.shapeName = "Close";
                                                   params.waitUntilFinished =
                                                       params.waitUntilFinished;
                                               });
 
        if (armarx::skills::skillExecutionFailed(status))
        {
            return MakeFailedResult();
        }
 
        return MakeSucceededResult();
    }
 
} // namespace armarx::control::skills::skills