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/provider/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 =
        SkillDescription{"GraspObject",
                         "Grasp an object",
                         {},
                         armarx::Duration::MilliSeconds(120000),
                         grasp_object::arondto::GraspObjectAcceptedType::ToAronType(),
                         GetDefaultParameterization().toAron()};
 
    GraspObjectSkill::GraspObjectSkill(armem::client::MemoryNameSystem& mns,
                                       armarx::viz::Client& arviz,
                                       TwoArmGraspControlSkillContext& context) :
        TwoArmGraspControlSkill(mns, arviz, Description.skillName, context),
        SimpleSpecializedSkill<ArgType>(Description)
    {
    }
 
    Skill::MainResult
    GraspObjectSkill::main(const SpecializedMainInput& in)
    {
        armem::robot_state::VirtualRobotReader robotReader(mns);
        armem::obj::instance::Reader objectReader(mns);
        armem::grasping::known_grasps::Reader graspReader(mns);
        armem::obj::instance::Writer objectWriter(mns);
 
        robotReader.connect();
        objectReader.connect();
        graspReader.connect();
        objectWriter.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};
        }
 
        // 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,
                           context.visualSearchSkillProvider,
                           WhatCanYouSeeNow::Description.skillName);
 
            if (prx.executeFullSkill(getSkillId().toString(), 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, context.handControlSkillProvider, OpenHand::Description.skillName});
 
            //if (prx.executeFullSkill(getSkillId().toString(in.executorName), params.toAron()).status != TerminatedSkillStatus::Succeeded)
            //{
            //    return {TerminatedSkillStatus::Failed, nullptr};
            //}
            prx.begin_executeFullSkill(getSkillId().toString(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, providerName, MovePlatformForGraspSkill::Description.skillName);
 
            if (prx.executeFullSkill(getSkillId().toString(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, providerName, ExecuteGraspSkill::Description.skillName});
 
            if (prx.executeFullSkill(getSkillId().toString(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, providerName, skills::CloseHandAndAttachSkill::Description.skillName});
 
            if (prx.executeFullSkill(getSkillId().toString(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,
                            context.tcpControlSkillProvider,
                            skills::MoveTCPToTargetPose::Description.skillName});
 
            if (prx.executeFullSkill(getSkillId().toString(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,
                            context.jointControlSkillProvider,
                            MoveJointsToPosition::Description.skillName});
 
            if (prx.executeFullSkill(getSkillId().toString(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, providerName, MovePlatformAfterGraspSkill::Description.skillName);
 
            if (prx.executeFullSkill(getSkillId().toString(in.executorName), params.toAron())
                    .status != TerminatedSkillStatus::Succeeded)
            {
                return {TerminatedSkillStatus::Failed, nullptr};
            }
        }
 
        clearLayer();
 
        return {TerminatedSkillStatus::Succeeded, nullptr};
    }
} // namespace armarx::skills