RetrieveHand.cpp

Go to the documentation of this file.

#include "RetrieveHand.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/aron/common/aron_conversions/packagepath.cpp>
 
#include <armarx/control/deprecated_njoint_mp_controller/task_space/ControllerInterface.h>
#include <armarx/control/retrieve_hand/constants/constants.h>
 
namespace armarx::control::retrieve_hand::skills
{
 
    ::armarx::skills::SkillDescription
    RetrieveHand::GetSkillDescription()
    {
        ParamType defaults;
        defaults.targetPose.header.agent = "Armar7";
        defaults.targetPose.header.frame = "root";
        defaults.targetPose.position = Eigen::Matrix<float, 3, 1>{-234.0f, 284.06f, 1103.0f};
        defaults.targetPose.orientation =
            Eigen::Quaternion<float>{0.7640368, -0.63349, 0.06744281, -0.10187};
 
        armarx::arondto::PackagePath& packagePath = defaults.fileNames.emplace_back();
        packagePath.package = "armar7_motions";
        packagePath.path = "tcp_trajectories/dmp/default_left.csv";
 
        // ARMARX_INFO << VAROUT(defaultParams.targetPose);
        // ARMARX_INFO << VAROUT(defaultParams.targetPose.position) << VAROUT(defaultParams.targetPose.orientation);
        // ARMARX_INFO << VAROUT(defaultParams.targetPose.header.frame) << VAROUT(defaultParams.targetPose.header.agent);
 
        // defaultParams.exampleStringParam = "exampleDefaultValue";
 
        auto skillId = armarx::skills::SkillID{
            .skillName = armarx::control::retrieve_hand::constants::SKILL_NAME};
 
        return ::armarx::skills::SkillDescription{.skillId = skillId,
                                                  .description =
                                                      "TODO: Description of skill RetrieveHand.",
                                                  .rootProfileDefaults = defaults.toAron(),
                                                  .timeout = ::armarx::Duration::MilliSeconds(1000),
                                                  .parametersType = ParamType::ToAronType()};
    }
 
    RetrieveHand::RetrieveHand(const Remote& r) : Base(GetSkillDescription()), remote(r)
 
    {
    }
 
    /*
    ::armarx::skills::Skill::InitResult
    RetrieveHand::init(const SpecializedInitInput&)
    {
        return InitResult{.status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
    */
 
 
    ::armarx::skills::Skill::MainResult
    RetrieveHand::main(const SpecializedMainInput& in)
    {
        // Enter main code of the skill here.
        core::RetrieveHand::Remote rem{.memoryNameSystem = remote.memoryNameSystem,
                                       .robotUnit = remote.robotUnit};
 
        armarx::FramedPose targetPose{in.parameters.targetPose.position,
                                      in.parameters.targetPose.orientation,
                                      in.parameters.targetPose.header.frame,
                                      in.parameters.targetPose.header.agent};
 
        std::vector<std::string> fileNames;
        for (const auto& aronPackagePath : in.parameters.fileNames)
        {
            armarx::PackagePath packagePath = fromAron<armarx::PackagePath>(aronPackagePath);
            fileNames.push_back(packagePath.toSystemPath().string());
        }
 
        core::RetrieveHand::Properties props{
            .robotName = in.parameters.robotName,
            .robotNodeSet = in.parameters.robotNodeSet,
            .timeDurationInSec = in.parameters.timeDurationInSec,
            .targetPose = targetPose,
            .kpLinear = in.parameters.kpLinear,
            .kpAngular = in.parameters.kpAngular,
            .kdLinear = in.parameters.kdLinear,
            .kdAngular = in.parameters.kdAngular,
            .fileNames = fileNames,
        };
 
 
        impl = std::make_unique<core::RetrieveHand>(rem, props);
 
        auto res = impl->execute();
        if (res)
        {
            return MakeSucceededResult();
        }
        return MakeFailedResult();
    }
 
    void
    RetrieveHand::onStopRequested()
    {
        ARMARX_INFO << "requesting to stop TSVMP controller ";
        if (impl)
        {
            impl->stop();
        }
    }
 
    /*
    ::armarx::skills::Skill::ExitResult
    RetrieveHand::exit(const SpecializedExitInput&)
    {
        return ExitResult{.status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
    */
 
} // namespace armarx::control::retrieve_hand::skills