ExecutePutdown.cpp

Go to the documentation of this file.

#include <SimoxUtility/algorithm/string/string_tools.h>
#include "ExecutePutdown.h"
 
#include <VirtualRobot/RobotNodeSet.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/skills/core/SkillProxy.h>
 
#include <RobotSkillTemplates/libraries/skill_joint_control/MoveJointsToPosition.h>
#include <RobotSkillTemplates/libraries/skill_tcp_control/MoveTCPToTargetPose.h>
 
#include "util/PutdownObjectUtil.h"
 
namespace armarx::skills
{
    namespace
    {
        grasp_object::arondto::ExecutePutdownAcceptedType
        GetDefaultParameterization()
        {
            grasp_object::arondto::ExecutePutdownAcceptedType ret;
            ret.orientationalAccuracy = 0.01;
            ret.positionalAccuracy = 25;
            return ret;
        }
    } // namespace
 
    SkillDescription ExecutePutdownSkill::Description = skills::SkillDescription{
        .skillId = {.skillName = "ExecutePutdown"},
        .description = "Execute a putdown for some grasped object",
        .rootProfileDefaults = GetDefaultParameterization().toAron(),
        .timeout = armarx::Duration::MilliSeconds(120000),
        .parametersType = grasp_object::arondto::ExecutePutdownAcceptedType::ToAronType(),
    };
 
    ExecutePutdownSkill::ExecutePutdownSkill(armem::client::MemoryNameSystem& mns,
                                             armarx::viz::Client& arviz,
                                             TwoArmGraspControlSkillContext& context) :
        TwoArmGraspControlSkill(mns, arviz, Description.skillId.skillName, context),
        SimpleSpecializedSkill<ArgType>(Description)
    {
    }
 
    Skill::MainResult
    ExecutePutdownSkill::main(const SpecializedMainInput& in)
    {
        armem::robot_state::VirtualRobotReader robotReader;
        armem::obj::instance::Reader objectReader;
        armem::grasping::known_grasps::Reader graspReader;
 
        robotReader.connect(mns);
        objectReader.connect(mns);
        graspReader.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
        // //////////////////////////////
        auto objInstance = objectReader.queryLatestObjectInstance(in.parameters.objectEntityId);
        if (!objInstance)
        {
            ARMARX_ERROR << "Lost object pose.";
            return {TerminatedSkillStatus::Failed, nullptr};
        }
 
 
        ARMARX_CHECK(objInstance->attachment->agentName == in.parameters.robotName);
 
        std::string kinematicChainName = "LeftArm";
        std::string otherArmKinematicChainName = "RightArm";
        if (simox::alg::ends_with(objInstance->attachment->frameName, " R"))
        {
            kinematicChainName = "RightArm";
            otherArmKinematicChainName = "LeftArm";
        }
 
        // //////////////////////////////
        // Move TCPs
        // //////////////////////////////
        {
            skills::joint_control::arondto::MoveJointsToPositionAcceptedType params;
            params.robotName = in.parameters.robotName;
            params.jointTargetTolerance = 0.03;
            params.jointMaxSpeed = 0.75;
            params.accelerationTime = 500;
            params.setVelocitiesToZeroAtEnd = true;
 
            params.targetJointMap[robot->getRobotNodeSet(otherArmKinematicChainName)
                                      ->getNodeNames()[0]] = 0.47;
            params.targetJointMap[robot->getRobotNodeSet(otherArmKinematicChainName)
                                      ->getNodeNames()[1]] = 0.0;
            params.targetJointMap[robot->getRobotNodeSet(otherArmKinematicChainName)
                                      ->getNodeNames()[2]] = 0.0;
            params.targetJointMap[robot->getRobotNodeSet(otherArmKinematicChainName)
                                      ->getNodeNames()[3]] = 0.32;
            params.targetJointMap[robot->getRobotNodeSet(otherArmKinematicChainName)
                                      ->getNodeNames()[4]] = 0.47;
 
            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};
            }
        }
 
        // Look at hand
        {
            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;
            params.targetJointMap["Neck_1_Pitch"] = 0.6;
 
            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};
            }
        }
 
        ARMARX_CHECK(robotReader.synchronizeRobot(*robot, armem::Time::Now()));
 
        const auto objPoseLocal = objInstance->objectPoseRobot;
        const auto objPoseGlobal = robot->getRootNode()->toGlobalCoordinateSystem(objPoseLocal);
        const auto objectInTCPFrame = objInstance->attachment->poseInFrame;
        Eigen::Matrix4f tcpPlacePoseGlobal = objPoseGlobal * objectInTCPFrame.inverse();
        tcpPlacePoseGlobal(2, 3) += 50;
 
        auto tcpPlacePrePoseGlobal = tcpPlacePoseGlobal;
        tcpPlacePrePoseGlobal(2, 3) += 80;
 
        std::vector<Eigen::Matrix4f> targetPosesGlobal = {tcpPlacePrePoseGlobal,
                                                          tcpPlacePoseGlobal};
 
        for (const auto& tcpTargetPoseGlobal : targetPosesGlobal)
        {
            skills::tcp_control::arondto::MoveTCPToTargetPoseAcceptedType params;
            params.robotName = in.parameters.robotName;
            params.kinematicChainName = kinematicChainName;
            params.targetPoseGlobal = tcpTargetPoseGlobal;
            params.orientationalAccuracy = in.parameters.orientationalAccuracy;
            params.positionalAccuracy = in.parameters.positionalAccuracy;
 
            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};
            }
        }
 
        // Look up
        {
            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;
            params.targetJointMap["Neck_1_Pitch"] = 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};
            }
        }
 
        return {TerminatedSkillStatus::Succeeded, nullptr};
    }
} // namespace armarx::skills