OpenHandAndDetach.cpp

Go to the documentation of this file.

#include "OpenHandAndDetach.h"
 
#include <VirtualRobot/RobotNodeSet.h>
 
#include <RobotAPI/libraries/skills/provider/SkillProxy.h>
 
#include <RobotSkillTemplates/libraries/skill_hand_control/OpenHand.h>
 
namespace armarx::skills
{
    SkillDescription OpenHandAndDetachSkill::Description = skills::SkillDescription{
        "OpenHandAndDetach",
        "Open the hand and detach an object from it",
        {},
        armarx::Duration::MilliSeconds(120000),
        grasp_object::arondto::OpenHandAndDetachAcceptedType::ToAronType()};
 
    OpenHandAndDetachSkill::OpenHandAndDetachSkill(armem::client::MemoryNameSystem& mns,
                                                   armarx::viz::Client& arviz,
                                                   TwoArmGraspControlSkillContext& context) :
        TwoArmGraspControlSkill(mns, arviz, Description.skillName, context),
        SimpleSpecializedSkill<ArgType>(Description)
    {
    }
 
    Skill::MainResult
    OpenHandAndDetachSkill::main(const SpecializedMainInput& in)
    {
        armem::robot_state::VirtualRobotReader robotReader(mns);
        armem::obj::instance::Writer objectWriter(mns);
        armem::obj::instance::Reader objectReader(mns);
 
        robotReader.connect();
        objectWriter.connect();
        objectReader.connect();
 
        // //////////////////////////////
        // 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
        // //////////////////////////////
        auto objInstance = objectReader.queryLatestObjectInstance(in.parameters.objectEntityId);
        if (!objInstance)
        {
            ARMARX_ERROR << "Lost object pose.";
            return {TerminatedSkillStatus::Failed, nullptr};
        }
 
        // Open hand
        skills::hand_control::arondto::OpenHandAcceptedType params;
        params.kinematicChainName = in.parameters.kinematicChainName;
 
        SkillProxy prx(
            {manager, context.handControlSkillProvider, OpenHand::Description.skillName});
        auto su = prx.executeFullSkill(getSkillId().toString(in.executorName), params.toAron());
 
        ARMARX_CHECK(robotReader.synchronizeRobot(*robot, armem::Time::Now()));
        objInstance = objectReader.queryLatestObjectInstance(in.parameters.objectEntityId);
        if (!objInstance)
        {
            ARMARX_ERROR << "Lost object pose.";
            return {TerminatedSkillStatus::Failed, nullptr};
        }
 
        auto objPoseInRootFrame = objInstance->objectPoseRobot;
        auto robotPoseGlobal = robot->getRootNode()->getGlobalPose();
 
        auto newObjPoseGlobal = robotPoseGlobal * objPoseInRootFrame;
 
        //auto objPoseInTCPFrame = robot->getRobotNodeSet(in.kinematicChainName)->getTCP()->toLocalCoordinateSystem(objPoseInGlobalFrame);
 
        armem::arondto::ObjectInstance arondto;
 
        // TODO
        //        objInstance->attachmentValid = false;
        //        objInstance->attachment.agentName = "";
        //        objInstance->attachment.frameName = "";
        //        objInstance->objectPoseGlobal = newObjPoseGlobal;
        //        objInstance->objectPoseRobot = objPoseInRootFrame;
        //        objInstance->robotPose = robot->getRootNode()->getGlobalPose();
        //        objInstance->robotConfig = robot->getJointValues();
 
        objectWriter.commitObject(arondto, objInstance->providerName, armem::Time::Now());
 
        return {su.status, nullptr};
    }
} // namespace armarx::skills