ZeroPose.cpp

Go to the documentation of this file.

#include "ZeroPose.h"
 
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
#include <RobotAPI/libraries/skills/provider/SpecializedSkillProxy.h>
 
#include <armarx/control/skills/aron/MoveJointsToNamedConfigurationParams.aron.generated.h>
#include <armarx/control/skills/constants/constants.h>
#include <armarx/control/skills/skills/param_conversions.h>
 
namespace armarx::control::skills::skills::meta
{
 
    ZeroPose::ZeroPose() : SimpleSpecializedSkill(GetSkillDescription())
    {
    }
 
    armarx::skills::SkillDescription
    ZeroPose::GetSkillDescription()
    {
        ParamType defaultParams;
 
        std::stringstream desc;
        desc << "Move all joints to zero (except joints which would cause a self collision)."
             << "\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::ZERO_POSE_SKILL_NAME},
            .description = desc.str(),
            .rootProfileDefaults = defaultParams.toAron(),
            .timeout = ::armarx::Duration::Seconds(30),
            .parametersType = ParamType::ToAronType(),
        };
    }
 
    ::armarx::skills::Skill::MainResult
    ZeroPose::main(const SpecializedMainInput& in)
    {
 
        ::armarx::skills::SkillID skillId{
            .providerId = getSkillId().providerId,
            .skillName = constants::MOVE_JOINTS_TO_NAMED_CONFIGURATION_SKILL_NAME,
        };
 
        using Parameters = arondto::MoveJointsToNamedConfigurationParams;
 
        auto status = callSubskill<Parameters>(skillId,
                                               [&](Parameters& params)
                                               {
                                                   params.configuration = "Zero";
                                                   copy_params(in.parameters, params);
                                               });
 
        if (armarx::skills::skillExecutionFailed(status))
        {
            return MakeFailedResult();
        }
 
        return MakeSucceededResult();
    }
 
 
} // namespace armarx::control::skills::skills::meta