GuidingWithCollAvoidance.cpp

Go to the documentation of this file.

#include "GuidingWithCollAvoidance.h"
 
#include <SimoxUtility/json/json.hpp>
 
#include <armarx/control/common/aron_conversions.h>
#include <armarx/control/njoint_controller/controller_descriptions.h>
#include <armarx/control/skills/skills/ShapeHand.h>
 
namespace armarx::control::skills::skills
{
 
    GuidingWithCollAvoidance::GuidingWithCollAvoidance(const Services& srv) :
        ::armarx::skills::SimpleSpecializedSkill<ParamType>(GetSkillDescription()), srv_(srv)
    {
    }
 
    ::armarx::skills::Skill::ExitResult
    GuidingWithCollAvoidance::exit(const SpecializedExitInput& in)
    {
 
        return {::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    ::armarx::skills::Skill::MainResult
    GuidingWithCollAvoidance::main(const SpecializedMainInput& in)
    {
        ARMARX_INFO << "in main of execute trajectory";
        ARMARX_TRACE;
 
        // create controller
        std::string controllerNamePrefix = "Guiding";
        const auto controllerType = armarx::control::common::ControllerType::TSMixImpVelCol;
        const std::string controllerClassName = common::ControllerTypeNames.to_name(controllerType);
        auto builder = srv_.controlComponentPluginUser->createControllerBuilder<controllerType>();
 
        std::string suffix;
        if (in.parameters.mode == arondto::GuidingMode::left_arm)
        {
            suffix = "_left";
        }
        else if (in.parameters.mode == arondto::GuidingMode::right_arm)
        {
            suffix = "_right";
        }
        else
        {
            suffix = "";
        }
 
        auto const configFileName = "/default_a7" + suffix + "_zero_torque.json";
        const armarx::PackagePath configPath(
            "armarx_control", "controller_config/" + controllerClassName + configFileName);
        auto ctrl = builder.createGetTSComplianceCtrl(
            controllerNamePrefix, nullptr, configPath.toSystemPath());
 
        ARMARX_CHECK_NOT_NULL(ctrl);
        throwIfSkillShouldTerminate();
        while (not shouldSkillTerminate())
        {
            armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(100));
        }
        ctrl->deactivateAndDeleteController();
 
        return MakeSucceededResult();
    }
 
 
} // namespace armarx::control::skills::skills