HomePose.cpp

Go to the documentation of this file.

#include "HomePose.h"
 
#include <sstream>
#include <string>
#include <vector>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Duration.h>
 
#include <RobotAPI/libraries/skills/core/Skill.h>
#include <RobotAPI/libraries/skills/core/SkillDescription.h>
#include <RobotAPI/libraries/skills/core/SkillExecutionHandle.h>
#include <RobotAPI/libraries/skills/core/SkillExecutionID.h>
#include <RobotAPI/libraries/skills/core/SkillID.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
#include <RobotAPI/libraries/skills/provider/SpecializedSkillProxy.h>
 
#include <armarx/control/skills/aron/HandParam.aron.generated.h>
#include <armarx/control/skills/aron/MoveJointsToNamedConfigurationParams.aron.generated.h>
#include <armarx/control/skills/aron/NamedConfigurationParams.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(const std::string& homePoseConfiguration) :
        SimpleSpecializedSkill(GetSkillDescription()), homePoseConfiguration_(homePoseConfiguration)
    {
    }
 
    armarx::skills::SkillDescription
    HomePose::GetSkillDescription()
    {
        ParamType defaultParams;
 
        defaultParams.disabledJoints = {"LeftHandFingers",
                                        "RightHandFingers",
                                        "LeftHandIndex",
                                        "RightHandIndex",
                                        "LeftHandThumbCircumduction",
                                        "RightHandThumbCircumduction",
                                        "LeftHandThumbFlexion",
                                        "RightHandThumbFlexion"};
 
        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)
    {
        const ::armarx::skills::SkillID moveToConfigurationSkillId{
            .providerId = getSkillId().providerId,
            .skillName = constants::MOVE_JOINTS_TO_NAMED_CONFIGURATION_SKILL_NAME,
        };
 
        const ::armarx::skills::SkillID openHandSkillId{
            .providerId = getSkillId().providerId,
            .skillName = constants::SHAPE_HAND_SKILL_NAME,
        };
        std::unique_ptr<armarx::skills::SkillExecutionHandle> handleLeftHand;
        if (in.parameters.enableLeftHand)
        {
            handleLeftHand = [&]
            {
                using Parameters = arondto::ShapeHandParams;
 
                return callSubskillAsync<Parameters>(
                    openHandSkillId,
                    [](Parameters& params)
                    {
                        params.shapeName = "Open";
                        params.hand = armarx::control::skills::arondto::Hand::Left;
                    });
            }();
        }
 
        std::unique_ptr<armarx::skills::SkillExecutionHandle> handleRightHand;
        if (in.parameters.enableRightHand)
        {
            handleRightHand = [&]
            {
                using Parameters = arondto::ShapeHandParams;
 
                return callSubskillAsync<Parameters>(
                    openHandSkillId,
                    [](Parameters& params)
                    {
                        params.shapeName = "Open";
                        params.hand = armarx::control::skills::arondto::Hand::Right;
                    });
            }();
        }
 
        const auto handleHomePose = [&]
        {
            using Parameters =
                armarx::control::skills::arondto::MoveJointsToNamedConfigurationParams;
 
            return callSubskillAsync<Parameters>(
                moveToConfigurationSkillId,
                [&](Parameters& subParams)
                {
                    subParams.configuration = homePoseConfiguration_;
 
                    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);
                });
        }();
 
        ARMARX_INFO << "Waiting for subskills to join";
 
        armarx::skills::util::JoinAllResult result;
        if (in.parameters.enableLeftHand and in.parameters.enableRightHand)
        {
            result =
                armarx::skills::util::joinAllAny(handleLeftHand, handleRightHand, handleHomePose);
        }
        if (in.parameters.enableLeftHand)
        {
            result = armarx::skills::util::joinAllAny(handleLeftHand, handleHomePose);
        }
        else if (in.parameters.enableRightHand)
        {
            result = armarx::skills::util::joinAllAny(handleRightHand, handleHomePose);
        }
        else
        {
            result = armarx::skills::util::joinAllAny(handleHomePose);
        }
        ARMARX_INFO << "All subskills have been joined.";
 
        if (not result)
        {
            ARMARX_WARNING << "The skills " << result.failedSkillIDs() << " failed";
            return MakeFailedResult();
        }
 
 
        return MakeSucceededResult();
    }
 
 
} // namespace armarx::control::skills::skills::meta