MoveJointsToPosition.cpp

Go to the documentation of this file.

#include "MoveJointsToPosition.h"
 
#include "util/JointControlUtil.h"
 
namespace armarx::skills
{
    namespace
    {
        joint_control::arondto::MoveJointsToPositionAcceptedType GetDefaultParameterization()
        {
            joint_control::arondto::MoveJointsToPositionAcceptedType ret;
            ret.jointTargetTolerance = 0.05;
            return ret;
        }
    }
 
    SkillDescription MoveJointsToPosition::Description = skills::SkillDescription{
        "MoveJointsToPosition", "Move the joints to position",
        {}, armarx::Duration::MilliSeconds(5000),
        joint_control::arondto::MoveJointsToPositionAcceptedType::ToAronType(),
        GetDefaultParameterization().toAron()
    };
 
    MoveJointsToPosition::MoveJointsToPosition(armem::client::MemoryNameSystem& mns, JointControlSkillContext& context) :
        JointControlSkill(mns, context),
        mixin::RobotReadingSkillMixin(mns),
        PeriodicSimpleSpecializedSkill<joint_control::arondto::MoveJointsToPositionAcceptedType>(Description, armarx::core::time::Frequency::Hertz(10))
    {
    }
 
    void MoveJointsToPosition::reset()
    {
        already_running = false;
    }
 
    Skill::InitResult MoveJointsToPosition::init(const SpecializedInitInput &in)
    {
        robotReader.connect();
        return {.status = TerminatedSkillStatus::Succeeded};
    }
 
    PeriodicSkill::StepResult MoveJointsToPosition::step(const SpecializedMainInput& in)
    {
        auto robot = robotReader.queryState({in.parameters.robotName}, armem::Time::Now());
        if (!robot)
        {
            ARMARX_WARNING << "Unable to get robot from robotstatememory. Abort.";
            return {ActiveOrTerminatedSkillStatus::Failed, nullptr};
        }
 
        if ((armem::Time::Now() - robot->timestamp).toMilliSeconds() > 300)
        {
            ARMARX_WARNING << deactivateSpam(1) << "Robot from Memory is too old. Try again.";
            return {ActiveOrTerminatedSkillStatus::Running, nullptr};
        }
 
        std::map<std::string, float> filteredTargets;
        auto currentValues = robot->jointMap;
        for (const auto& [key, val] : in.parameters.targetJointMap)
        {
            if (const auto& it = currentValues.find(key); it != currentValues.end())
            {
                ARMARX_INFO << deactivateSpam(1) << "Current value of " << key << " is " << it->second << " and it will be set to " << val;
                filteredTargets.insert({key, val});
            }
        }
 
        if (filteredTargets.size() == 0)
        {
            return {ActiveOrTerminatedSkillStatus::Failed, nullptr};
        }
 
        auto currentJointValueTolerance = in.parameters.jointTargetTolerance;
        ARMARX_INFO << deactivateSpam(1) << "Joint target tolerance: " << currentJointValueTolerance;
 
        auto currentMaxDelta = std::numeric_limits<float>::max();
 
        robot = robotReader.queryState({in.parameters.robotName}, armem::Time::Now());
 
        currentValues = robot->jointMap;
        float maxDeltaTmp = 0;
        for (const auto& [key, value] : filteredTargets)
        {
            const float delta = fabs((value - currentValues.at(key)));
            maxDeltaTmp = std::max(delta, maxDeltaTmp);
        }
        currentMaxDelta = maxDeltaTmp;
 
        ARMARX_IMPORTANT << deactivateSpam(1) << "Current joint values are: \n" << currentValues;
        ARMARX_IMPORTANT << deactivateSpam(1) << "MaxDelta is: " << currentMaxDelta;
 
        if (currentMaxDelta <= currentJointValueTolerance)
        {
            return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
        }
 
        if (!already_running)
        {
            already_running = true;
 
            NameControlModeMap controlModes;
            for (const auto& p : filteredTargets)
            {
                controlModes[p.first] = ePositionControl;
            }
            context.kinematicUnitPrx->switchControlMode(controlModes);
 
            context.kinematicUnitPrx->setJointAngles(filteredTargets);
        }
        return {ActiveOrTerminatedSkillStatus::Running, nullptr};
    }
 
    Skill::ExitResult MoveJointsToPosition::exit(const SpecializedExitInput &in)
    {
        std::vector<std::string> joints;
        std::for_each(in.parameters.targetJointMap.begin(), in.parameters.targetJointMap.end(), [&joints](const std::pair<std::string, float>& p) { joints.push_back(p.first); });
 
        stopJointMovement(joints);
 
        return {.status = TerminatedSkillStatus::Succeeded};
    }
}