SimpleHandoverObject.cpp

Go to the documentation of this file.

#include <SimoxUtility/algorithm/string/string_tools.h>
#include "SimpleHandoverObject.h"
 
#include <VirtualRobot/Grasping/GraspSet.h>
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
namespace armarx::skills
{
    const SkillDescription GraspObjectSkillDesc = SkillDescription{
        .skillId = {.skillName = "SimpleHandoverObject"},
        .description = "Handover an object already in hand of robot",
        .timeout = armarx::Duration::MilliSeconds(40000),
        .parametersType = grasp_object::arondto::PutdownObjectAcceptedType::ToAronType(),
    };
 
    /*SimpleHandoverObject::SimpleHandoverObject(armem::client::MemoryNameSystem& mns, armarx::viz::Client& arviz, GraspControlSkillContext& context) :
        Base(GraspObjectDesc, mns, arviz, context),
        movePlatformToPose(mns, arviz, context.platformControlSkillContext)
    {
    }
 
    void SimpleHandoverObject::reset()
    {
        Base::reset();
        movePlatformToPose.reset();
    }
 
    void SimpleHandoverObject::notifyStopped()
    {
        Base::notifyStopped();
        movePlatformToPose.notifyStopped();
    }
 
    void SimpleHandoverObject::notifyTimeoutReached()
    {
        Base::notifyTimeoutReached();
        movePlatformToPose.notifyTimeoutReached();
    }
 
    Skill::Status SimpleHandoverObject::execute(const grasp_object::arondto::PutdownObjectAcceptedType& in, const CallbackT& callback)
    {
        // Members
        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();
 
        // //////////////////////////////
        // check args
        // //////////////////////////////
        auto split = simox::alg::split(in.objectEntityId, "/");
        auto objectId = in.objectEntityId;
        if (split.size() > 3)
        {
            ARMARX_ERROR << "Unknown structure of object entitiy id!";
            return Skill::Status::Failed;
        }
        else if (split.size() == 3)
        {
            objectId = split[0] + "/" + split[1];
        }
 
        if (in.kinematicChainName.empty())
        {
            ARMARX_ERROR << "No kinematic chain candidate set";
            return Skill::Status::Failed;
        }
 
        auto now = armem::Time::now();
        // //////////////////////////////
        // get robot
        // //////////////////////////////
        auto robot = robotReader.getSynchronizedRobot(in.robot, now, VirtualRobot::RobotIO::RobotDescription::eStructure);
 
        auto kinematicChain = robot->getRobotNodeSet(in.kinematicChainName);
        const auto tcpName = kinematicChain->getTCP()->getName();
        const auto kinematicChainJointNames = kinematicChain->getNodeNames();
 
 
        // //////////////////////////////
        // get object pose
        // //////////////////////////////
        auto objInstanceOpt = objectReader.queryObjectByEntityID(in.objectEntityId, now);
        if (!objInstanceOpt)
        {
            ARMARX_ERROR << "Lost object pose.";
            return Skill::Status::Failed;
        }
        auto objInstance = *objInstanceOpt;
 
        // //////////////////////////////
        // Setup vars
        // //////////////////////////////
        auto skillRet = Skill::Status::Succeeded;
 
        auto handRootNodeName = "Hand L Base";
        if (simox::alg::starts_with(in.kinematicChainName, "Right"))
        {
            handRootNodeName = "Hand R Base";
        }
        const FramedPosePtr handRoot = new FramedPose(robot->getRobotNode(handRootNodeName)->getPoseInRootFrame(), robot->getRootNode()->getName(), robot->getName());
        const FramedPosePtr tcp = new FramedPose(robot->getRobotNode(tcpName)->getPoseInRootFrame(), robot->getRootNode()->getName(), robot->getName());
        const Eigen::Matrix4f handRootPoseGlobal = handRoot->toGlobalEigen(robot);
        const Eigen::Matrix4f tcpPoseGlobal = tcp->toGlobalEigen(robot);
 
        const Eigen::Matrix4f tcp2handRoot = tcpPoseGlobal.inverse() * handRootPoseGlobal;
 
 
        // //////////////////////////////
        // Move arm back
        // //////////////////////////////
        {
            auto otherArmKinematicChain = robot->getRobotNodeSet(in.otherArmKinematicChainName);
            auto otherArmJointNames = otherArmKinematicChain->getNodeNames();
 
            ARMARX_CHECK_EQUAL(otherArmJointNames.size(), 7); // TODO
 
            joint_control::arondto::MoveJointsToPositionAcceptedType nextArgs;
            nextArgs.accelerationTime = 500;
            nextArgs.jointMaxSpeed = 0.75;
            nextArgs.jointTargetTolerance = 0.03;
            nextArgs.setVelocitiesToZeroAtEnd = true;
            nextArgs.targetJointMap[otherArmJointNames[0]] = 0.47;
            nextArgs.targetJointMap[otherArmJointNames[1]] = 0.0;
            nextArgs.targetJointMap[otherArmJointNames[2]] = 0.0;
            nextArgs.targetJointMap[otherArmJointNames[3]] = 0.32;
            nextArgs.targetJointMap[otherArmJointNames[4]] = 0.47;
            skillRet = moveJointsToPosition._execute(nextArgs, callback);
        }
 
        if (skillRet != Skill::Status::Succeeded)
        {
            clearLayer();
            return skillRet;
        }
 
        // //////////////////////////////
        // Move platform relative
        // //////////////////////////////
        {
            // not used here
        }
 
        if (skillRet != Skill::Status::Succeeded)
        {
            clearLayer();
            return skillRet;
        }
 
 
        // //////////////////////////////
        // Move TCP to handover pose
        // //////////////////////////////
        {
            now = IceUtil::Time::now();
            robotReader.synchronizeRobot(*robot, now);
 
            FramedPose tcp(robot->getRobotNode(tcpName)->getPoseInRootFrame(), robot->getRootNode()->getName(), robot->getName());
            //const Eigen::Matrix4f robotRoot = robotRootFramedPose.toEigen();
            //const Eigen::Matrix4f robotRootPoseGlobalEigen = robotRootFramedPose.toGlobalEigen(robot);
 
            // query obj target location from grasp
            auto objPoseRobot = objInstance.pose.objectPoseRobot;
            objPoseRobot(1,3) += 40; // move further away
            FramedPose targetObjectFramedPoseRoot(objPoseRobot, robot->getRootNode()->getName(), robot->getName());
            const auto targetObjectPoseGlobalEigen = targetObjectFramedPoseRoot.toGlobalEigen(robot);
 
            const auto transformationFromTCPToObject = objInstance.pose.attachment.poseInFrame;
 
            const auto placePoseGlobal = Eigen::Matrix4f(targetObjectPoseGlobalEigen * transformationFromTCPToObject.inverse());
 
            Eigen::Matrix4f handRootTargetPoseGlobal = placePoseGlobal * tcp2handRoot;
            handRootTargetPoseGlobal(2,3) += 20;
 
            auto l = arviz.layer(layerName);
            auto o = armarx::viz::Robot("grasp");
            o.file("RobotAPI", "RobotAPI/robots/Armar3/ArmarIII-LeftHand.xml"); // TODO!
            o.pose(handRootTargetPoseGlobal);
            l.add(o);
            arviz.commit(l);
 
            {
                tcp_control::arondto::MoveTCPToTargetPoseAcceptedType nextArgs;
                nextArgs.kinematicChainName = in.kinematicChainName;
                nextArgs.orientationalAccuracy = in.orientationalAccuracy;
                nextArgs.positionalAccuracy = in.positionalAccuracy;
                nextArgs.targetPoseGlobal = handRootTargetPoseGlobal;
                skillRet = moveTcpToTargetSkill.execute(nextArgs, callback);
            }
 
            clearLayer();
        }
 
 
        if (skillRet != Skill::Status::Succeeded)
        {
            clearLayer();
            return skillRet;
        }
 
 
        // //////////////////////////////
        // Open hand and detach from memory
        // //////////////////////////////
        {
            now = IceUtil::Time::now();
            robotReader.synchronizeRobot(*robot, now);
 
            FramedPose robotRootFramedPose(robot->getRootNode()->getPoseInRootFrame(), robot->getRootNode()->getName(), robot->getName());
            //const Eigen::Matrix4f robotRoot = robotRootFramedPose.toEigen();
            const Eigen::Matrix4f robotRootGlobal = robotRootFramedPose.toGlobalEigen(robot);
 
            FramedPose objInRobotRootFramedPose(objInstance.pose.objectPoseRobot, robot->getRootNode()->getName(), robot->getName());
            const auto objPoseGlobalEigen = objInRobotRootFramedPose.toGlobalEigen(robot);
            const auto objPoseRootEigen = objInRobotRootFramedPose.toEigen();
 
            //objInstanceUpdate.pose.providerName = description.skillName;
            objInstance.pose.attachmentValid = false;
            objInstance.pose.attachment.resetHard();
            objInstance.pose.objectPoseGlobal = objPoseGlobalEigen;
            objInstance.pose.objectPoseRobot = objPoseRootEigen;
            objInstance.pose.robotPose = robotRootGlobal;
            objInstance.pose.robotConfig = robot->getJointValues();
 
            objectWriter.commitObject(objInstance, objInstance.pose.providerName, IceUtil::Time::now());
 
            skillRet = openHand.execute(nullptr, callback);
        }
 
        if (skillRet != Skill::Status::Succeeded)
        {
            clearLayer();
            return skillRet;
        }
 
 
        // //////////////////////////////
        // Move TCP to retreat pose
        // //////////////////////////////
        {
            const float xRetreatOffset = 90;
            const float yRetreatOffset = 10;
 
            Eigen::Matrix4f offset = Eigen::Matrix4f::Identity();
            offset(0,3) = xRetreatOffset;
            offset(1,3) = yRetreatOffset;
 
            now = IceUtil::Time::now();
            robotReader.synchronizeRobot(*robot, now);
 
            auto TcpPoseGlobalEigen = robot->getRobotNode(tcpName)->getGlobalPose();
            Eigen::Matrix4f newTCPPoseGlobalEigen = TcpPoseGlobalEigen * offset;
 
            tcp_control::arondto::MoveTCPToTargetPoseAcceptedType nextArgs;
            nextArgs.kinematicChainName = in.kinematicChainName;
            nextArgs.orientationalAccuracy = in.orientationalAccuracy;
            nextArgs.positionalAccuracy = in.positionalAccuracy;
            nextArgs.targetPoseGlobal = newTCPPoseGlobalEigen;
            skillRet = moveTcpToTargetSkill.execute(nextArgs, callback);
        }
 
        if (skillRet != Skill::Status::Succeeded)
        {
            clearLayer();
            return skillRet;
        }
 
        // //////////////////////////////
        // Move joints to zero position
        // //////////////////////////////
        {
            joint_control::arondto::MoveJointsToPositionAcceptedType nextArgs;
            nextArgs.accelerationTime = 500;
            nextArgs.jointMaxSpeed = 0.75;
            nextArgs.jointTargetTolerance = 0.03;
            nextArgs.setVelocitiesToZeroAtEnd = true;
            for (const auto& j : kinematicChainJointNames)
            {
                nextArgs.targetJointMap[j] = 0.0;
            }
            for (const auto& j : robot->getRobotNodeSet(in.otherArmKinematicChainName)->getNodeNames())
            {
                nextArgs.targetJointMap[j] = 0.0;
            }
            skillRet = moveJointsToPosition._execute(nextArgs, callback);
        }
 
        clearLayer();
        return skillRet;
    }*/
} // namespace armarx::skills