BringObjectToLocation.cpp

Go to the documentation of this file.

#include "BringObjectToLocation.h"
 
#include <RobotAPI/libraries/skills/provider/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;
        }
    }
 
    SkillDescription BringObjectToLocation::Description = SkillDescription{
        "BringObjectToLocation", "Bring an object to some location",
        {}, armarx::Duration::MilliSeconds(60000),
        arondto::BringObjectToLandmarkAcceptedType::ToAronType(),
        GetDefaultParameterization().toAron()
    };
 
    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, context.platformControlSkillProvider, skills::MovePlatformToLandmark::Description);
 
            if (prx.executeFullSkill(getSkillId().toString(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, context.graspControlSkillProvider, skills::GraspObjectSkill::Description});
 
            if (prx.executeFullSkill(getSkillId().toString(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, context.platformControlSkillProvider, skills::MovePlatformToLandmark::Description.skillName});
 
            if (prx.executeFullSkill(getSkillId().toString(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, context.graspControlSkillProvider, skills::PutdownObjectSkill::Description.skillName});
 
            if (prx.executeFullSkill(getSkillId().toString(in.executorName), params.toAron()).status != TerminatedSkillStatus::Succeeded)
            {
                return {TerminatedSkillStatus::Failed, nullptr};
            }
        }
 
        return {TerminatedSkillStatus::Succeeded, nullptr};
    }
}