GraspObject.cpp

Go to the documentation of this file.

#include "GraspObject.h"
 
#include <VirtualRobot/Grasping/GraspSet.h>
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/skills/core/SkillProxy.h>
 
#include "util/GraspObjectUtil.h"
 
namespace armarx::skills
{
    namespace
    {
        grasp_object::arondto::GraspObjectAcceptedType
        GetDefaultParameterization()
        {
            grasp_object::arondto::GraspObjectAcceptedType ret;
            ret.platformOrientationalAccuracy = 0.1;
            ret.platformPositionalAccuracy = 10;
            ret.tcpOrientationalAccuracy = 0.1;
            ret.tcpPositionalAccuracy = 20;
            return ret;
        }
    } // namespace
 
    SkillDescription GraspObjectSkill::Description = skills::SkillDescription{
        .skillId = {.skillName = "GraspObject"},
        .description = "Grasp an object",
        .rootProfileDefaults = GetDefaultParameterization().toAron(),
        .timeout = armarx::Duration::MilliSeconds(120000),
        .parametersType = grasp_object::arondto::GraspObjectAcceptedType::ToAronType(),
    };
 
    GraspObjectSkill::GraspObjectSkill(armem::client::MemoryNameSystem& mns,
                                       armarx::viz::Client& arviz,
                                       TwoArmGraspControlSkillContext& context) :
        TwoArmGraspControlSkill(mns, arviz, Description.skillId.skillName, context),
        SimpleSpecializedSkill<ArgType>(Description)
    {
    }
 
