ExecuteGrasp.cpp

Go to the documentation of this file.

#include "ExecuteGrasp.h"
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/skills/provider/SkillProxy.h>
 
#include <RobotSkillTemplates/libraries/skill_tcp_control/MoveTCPToTargetPose.h>
 
#include "util/GraspObjectUtil.h"
 
namespace armarx::skills
{
    namespace
    {
        grasp_object::arondto::ExecuteGraspAcceptedType
        GetDefaultParameterization()
        {
            grasp_object::arondto::ExecuteGraspAcceptedType ret;
            ret.orientationalAccuracy = 0.01;
            ret.positionalAccuracy = 25;
            return ret;
        }
    } // namespace
 
    SkillDescription ExecuteGraspSkill::Description =
        skills::SkillDescription{"ExecuteGrasp",
                                 "Execute a graspset on some object",
                                 {},
                                 armarx::Duration::MilliSeconds(110000),
                                 grasp_object::arondto::ExecuteGraspAcceptedType::ToAronType(),
                                 GetDefaultParameterization().toAron()};
 
    ExecuteGraspSkill::ExecuteGraspSkill(armem::client::MemoryNameSystem& mns,
                                         armarx::viz::Client& arviz,
                                         TwoArmGraspControlSkillContext& context) :
        TwoArmGraspControlSkill(mns, arviz, Description.skillName, context),
        SimpleSpecializedSkill<ArgType>(Description)
    {
    }
 
    Skill::MainResult
    ExecuteGraspSkill::main(const SpecializedMainInput& in)
    {
        armem::robot_state::VirtualRobotReader robotReader(mns);
        armem::obj::instance::Reader objectReader(mns);
        armem::grasping::known_grasps::Reader graspReader(mns);
 
        robotReader.connect();
        objectReader.connect();
        graspReader.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};
        }
 
        // //////////////////////////////
        // get all possible grasps
        // //////////////////////////////
        auto graspInfo = graspReader.queryKnownGraspInfoByEntityName(in.parameters.objectEntityId,
                                                                     armem::Time::Now());
        if (!graspInfo)
        {
            ARMARX_ERROR << "Lost grasp info.";
            return {TerminatedSkillStatus::Failed, nullptr};
        }
 
        if (const auto& it = graspInfo->graspSets.find(in.parameters.graspSetName);
            it == graspInfo->graspSets.end())
        {
            ARMARX_ERROR << "Grasp set name " << in.parameters.graspSetName << " not found.";
            return {TerminatedSkillStatus::Failed, nullptr};
        }
 
        for (const auto& grasp : graspInfo->graspSets.at(in.parameters.graspSetName).grasps)
        {
            {
                // Generate Grasp
                auto objPose = objectReader.queryLatestObjectInstance(in.parameters.objectEntityId);
 
                const Eigen::Matrix4f graspPoseGlobal =
                    objPose->objectPoseGlobal * grasp.pose.inverse();
                const Eigen::Matrix4f tcpTargetPoseGlobal = graspPoseGlobal;
 
                //}
 
                // Move TCP to poses
                //{
                skills::tcp_control::arondto::MoveTCPToTargetPoseAcceptedType params;
                params.robotName = in.parameters.robotName;
                params.kinematicChainName = in.parameters.kinematicChainName;
                params.targetPoseGlobal = tcpTargetPoseGlobal;
                params.orientationalAccuracy = in.parameters.orientationalAccuracy;
                params.positionalAccuracy = in.parameters.positionalAccuracy;
 
                SkillProxy prx({manager,
                                context.tcpControlSkillProvider,
                                skills::MoveTCPToTargetPose::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