SwitchCoordinationMode.cpp

Go to the documentation of this file.

#include "SwitchCoordinationMode.h"
 
#include <armarx/control/common/common.aron.generated.h>
#include <armarx/control/common/control_law/aron/TaskspaceControllerConfig.aron.generated.h>
#include <armarx/control/common/coordination/aron/CoordinationConfig.aron.generated.h>
#include <armarx/control/common/type.h>
#include <armarx/control/common/utils.h>
#include <armarx/control/njoint_controller/controller_descriptions.h>
 
namespace armarx::control::skills::skills
{
    using namespace armarx::control::common;
 
    SwitchCoordinationMode::SwitchCoordinationMode(const Services& srv) :
        ::armarx::skills::SimpleSpecializedSkill<ParamType>(GetSkillDescription()), srv_(srv)
    {
    }
 
    ::armarx::skills::Skill::ExitResult
    SwitchCoordinationMode::exit(const SpecializedExitInput& in)
    {
 
        return {::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    ::armarx::skills::Skill::MainResult
    SwitchCoordinationMode::main(const SpecializedMainInput& in)
    {
        ARMARX_INFO << "Switching coordination mode";
        ARMARX_TRACE;
 
        const auto& robotUnit =
            srv_.controlComponentPluginUser->getRobotUnitPlugin().getRobotUnit();
 
        auto names = robotUnit->getActivatedNJointControllerNames();
        ARMARX_INFO << "Activated controllers: \n" << common::sVecToString(names);
 
        auto candidateCtrlNames = filterControllerNames(names, in.parameters.controllerNamePattern);
        
        ARMARX_TRACE;
        if (candidateCtrlNames.size() > 1)
        {
            ARMARX_WARNING << "There are more than one controllers matching your name pattern: "
                           << common::sVecToString(candidateCtrlNames);
            return MakeFailedResult();
        }
        
        if (candidateCtrlNames.empty())
        {
            ARMARX_WARNING << "No controllers matching your pattern "
                           << in.parameters.controllerNamePattern
                           << " found. Available: " << common::sVecToString(names);
            return MakeFailedResult();
        }
 
        const std::string candidateName = candidateCtrlNames[0];
        ARMARX_INFO << "Found matching controller " << candidateName;
        const bool enable = in.parameters.enable;
 
        auto controller = Ice::checkedCast<armarx::control::TSColAvoidCtrlInterfacePrx>(
            robotUnit->getNJointController(candidateName));
 
        if (enable)
        {
            auto cfgCoord = coordination::arondto::SyncModeConfig();
            {
                const armarx::PackagePath configPath(in.parameters.configPackageName,
                                                     in.parameters.configRelPath);
                auto filename = configPath.toSystemPath();
                ARMARX_INFO << "using coordination config: " << filename;
                ARMARX_CHECK(std::filesystem::is_regular_file(filename))
                    << filename << " does not exists";
 
                std::ifstream ifs{filename};
 
                nlohmann::json jsonConfig;
                ifs >> jsonConfig;
 
                armarx::aron::data::reader::NlohmannJSONReaderWithoutTypeCheck reader;
                cfgCoord.read(reader, jsonConfig);
            }
 
            auto cfgDTO = controller->getConfig();
            auto cfg = control_law::arondto::TSMixImpVelColConfigDict();
            cfg.fromAron(cfgDTO);
            auto& desiredPose = cfgCoord.desiredPose;
            auto& poseL = cfg.limbs.at("LeftArm").desiredPose;
            auto& poseR = cfg.limbs.at("RightArm").desiredPose;
            if (cfgCoord.leadershipMode == coordination::arondto::LeadershipMode::object_as_leader)
            {
                desiredPose = poseL;
                desiredPose.block<3, 1>(0, 3) = 0.5 * (getPos(poseL) + getPos(poseR));
            }
            else if (cfgCoord.leadershipMode ==
                     coordination::arondto::LeadershipMode::left_as_leader)
            {
                desiredPose = poseL;
            }
            else if (cfgCoord.leadershipMode ==
                     coordination::arondto::LeadershipMode::right_as_leader)
            {
                desiredPose = poseR;
            }
            if (cfgCoord.followerFrameMode == armarx::control::common::arondto::PoseFrameMode::root)
            {
            }
            else
            {
                ARMARX_WARNING
                    << "The pose target computation only support 'root' mode. Please fix the code "
                       "before. Now the followerFrameMode is set to 'PoseFrameMode::root'.";
                cfgCoord.followerFrameMode = armarx::control::common::arondto::PoseFrameMode::root;
            }
            ARMARX_INFO << "Coordinator: setting leader " << VAROUT(desiredPose);
            for (auto& pair : cfg.limbs)
            {
                ARMARX_INFO << "Disableing force torque guard for " << pair.first;
                controller->enableSafeGuardForceTorque(pair.first, false, false);
            }
            controller->useCoordinator("", cfgCoord.toAronDTO());
        }
        else
        {
            controller->disableCoordinator();
        }
        return MakeSucceededResult();
    }
 
    std::vector<std::string>
    SwitchCoordinationMode::filterControllerNames(const std::vector<std::string>& controllerNames,
                                                  const std::vector<std::string>& patterns)
    {
        if (patterns.empty())
        {
            return controllerNames;
        }
 
        std::vector<std::string> matches;
        // No reserve() needed here since we expect very few results.
 
        for (const auto& name : controllerNames)
        {
            bool allPatternsFound = true;
 
            for (const auto& pattern : patterns)
            {
                if (name.find(pattern) == std::string::npos)
                {
                    allPatternsFound = false;
                    break;
                }
            }
 
            if (allPatternsFound)
            {
                matches.push_back(name);
            }
        }
 
        return matches;
    }
 
 
} // namespace armarx::control::skills::skills