HomePose.cpp

Go to the documentation of this file.

#include "HomePose.h"
 
#include <RobotAPI/libraries/skills/provider/SpecializedSkillProxy.h>
 
#include <armarx/control/skills/aron/MoveJointsToNamedConfigurationParams.aron.generated.h>
#include <armarx/control/skills/aron/ShapeHandParams.aron.generated.h>
#include <armarx/control/skills/constants/constants.h>
#include <armarx/control/skills/skills/param_conversions.h>
 
namespace armarx::control::skills::skills::meta
{
 
    HomePose::HomePose() : SimpleSpecializedSkill(GetSkillDescription())
    {
    }
 
    armarx::skills::SkillDescription
    HomePose::GetSkillDescription()
    {
        ParamType defaultParams;
 
        std::stringstream desc;
        desc << "Move all joints to a natural idle position that also allows "
                "safe movement and safe bus crashes."
             << "\n\n"
             << "The `enable*` parameters can be used to enable/disable movement for different "
                "body parts of the robot.";
 
        return armarx::skills::SkillDescription{
            .skillId = {.skillName = constants::HOME_POSE_SKILL_NAME},
            .description = desc.str(),
            .rootProfileDefaults = defaultParams.toAron(),
            .timeout = ::armarx::Duration::Seconds(30),
            .parametersType = ParamType::ToAronType(),
        };
    }
 
    ::armarx::skills::Skill::MainResult
    HomePose::main(const SpecializedMainInput& in)
    {
        auto moveToConfiguration = armarx::skills::SpecializedSkillProxy<
            armarx::control::skills::arondto::MoveJointsToNamedConfigurationParams>(
            this->manager,
            ::armarx::skills::SkillID{
                .providerId = getSkillId().providerId,
                .skillName = constants::MOVE_JOINTS_TO_NAMED_CONFIGURATION_SKILL_NAME,
            });
 
        auto openHand = armarx::skills::SpecializedSkillProxy<
            armarx::control::skills::arondto::ShapeHandParams>(
            this->manager,
            ::armarx::skills::SkillID{
                .providerId = getSkillId().providerId,
                .skillName = constants::SHAPE_HAND_SKILL_NAME,
            });
 
        auto subParams = moveToConfiguration.getRootProfileParameters();
        subParams.configuration = "Home";
 
        subParams.enableHead =
            true; // should be via gaze controller, but due to dependency chain, this skill would have to be moved
        subParams.enableLeftHand = false; // via hand unit
        subParams.enableRightHand = false; // via hand unit
 
        copy_params(in.parameters, subParams);
 
        auto openHandParams = openHand.getRootProfileParameters();
        openHandParams.shapeName = "Open";
 
        auto openHandParamsLeft = openHandParams;
        openHandParamsLeft.hand = armarx::control::skills::arondto::Hand::Left;
 
        auto openHandParamsRight = openHandParams;
        openHandParamsLeft.hand = armarx::control::skills::arondto::Hand::Right;
 
        this->callSubskill(openHand, openHandParamsLeft.toAron());
        this->callSubskill(openHand, openHandParamsRight.toAron());
        this->callSubskill(moveToConfiguration, subParams.toAron());
 
        return MakeSucceededResult();
    }
 
} // namespace armarx::control::skills::skills::meta