PutdownObject.cpp

Go to the documentation of this file.

#include <SimoxUtility/algorithm/string/string_tools.h>
#include "PutdownObject.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>
 
namespace armarx::skills
{
    namespace
    {
        grasp_object::arondto::PutdownObjectAcceptedType
        GetDefaultParameterization()
        {
            grasp_object::arondto::PutdownObjectAcceptedType ret;
            ret.platformOrientationalAccuracy = 0.1;
            ret.platformPositionalAccuracy = 10;
            ret.tcpOrientationalAccuracy = 0.1;
            ret.tcpPositionalAccuracy = 20;
            return ret;
        }
    } // namespace
 
    SkillDescription PutdownObjectSkill::Description = skills::SkillDescription{
        .skillId = {.skillName = "PutdownObject"},
        .description = "Putdown an object already in hand of robot",
        .rootProfileDefaults = GetDefaultParameterization().toAron(),
        .timeout = armarx::Duration::MilliSeconds(40000),
        .parametersType = grasp_object::arondto::PutdownObjectAcceptedType::ToAronType(),
    };
 
    PutdownObjectSkill::PutdownObjectSkill(armem::client::MemoryNameSystem& mns,
                                           armarx::viz::Client& arviz,
                                           TwoArmGraspControlSkillContext& context) :
        TwoArmGraspControlSkill(mns, arviz, Description.skillId.skillName, context),
        SimpleSpecializedSkill<ArgType>(Description)
    {
    }
 
    Skill::MainResult
    PutdownObjectSkill::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);
 
        // //////////////////////////////
        // 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";
        if (simox::alg::ends_with(objInstance->attachment->frameName, " R"))
        {
            kinematicChainName = "RightArm";
        }
 
 
        // //////////////////////////////
        // Call subskills
        // //////////////////////////////
        // Move into a besser position
        {
            skills::grasp_object::arondto::MovePlatformForPutdownAcceptedType params;
            params.robotName = in.parameters.robotName;
            params.orientationalAccuracy = in.parameters.platformOrientationalAccuracy;
            params.positionalAccuracy = in.parameters.platformPositionalAccuracy;
            params.objectEntityId = in.parameters.objectEntityId;
            params.tcpName = robot->getRobotNodeSet(kinematicChainName)->getTCP()->getName();
 
            SkillProxy prx(manager, SkillID{
                .providerId = ProviderID{.providerName = context.graspControlSkillProvider},
                .skillName = MovePlatformForPutdownSkill::Description.skillId.skillName});
 
            if (prx.executeSkill(in.executorName, params.toAron())
                    .status != TerminatedSkillStatus::Succeeded)
            {
                return {TerminatedSkillStatus::Failed, nullptr};
            }
        }
 
        // Execute Putdown motions
        {
            skills::grasp_object::arondto::ExecutePutdownAcceptedType params;
            params.robotName = in.parameters.robotName;
            params.orientationalAccuracy = in.parameters.platformOrientationalAccuracy;
            params.positionalAccuracy = in.parameters.platformPositionalAccuracy;
            params.objectEntityId = in.parameters.objectEntityId;
 
            SkillProxy prx(manager, SkillID{
                .providerId = ProviderID{.providerName = context.graspControlSkillProvider},
                .skillName = ExecutePutdownSkill::Description.skillId.skillName});
 
            if (prx.executeSkill(in.executorName, params.toAron())
                    .status != TerminatedSkillStatus::Succeeded)
            {
                return {TerminatedSkillStatus::Failed, nullptr};
            }
        }
 
        // Open hand and detach from memory
        {
            skills::grasp_object::arondto::OpenHandAndDetachAcceptedType params;
            params.kinematicChainName = kinematicChainName;
            params.objectEntityId = in.parameters.objectEntityId;
            params.robotName = in.parameters.robotName;
 
            SkillProxy prx(manager, SkillID{
                .providerId = ProviderID{.providerName = context.graspControlSkillProvider},
                .skillName = skills::OpenHandAndDetachSkill::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()));
 
            Eigen::Matrix4f tcpOffset = Eigen::Matrix4f::Identity();
            tcpOffset(0, 3) = 90;
            tcpOffset(1, 3) = 10;
 
            Eigen::Matrix4f tcpTargetPoseGlobal =
                robot->getRobotNodeSet(kinematicChainName)->getTCP()->getGlobalPose() * tcpOffset;
 
            skills::tcp_control::arondto::MoveTCPToTargetPoseAcceptedType params;
            params.robotName = in.parameters.robotName;
            params.kinematicChainName = kinematicChainName;
            params.targetPoseGlobal = tcpTargetPoseGlobal;
            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;
            params.jointMaxSpeed = 0.75;
            params.accelerationTime = 500;
            params.setVelocitiesToZeroAtEnd = true;
 
            for (const auto& j : robot->getRobotNodeSet(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 back
        {
            skills::grasp_object::arondto::MovePlatformAfterPutdownAcceptedType 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 = MovePlatformAfterPutdownSkill::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