PutdownObject.cpp

Go to the documentation of this file.

#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/provider/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 =
        SkillDescription{"PutdownObject",
                         "Putdown an object already in hand of robot",
                         {},
                         armarx::Duration::MilliSeconds(40000),
                         grasp_object::arondto::PutdownObjectAcceptedType::ToAronType(),
                         GetDefaultParameterization().toAron()};
 
    PutdownObjectSkill::PutdownObjectSkill(armem::client::MemoryNameSystem& mns,
                                           armarx::viz::Client& arviz,
                                           TwoArmGraspControlSkillContext& context) :
        TwoArmGraspControlSkill(mns, arviz, Description.skillName, context),
        SimpleSpecializedSkill<ArgType>(Description)
    {
    }
 
    Skill::MainResult
    PutdownObjectSkill::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);
 
        // //////////////////////////////
        // 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, providerName, MovePlatformForPutdownSkill::Description.skillName);
 
            if (prx.executeFullSkill(getSkillId().toString(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, providerName, ExecutePutdownSkill::Description.skillName);
 
            if (prx.executeFullSkill(getSkillId().toString(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, providerName, skills::OpenHandAndDetachSkill::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()));
 
            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,
                            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;
            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,
                            context.jointControlSkillProvider,
                            MoveJointsToPosition::Description.skillName});
 
            if (prx.executeFullSkill(getSkillId().toString(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, providerName, MovePlatformAfterPutdownSkill::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