CloseHandAndAttach.cpp

Go to the documentation of this file.

#include "CloseHandAndAttach.h"
 
#include <VirtualRobot/RobotNodeSet.h>
 
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/skills/core/SkillProxy.h>
 
namespace armarx::skills
{
    SkillDescription CloseHandAndAttachSkill::Description = skills::SkillDescription{
        .skillId = {.skillName = "CloseHandAndAttach"},
        .description = "Close the hand and attach an object to it",
        .timeout = armarx::Duration::MilliSeconds(5000),
        .parametersType = grasp_object::arondto::CloseHandAndAttachAcceptedType::ToAronType(),
    };
 
    CloseHandAndAttachSkill::CloseHandAndAttachSkill(armem::client::MemoryNameSystem& mns,
                                                     armarx::viz::Client& arviz,
                                                     TwoArmGraspControlSkillContext& context) :
        TwoArmGraspControlSkill(mns, arviz, Description.skillId.skillName, context),
        SimpleSpecializedSkill<ArgType>(Description)
    {
    }
 
    Skill::MainResult
    CloseHandAndAttachSkill::main(const SpecializedMainInput& in)
    {
        armem::robot_state::VirtualRobotReader robotReader;
        armem::obj::instance::Writer objectWriter;
        armem::obj::instance::Reader objectReader;
 
        robotReader.connect(mns);
        objectWriter.connect(mns);
        objectReader.connect(mns);
 
        // //////////////////////////////
        // get robot
        // //////////////////////////////
        auto robot =
            robotReader.getSynchronizedRobot(in.parameters.robotName,
                                             armem::Time::Now(),
                                             VirtualRobot::RobotIO::RobotDescription::eStructure);
        if (!robot)
        {
            ARMARX_ERROR << "Lost robot.";
            return {TerminatedSkillStatus::Failed, nullptr};
        }
 
        // //////////////////////////////
        // get object pose
        // //////////////////////////////
        objectReader.requestLocalization(in.parameters.objectEntityId,
                                         armarx::core::time::Duration::MilliSeconds(500));
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
 
        auto objInstance = objectReader.queryLatestObjectInstance(in.parameters.objectEntityId);
        if (!objInstance)
        {
            ARMARX_ERROR << "Lost object pose.";
            return {TerminatedSkillStatus::Failed, nullptr};
        }
 
        // Close hand
        skills::hand_control::arondto::CloseHandAcceptedType params;
        params.kinematicChainName = in.parameters.kinematicChainName;
 
        SkillProxy prx(manager, SkillID{
            .providerId = ProviderID{.providerName = "HandControlSkillProvider"},
            .skillName = CloseHand::Description.skillId.skillName});
        auto su = prx.executeSkill(in.executorName, params.toAron());
 
        ARMARX_CHECK(robotReader.synchronizeRobot(*robot, armem::Time::Now()));
        objInstance = objectReader.queryLatestObjectInstance(in.parameters.objectEntityId);
 
        auto objPoseInGlobalFrame = objInstance->objectPoseGlobal;
        auto objPoseInRootFrame =
            robot->getRootNode()->toLocalCoordinateSystem(objPoseInGlobalFrame);
        auto objPoseInTCPFrame = robot->getRobotNodeSet(in.parameters.kinematicChainName)
                                     ->getTCP()
                                     ->toLocalCoordinateSystem(objPoseInGlobalFrame);
 
        armem::arondto::ObjectInstance arondto;
        armarx::armem::toAron(arondto, *objInstance);
 
        // TODO refactor writer to accept non-dto objects
        arondto.pose.attachmentValid = true;
        arondto.pose.attachment.agentName = robot->getName();
        arondto.pose.attachment.frameName =
            robot->getRobotNodeSet(in.parameters.kinematicChainName)->getTCP()->getName();
        arondto.pose.attachment.poseInFrame = objPoseInTCPFrame;
        arondto.pose.objectPoseRobot = objPoseInRootFrame;
        arondto.pose.robotPose = objPoseInGlobalFrame;
        arondto.pose.robotConfig = robot->getJointValues();
 
        objectWriter.commitObject(arondto, objInstance->providerName, armem::Time::Now());
 
        return {su.status, nullptr};
    }
} // namespace armarx::skills