GraspObjectUtil.cpp

Go to the documentation of this file.

#include <SimoxUtility/algorithm/string/string_tools.h>
#include "GraspObjectUtil.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
    {
        SelectBestKinematicChainForObjectGraspOutput
        SelectBestKinematicChainForObjectGrasp(
            const SelectBestKinematicChainForObjectGraspInput& in)
        {
            SelectBestKinematicChainForObjectGraspOutput out;
            out.distanceToObject = std::numeric_limits<float>::max();
            out.tcpName = "";
            out.kinematicChainName = "";
            int numOfJoints = std::numeric_limits<int>::max();
 
            std::vector<VirtualRobot::RobotNodeSetPtr> possibleNodeSets;
            for (const auto& nodeSet : in.synchronizedRobot.getRobotNodeSets())
            {
                // Here i hardcoded some requrements for the robot nodes. Not sure whether this is true for all our robots
                if (nodeSet->getTCP() and simox::alg::contains(nodeSet->getName(), "Arm") and
                    !simox::alg::contains(nodeSet->getName(), "ColModel") and
                    simox::alg::starts_with(nodeSet->getTCP()->getName(), "TCP"))
                {
                    ARMARX_INFO << "Found possible node set for grasp: " << nodeSet->getName()
                                << ". The TCP is: " << nodeSet->getTCP()->getName();
                    auto nodeSetName = nodeSet->getName();
                    auto tcpName = nodeSet->getTCP()->getName();
                    auto tcpPoseGlobal =
                        in.synchronizedRobot.getRobotNode(tcpName)->getGlobalPose();
                    auto currentNumOfJoints = nodeSet->size();
 
                    auto distance = (in.objectPose.objectPoseGlobal.block<3, 1>(0, 3) -
                                     tcpPoseGlobal.block<3, 1>(0, 3))
                                        .norm();
 
                    if ((tcpName == out.tcpName &&
                         numOfJoints >
                             (int)
                                 currentNumOfJoints) or // We found a better kinematic chain with less nodes in it but it has the same tcp
                        (distance < out.distanceToObject)) // the distance is better of that tcp
                    {
                        ARMARX_INFO << "Deciding to use this kinematic chain: " << VAROUT(tcpName)
                                    << ", " << VAROUT(distance) << " <-> "
                                    << VAROUT(out.distanceToObject) << ", "
                                    << VAROUT(currentNumOfJoints) << " <-> " << VAROUT(numOfJoints);
                        out.kinematicChainName = nodeSetName;
                        out.tcpName = tcpName;
                        out.distanceToObject = distance;
                        numOfJoints = currentNumOfJoints;
                    }
                }
            }
            return out;
        }
 
        SelectBestGraspSetForObjectOutput
        SelectBestGraspSetForObject(const SelectBestGraspSetForObjectInput& in)
        {
            SelectBestGraspSetForObjectOutput out;
            out.graspSetName = "";
 
            for (const auto& [name, gs] : in.graspInfo.graspSets)
            {
                // Check if grasp is applicable
                if (gs.endeffector != in.tcpName)
                {
                    continue;
                }
 
                // GraspSet is ok
                out.graspSetName = name;
                out.graspSet = gs;
            }
            return out;
        }
 
        GetPlatformOffsetForObjectGraspOutput
        GetGlobalPlatformPoseForObjectGrasp(const GetPlatformOffsetForObjectGraspInput& in)
        {
            GetPlatformOffsetForObjectGraspOutput out;
 
            auto tcpPoseLocal = in.synchronizedRobot.getRobotNode(in.tcpName)->getPoseInRootFrame();
            auto targetPlatformPoseLocal =
                in.synchronizedRobot.getRootNode()->toLocalCoordinateSystem(
                    in.objectPose.objectPoseGlobal);
 
            targetPlatformPoseLocal(1, 3) += -600;
            if (tcpPoseLocal(0, 3) < 0) // Left Hand
            {
                targetPlatformPoseLocal(0, 3) += 100;
            }
            else
            {
                targetPlatformPoseLocal(0, 3) += -100;
            }
 
            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 grasp an object the robot should drive from " << in.synchronizedRobot.getRootNode()->getGlobalPose() << " to " << out.platformGlobalPose << "(distance: " << out.distanceToDrive << ").";
            return out;
        }
 
        GetPlatformOffsetAfterObjectGraspOutput
        GetGlobalPlatformPoseAfterObjectGrasp(const GetPlatformOffsetAfterObjectGraspInput& in)
        {
            GetPlatformOffsetAfterObjectGraspOutput out;
 
            Eigen::Matrix4f movePlatformBack = Eigen::Matrix4f::Identity();
            movePlatformBack(1, 3) = -400;
 
            out.platformGlobalPose =
                in.synchronizedRobot.getRootNode()->getGlobalPose() * movePlatformBack;
            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 retreat from a grasp an object the robot should drive from " << in.synchronizedRobot.getRootNode()->getGlobalPose() << " to " << out.platformGlobalPose << "(distance: " << out.distanceToDrive << ").";
            return out;
        }
    } // namespace grasp_control::util
} // namespace armarx::skills