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;
        }
    } // namespace
 
    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};
    }
} // namespace armarx::skills