ZeroTorque.cpp

Go to the documentation of this file.

#include "ZeroTorque.h"
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <RobotAPI/interface/units/RobotUnit/NjointZeroTorqueController.h>
 
namespace armarx::control::skills::skills
{
    armarx::skills::SkillDescription
    ZeroTorque::GetSkillDescription()
    {
        ParamType defaults;
        defaults.nodeSetName = "RightArm";
        return ::armarx::skills::SkillDescription{
            .skillId = {.skillName = "ZeroTorque"},
            .description = "Enables zero torque mode for the runtime",
            .rootProfileDefaults = defaults.toAron(),
            .timeout = ::armarx::Duration::Minutes(10),
            .parametersType = ParamType::ToAronType(),
        };
    }
 
    ZeroTorque::ZeroTorque(const config::ZeroTorque& r) :
        ::armarx::skills::SimpleSpecializedSkill<params::ZeroTorque>(GetSkillDescription()),
        config_(r)
    {
    }
 
    armarx::skills::Skill::MainResult
    ZeroTorque::main(const SpecializedMainInput& in)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(controller_);
        controller_->activateController();
        ARMARX_INFO << shouldSkillTerminate();
        while (not shouldSkillTerminate())
        {
            ARMARX_INFO << "Waiting";
            armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(100));
        }
        controller_->deactivateController();
        return {.status = armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    void
    ZeroTorque::connectTo(const ZeroTorque::Context& context)
    {
        context_ = context;
    }
 
    armarx::skills::Skill::InitResult
    ZeroTorque::init(
        const armarx::skills::SimpleSpecializedSkill<params::ZeroTorque>::SpecializedInitInput& in)
    {
        ARMARX_TRACE;
        NJointZeroTorqueControllerConfigPtr config(new NJointZeroTorqueControllerConfig());
        config->maxTorque = 0;
        controllerName_ = "ControlSkills_ZeroTorque_" + in.parameters.nodeSetName;
        ARMARX_CHECK_EXPRESSION(context_.localRobot_->hasRobotNodeSet(in.parameters.nodeSetName))
            << "RNS " << in.parameters.nodeSetName << " is unknown";
        auto nodeSet = context_.localRobot_->getRobotNodeSet(in.parameters.nodeSetName);
        for (auto& name : nodeSet->getNodeNames())
        {
            config->jointNames.push_back(name);
        }
        ARMARX_CHECK_GREATER(config->jointNames.size(), 0) << "RNS is empty!";
        controller_ = armarx::NJointZeroTorqueControllerInterfacePrx::checkedCast(
            context_.robotUnitPlugin_->getRobotUnit()->getNJointController(controllerName_));
        if (not controller_)
        {
            controller_ = armarx::NJointZeroTorqueControllerInterfacePrx::checkedCast(
                context_.robotUnitPlugin_->createNJointController(
                    armarx::RobotUnitControllerNames::NJointZeroTorqueController,
                    controllerName_,
                    config));
        }
        return {.status = armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    armarx::skills::Skill::ExitResult
    ZeroTorque::exit(
        const armarx::skills::SimpleSpecializedSkill<params::ZeroTorque>::SpecializedExitInput& in)
    {
        ARMARX_TRACE;
        context_.robotUnitPlugin_->getRobotUnit()->deactivateAndDeleteNJointController(
            controllerName_);
        //        controller_->deactivateAndDeleteController();
        return {.status = armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
} // namespace armarx::control::skills::skills