    Skill::MainResult
    GraspObjectSkill::main(const SpecializedMainInput& in)
    {
        armem::robot_state::VirtualRobotReader robotReader;
        armem::obj::instance::Reader objectReader;
        armem::grasping::known_grasps::Reader graspReader;
        armem::obj::instance::Writer objectWriter;
 
        robotReader.connect(mns);
        objectReader.connect(mns);
        graspReader.connect(mns);
        objectWriter.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};
        }
 
        // Relocalize the obj. May be removed later
        {
            skills::visual_search::arondto::WhatCanYouSeeNowAcceptedType params;
            params.robotName = in.parameters.robotName;
            params.objectEntityId = in.parameters.objectEntityId;
 
            SkillProxy prx(manager, SkillID{
                .providerId = ProviderID{.providerName = context.visualSearchSkillProvider},
                .skillName = WhatCanYouSeeNow::Description.skillId.skillName});
 
            if (prx.executeSkill(in.executorName, params.toAron()).status !=
                TerminatedSkillStatus::Succeeded)
            {
                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};
        }
 
        // //////////////////////////////
        // select a kinematic chain
        // //////////////////////////////
        auto kinematicChainSelection =
            skills::grasp_control::util::SelectBestKinematicChainForObjectGrasp(
                {*objInstance, *robot});
        if (kinematicChainSelection.kinematicChainName.empty())
        {
            ARMARX_ERROR << "No kinematic chain found! ";
            return {TerminatedSkillStatus::Failed, nullptr};
        }
        else
        {
            ARMARX_INFO << "Using kinematic chain: " << kinematicChainSelection.kinematicChainName;
        }
 
        // //////////////////////////////
        // select a grasp
        // //////////////////////////////
        auto graspSetSelection = skills::grasp_control::util::SelectBestGraspSetForObject(
            {*graspInfo,
             robot->getRobotNodeSet(kinematicChainSelection.kinematicChainName)
                 ->getTCP()
                 ->getName()});
 
        if (graspSetSelection.graspSetName.empty())
        {
            ARMARX_ERROR << "No grasp info for object " << in.parameters.objectEntityId
                         << " found! ";
            return {TerminatedSkillStatus::Failed, nullptr};
        }
        else
        {
            ARMARX_INFO << "Using grasp: " << graspSetSelection.graspSetName;
        }
 
        // //////////////////////////////
        // CALL SUBSKILLS
        // //////////////////////////////
        // Open Hand async
        {
            skills::hand_control::arondto::OpenHandAcceptedType params;
            params.kinematicChainName = kinematicChainSelection.kinematicChainName;
 
            SkillProxy prx(manager, SkillID{
                .providerId = ProviderID{.providerName = context.handControlSkillProvider},
                .skillName = OpenHand::Description.skillId.skillName});
 
            //if (prx.executeSkill(in.executorName, params.toAron()).status != TerminatedSkillStatus::Succeeded)
            //{
            //    return {TerminatedSkillStatus::Failed, nullptr};
            //}
            prx.executeSkillAsync(in.executorName, params.toAron());
        }
 
        // Move platform relative to obj
        {
            skills::grasp_object::arondto::MovePlatformForGraspAcceptedType params;
            params.robotName = in.parameters.robotName;
            params.orientationalAccuracy = in.parameters.platformOrientationalAccuracy;
            params.positionalAccuracy = in.parameters.platformPositionalAccuracy;
            params.objectEntityId = in.parameters.objectEntityId;
            params.tcpName = kinematicChainSelection.tcpName;
 
            SkillProxy prx(manager, SkillID{
                .providerId = ProviderID{.providerName = context.graspControlSkillProvider},
                .skillName = MovePlatformForGraspSkill::Description.skillId.skillName});
 
            if (prx.executeSkill(in.executorName, params.toAron())
                    .status != TerminatedSkillStatus::Succeeded)
            {
                return {TerminatedSkillStatus::Failed, nullptr};
            }
        }
 
        // Execute the grasp set
        {
            skills::grasp_object::arondto::ExecuteGraspAcceptedType params;
            params.robotName = in.parameters.robotName;
            params.graspSetName = graspSetSelection.graspSetName;
            params.objectEntityId = in.parameters.objectEntityId;
            params.kinematicChainName = kinematicChainSelection.kinematicChainName;
            params.orientationalAccuracy = in.parameters.tcpOrientationalAccuracy;
            params.positionalAccuracy = in.parameters.tcpPositionalAccuracy;
 
            SkillProxy prx(manager, SkillID{
                .providerId = ProviderID{.providerName = context.graspControlSkillProvider},
                .skillName = ExecuteGraspSkill::Description.skillId.skillName});
 
            if (prx.executeSkill(in.executorName, params.toAron())
                    .status != TerminatedSkillStatus::Succeeded)
            {
                return {TerminatedSkillStatus::Failed, nullptr};
            }
        }
 
        // Close hand and attach to memory
        {
            skills::grasp_object::arondto::CloseHandAndAttachAcceptedType params;
            params.kinematicChainName = kinematicChainSelection.kinematicChainName;
            params.objectEntityId = in.parameters.objectEntityId;
            params.robotName = in.parameters.robotName;
 
            SkillProxy prx(manager, SkillID{
                .providerId = ProviderID{.providerName = context.graspControlSkillProvider},
                .skillName = skills::CloseHandAndAttachSkill::Description.skillId.skillName});
 
            if (prx.executeSkill(in.executorName, params.toAron())
                    .status != TerminatedSkillStatus::Succeeded)
            {
                return {TerminatedSkillStatus::Failed, nullptr};
            }
        }
 
        // Move TCP a bit up
        {
            ARMARX_CHECK(robotReader.synchronizeRobot(*robot, armem::Time::Now()));
            auto tcpPoseGlobal =
                robot->getRobotNode(kinematicChainSelection.tcpName)->getGlobalPose();
 
            auto targetTCPPoseGlobal = tcpPoseGlobal;
            targetTCPPoseGlobal(2, 3) += 100;
 
            skills::tcp_control::arondto::MoveTCPToTargetPoseAcceptedType params;
            params.robotName = in.parameters.robotName;
            params.kinematicChainName = kinematicChainSelection.kinematicChainName;
            params.targetPoseGlobal = targetTCPPoseGlobal;
            params.orientationalAccuracy = in.parameters.tcpOrientationalAccuracy;
            params.positionalAccuracy = in.parameters.tcpPositionalAccuracy;
 
            SkillProxy prx(manager, SkillID{
                .providerId = ProviderID{.providerName = context.tcpControlSkillProvider},
                .skillName = skills::MoveTCPToTargetPose::Description.skillId.skillName});
 
            if (prx.executeSkill(in.executorName, params.toAron())
                    .status != TerminatedSkillStatus::Succeeded)
            {
                return {TerminatedSkillStatus::Failed, nullptr};
            }
        }
 
        // Move joints to zero position
        {
            skills::joint_control::arondto::MoveJointsToPositionAcceptedType params;
            params.robotName = in.parameters.robotName;
            params.jointTargetTolerance = 0.03; // Todo
            params.jointMaxSpeed = 0.75;
            params.accelerationTime = 500;
            params.setVelocitiesToZeroAtEnd = true;
 
            for (const auto& j : robot->getRobotNodeSet(kinematicChainSelection.kinematicChainName)
                                     ->getAllRobotNodes())
            {
                params.targetJointMap[j->getName()] = 0.0;
            }
 
            SkillProxy prx(manager, SkillID{
                .providerId = ProviderID{.providerName = context.jointControlSkillProvider},
                .skillName = MoveJointsToPosition::Description.skillId.skillName});
 
            if (prx.executeSkill(in.executorName, params.toAron())
                    .status != TerminatedSkillStatus::Succeeded)
            {
                return {TerminatedSkillStatus::Failed, nullptr};
            }
        }
 
        // Move platform relative after grasp
        {
            skills::grasp_object::arondto::MovePlatformAfterGraspAcceptedType params;
            params.robotName = in.parameters.robotName;
            params.orientationalAccuracy = in.parameters.platformOrientationalAccuracy;
            params.positionalAccuracy = in.parameters.platformPositionalAccuracy;
 
            SkillProxy prx(manager, SkillID{
                .providerId = ProviderID{.providerName = context.graspControlSkillProvider},
                .skillName = MovePlatformAfterGraspSkill::Description.skillId.skillName});
 
            if (prx.executeSkill(in.executorName, params.toAron())
                    .status != TerminatedSkillStatus::Succeeded)
            {
                return {TerminatedSkillStatus::Failed, nullptr};
            }
        }
 
        clearLayer();
 
        return {TerminatedSkillStatus::Succeeded, nullptr};
    }
} // namespace armarx::skills