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;
        }
    } // namespace
 
    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};
    }
} // namespace armarx::skills