PutdownObjectUtil.cpp

Go to the documentation of this file.

#include "PutdownObjectUtil.h"
 
#include <VirtualRobot/Grasping/GraspSet.h>
#include <VirtualRobot/ManipulationObject.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;
        }
    } // namespace grasp_control::util
} // namespace armarx::skills