BringObjectToLocation.cpp

Go to the documentation of this file.

#include "BringObjectToLocation.h"
 
#include <RobotAPI/libraries/skills/core/SkillProxy.h>
 
#include <RobotSkillTemplates/libraries/skill_bring_object/aron/BringObjectToLandmarkAcceptedType.aron.generated.h>
 
namespace armarx::skills
{
    namespace
    {
        arondto::BringObjectToLandmarkAcceptedType
        GetDefaultParameterization()
        {
            arondto::BringObjectToLandmarkAcceptedType ret;
            ret.robotName = "Armar3";
            ret.graph = "KitchenKK";
            ret.objectEntityId = "Kitchen/green-cup/0";
            ret.landmarkFrom = "sideboard";
            ret.landmarkTo = "sink";
            return ret;
        }
    } // namespace
 
    SkillDescription BringObjectToLocation::Description = skills::SkillDescription{
        .skillId = {.skillName = "BringObjectToLocation"},
        .description = "Bring an object to some location",
        .rootProfileDefaults = GetDefaultParameterization().toAron(),
        .timeout = armarx::Duration::MilliSeconds(60000),
        .parametersType = arondto::BringObjectToLandmarkAcceptedType::ToAronType(),
    };
 
    BringObjectToLocation::BringObjectToLocation(const BringObjectSkillContext& context) :
        BringObjectSkill(context),
        SimpleSpecializedSkill<arondto::BringObjectToLandmarkAcceptedType>(Description)
    {
    }
 
    Skill::MainResult
    BringObjectToLocation::main(const SpecializedMainInput& in)
    {
        std::string objName = in.parameters.objectEntityId;
        std::string graphName = in.parameters.graph;
        std::string objLandmarkName = in.parameters.landmarkFrom;
        std::string targetLandmarkName = in.parameters.landmarkTo;
        std::string robotName = in.parameters.robotName;
        float platformOrientationalAccuracy = 0.1;
        float platformPositionalAccuracy = 20;
        float tcpOrientationalAccuracy = 0.3;
        float tcpPositionalAccuracy = 40;
 
        // move to landmark1
        {
            platform_control::arondto::MovePlatformToLandmarkAcceptedType params;
            params.landmark = objLandmarkName;
            params.robotName = robotName;
            params.graph = graphName;
            params.orientationalAccuracy = platformOrientationalAccuracy;
            params.positionalAccuracy = platformPositionalAccuracy;
 
            SkillProxy prx(manager, SkillID{
                .providerId = ProviderID{.providerName = context.platformControlSkillProvider},
                .skillName = skills::MovePlatformToLandmark::Description.skillId.skillName});
 
            if (prx.executeSkill(in.executorName, params.toAron())
                    .status != TerminatedSkillStatus::Succeeded)
            {
                return {TerminatedSkillStatus::Failed, nullptr};
            }
        }
 
        // grasp Object
        {
            grasp_object::arondto::GraspObjectAcceptedType params;
            params.objectEntityId = objName;
            params.robotName = robotName;
            params.tcpOrientationalAccuracy = tcpOrientationalAccuracy;
            params.tcpPositionalAccuracy = tcpPositionalAccuracy;
            params.platformOrientationalAccuracy = platformOrientationalAccuracy;
            params.platformPositionalAccuracy = platformPositionalAccuracy;
 
            SkillProxy prx(manager, SkillID{
                .providerId = ProviderID{.providerName = context.graspControlSkillProvider},
                .skillName = skills::GraspObjectSkill::Description.skillId.skillName});
 
            if (prx.executeSkill(in.executorName, params.toAron())
                    .status != TerminatedSkillStatus::Succeeded)
            {
                return {TerminatedSkillStatus::Failed, nullptr};
            }
        }
 
        // Move to target landmark
        {
            platform_control::arondto::MovePlatformToLandmarkAcceptedType params;
            params.landmark = targetLandmarkName;
            params.robotName = robotName;
            params.graph = graphName;
            params.orientationalAccuracy = platformOrientationalAccuracy;
            params.positionalAccuracy = platformPositionalAccuracy;
 
            SkillProxy prx(manager, SkillID{
                .providerId = ProviderID{.providerName = context.platformControlSkillProvider},
                .skillName = skills::MovePlatformToLandmark::Description.skillId.skillName});
 
            if (prx.executeSkill(in.executorName, params.toAron())
                    .status != TerminatedSkillStatus::Succeeded)
            {
                return {TerminatedSkillStatus::Failed, nullptr};
            }
        }
 
        // putdown object
        {
            grasp_object::arondto::PutdownObjectAcceptedType params;
            params.objectEntityId = objName;
            params.robotName = robotName;
            params.tcpOrientationalAccuracy = tcpOrientationalAccuracy;
            params.tcpPositionalAccuracy = tcpPositionalAccuracy;
            params.platformOrientationalAccuracy = platformOrientationalAccuracy;
            params.platformPositionalAccuracy = platformPositionalAccuracy;
 
            SkillProxy prx(manager, SkillID{
                .providerId = ProviderID{.providerName = context.graspControlSkillProvider},
                .skillName = skills::PutdownObjectSkill::Description.skillId.skillName});
 
            if (prx.executeSkill(in.executorName, params.toAron())
                    .status != TerminatedSkillStatus::Succeeded)
            {
                return {TerminatedSkillStatus::Failed, nullptr};
            }
        }
 
        return {TerminatedSkillStatus::Succeeded, nullptr};
    }
} // namespace armarx::skills