BringObjectToHuman.cpp

Go to the documentation of this file.

#include "BringObjectToHuman.h"
 
namespace armarx::skills
{
    const SkillDescription BringObjectToHumanDesc = SkillDescription{
        .skillId = {.skillName = "BringObjectToHuman"},
        .description = "Bring an object to some location",
        .timeout = armarx::Duration::MilliSeconds(60000),
    };
 
    /*BringObjectToHuman::BringObjectToHuman(armem::client::MemoryNameSystem& mns, armarx::viz::Client& arviz, GraspControlSkillContext& context) :
        Skill(BringObjectToHumanDesc),
        mns(mns),
        arviz(arviz),
        context(context),
        graspObject(mns, arviz, context),
        handoverObject(mns, arviz, context),
        movePlatformToLandmark(mns, arviz, context.platformControlSkillContext)
    {
    }
 
    void BringObjectToHuman::_reset()
    {
        graspObject.reset();
        handoverObject.reset();
        movePlatformToLandmark.reset();
        Skill::_reset();
    }
 
    void BringObjectToHuman::_notifyStopped()
    {
        graspObject.notifyStopped();
        handoverObject.notifyStopped();
        movePlatformToLandmark.notifyStopped();
        Skill::_notifyStopped();
    }
 
    void BringObjectToHuman::_notifyTimeoutReached()
    {
        graspObject.notifyTimeoutReached();
        handoverObject.notifyTimeoutReached();
        movePlatformToLandmark.notifyTimeoutReached();
        Skill::_notifyTimeoutReached();
    }
 
    Skill::Status BringObjectToHuman::_execute(const aron::data::DictPtr& in, const CallbackT& callback)
    {
        std::string objName = "Kitchen/green-cup/0";
        std::string objLandmarkName = "sideboard";
        std::string targetLandmarkName = "delivernode";
        std::string graspSetName = "TCP L";
        std::string kinematicChainName = "LeftArm";
        std::string otherKinematicChainName = "RightArm";
        std::string robot = "Armar3";
 
        Skill::Status ret = Skill::Status::Succeeded;
 
        // move to landmark1
        {
            platform_control::arondto::MovePlatformToLandmarkAcceptedType args;
            args.landmark = objLandmarkName;
            args.orientationalAccuracy = 0.1;
            args.positionalAccuracy = 20;
 
            ret = movePlatformToLandmark._execute(args, callback);
        }
 
        if (ret != Skill::Status::Succeeded)
        {
            return ret;
        }
 
        // grasp Object
        {
            grasp_object::arondto::GraspObjectAcceptedType args;
            args.graspSetName = graspSetName;
            args.kinematicChainNames = "LeftArm";
            args.objectEntityId = objName;
            args.orientationalAccuracy = 0.1;
            args.positionalAccuracy = 20;
            args.robot = robot;
 
            ret = graspObject._execute(args, callback);
        }
 
        if (ret != Skill::Status::Succeeded)
        {
            return ret;
        }
 
        // Move to target landmark
        {
            platform_control::arondto::MovePlatformToLandmarkAcceptedType args;
            args.landmark = targetLandmarkName;
            args.orientationalAccuracy = 0.1;
            args.positionalAccuracy = 20;
 
            ret = movePlatformToLandmark._execute(args, callback);
        }
 
        if (ret != Skill::Status::Succeeded)
        {
            return ret;
        }
 
        // putdown object
        {
            grasp_object::arondto::PutdownObjectAcceptedType args;
            args.kinematicChainName = kinematicChainName;
            args.objectEntityId = objName;
            args.orientationalAccuracy = 0.1;
            args.otherArmKinematicChainName = otherKinematicChainName;
            args.positionalAccuracy = 20;
            args.robot = robot;
 
            ret = handoverObject._execute(args, callback);
        }
 
        return ret;
    }*/
} // namespace armarx::skills