PutdownObjectUtil.cpp

Go to the documentation of this file.

#include "PutdownObjectUtil.h"
 
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/Grasping/GraspSet.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
namespace armarx::skills
{
    namespace grasp_control::util
    {
        GetPlatformOffsetForObjectPutdownOutput GetGlobalPlatformPoseForObjectPutdown(const GetPlatformOffsetForObjectPutdownInput& in)
        {
            GetPlatformOffsetForObjectPutdownOutput out;
 
            auto tcpPoseLocal = in.synchronizedRobot.getRobotNode(in.tcpName)->getPoseInRootFrame();
            Eigen::Matrix4f targetPlatformPoseLocal = Eigen::Matrix4f::Identity();
 
            targetPlatformPoseLocal(1,3) += 190;
            if (tcpPoseLocal(0,3) < 0) // Left Hand
            {
                targetPlatformPoseLocal(0,3) += 0;
            }
            else
            {
                targetPlatformPoseLocal(0,3) += -0;
            }
 
            out.platformGlobalPose = in.synchronizedRobot.getRootNode()->getGlobalPose() * targetPlatformPoseLocal;
            out.distanceToDrive = (in.synchronizedRobot.getRootNode()->getGlobalPose().block<3,1>(0,3) - out.platformGlobalPose.block<3,1>(0,3)).norm(); // only calc cartesian distance
 
            //ARMARX_INFO << "In order to putdown an object the robot should drive from " << in.synchronizedRobot.getRootNode()->getGlobalPose() << " to " << out.platformGlobalPose << "(distance: " << out.distanceToDrive << ").";
            return out;
        }
 
        GetPlatformOffsetAfterObjectPutdownOutput GetGlobalPlatformPoseAfterObjectPutdown(const GetPlatformOffsetAfterObjectPutdownInput& in)
        {
            GetPlatformOffsetAfterObjectPutdownOutput out;
 
            Eigen::Matrix4f targetPlatformPoseLocal = Eigen::Matrix4f::Identity();
            targetPlatformPoseLocal(1,3) += -190;
 
            out.platformGlobalPose = in.synchronizedRobot.getRootNode()->getGlobalPose() * targetPlatformPoseLocal;
            out.distanceToDrive = (in.synchronizedRobot.getRootNode()->getGlobalPose().block<3,1>(0,3) - out.platformGlobalPose.block<3,1>(0,3)).norm(); // only calc cartesian distance
 
            return out;
        }
    }
}