box_to_grasp_candidates.cpp

Go to the documentation of this file.

#include <sstream>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include "box_to_grasp_candidates.h"
 
namespace armarx
{
    void box_to_grasp_candidates::add_armar6_defaults()
    {
        auto& l = side("Left");
        auto& r = side("Right");
 
        l.hand_transverse = Eigen::Vector3f{0, 1, 0};
        r.hand_transverse = Eigen::Vector3f{0, 1, 0};
        l.hand_up         = Eigen::Vector3f{-1, 0, -1};
        r.hand_up         = Eigen::Vector3f{+1, 0, -1};
        l.tcp_shift       = Eigen::Vector3f{0, -30, 0};
        r.tcp_shift       = Eigen::Vector3f{0, -30, 0};
    }
 
    Eigen::Vector3f
    box_to_grasp_candidates::box_transverse_axis(const box_t& box, box_alignment align)
    {
        // *INDENT-OFF*
        switch (align)
        {
            case box_alignment::pos_y_pos_x: [[fallthrough]];
            case box_alignment::neg_y_pos_x: [[fallthrough]];
            case box_alignment::pos_z_pos_x: [[fallthrough]];
            case box_alignment::neg_z_pos_x: return box.axis_x();
 
            case box_alignment::pos_y_neg_x: [[fallthrough]];
            case box_alignment::neg_y_neg_x: [[fallthrough]];
            case box_alignment::pos_z_neg_x: [[fallthrough]];
            case box_alignment::neg_z_neg_x: return -box.axis_x();
 
            case box_alignment::pos_x_pos_y: [[fallthrough]];
            case box_alignment::neg_x_pos_y: [[fallthrough]];
            case box_alignment::pos_z_pos_y: [[fallthrough]];
            case box_alignment::neg_z_pos_y: return box.axis_y();
 
            case box_alignment::pos_x_neg_y: [[fallthrough]];
            case box_alignment::neg_x_neg_y: [[fallthrough]];
            case box_alignment::pos_z_neg_y: [[fallthrough]];
            case box_alignment::neg_z_neg_y: return -box.axis_y();
 
            case box_alignment::pos_x_pos_z: [[fallthrough]];
            case box_alignment::neg_x_pos_z: [[fallthrough]];
            case box_alignment::pos_y_pos_z: [[fallthrough]];
            case box_alignment::neg_y_pos_z: return box.axis_z();
 
            case box_alignment::pos_x_neg_z: [[fallthrough]];
            case box_alignment::neg_x_neg_z: [[fallthrough]];
            case box_alignment::pos_y_neg_z: [[fallthrough]];
            case box_alignment::neg_y_neg_z: return -box.axis_z();
        }
        // *INDENT-ON*
        ARMARX_TRACE;
        throw std::logic_error{"reached unreachable code"};
    }
 
    float
    box_to_grasp_candidates::box_transverse_len(const box_t& box, box_alignment align)
    {
        // *INDENT-OFF*
        switch (align)
        {
            case box_alignment::pos_y_pos_x: [[fallthrough]];
            case box_alignment::neg_y_pos_x: [[fallthrough]];
            case box_alignment::pos_z_pos_x: [[fallthrough]];
            case box_alignment::neg_z_pos_x: [[fallthrough]];
 
            case box_alignment::pos_y_neg_x: [[fallthrough]];
            case box_alignment::neg_y_neg_x: [[fallthrough]];
            case box_alignment::pos_z_neg_x: [[fallthrough]];
            case box_alignment::neg_z_neg_x: return box.dimension_x();
 
            case box_alignment::pos_x_pos_y: [[fallthrough]];
            case box_alignment::neg_x_pos_y: [[fallthrough]];
            case box_alignment::pos_z_pos_y: [[fallthrough]];
            case box_alignment::neg_z_pos_y: [[fallthrough]];
 
            case box_alignment::pos_x_neg_y: [[fallthrough]];
            case box_alignment::neg_x_neg_y: [[fallthrough]];
            case box_alignment::pos_z_neg_y: [[fallthrough]];
            case box_alignment::neg_z_neg_y: return box.dimension_y();
 
            case box_alignment::pos_x_pos_z: [[fallthrough]];
            case box_alignment::neg_x_pos_z: [[fallthrough]];
            case box_alignment::pos_y_pos_z: [[fallthrough]];
            case box_alignment::neg_y_pos_z: [[fallthrough]];
 
            case box_alignment::pos_x_neg_z: [[fallthrough]];
            case box_alignment::neg_x_neg_z: [[fallthrough]];
            case box_alignment::pos_y_neg_z: [[fallthrough]];
            case box_alignment::neg_y_neg_z: return box.dimension_z();
        }
        // *INDENT-ON*
        ARMARX_TRACE;
        throw std::logic_error{"reached unreachable code"};
    }
 
    Eigen::Vector3f
    box_to_grasp_candidates::box_up_axis(const box_t& box, box_alignment align)
    {
        // *INDENT-OFF*
        switch (align)
        {
            case box_alignment::pos_x_pos_y: [[fallthrough]];
            case box_alignment::pos_x_pos_z: [[fallthrough]];
            case box_alignment::pos_x_neg_y: [[fallthrough]];
            case box_alignment::pos_x_neg_z: return box.axis_x();
 
            case box_alignment::neg_x_pos_y: [[fallthrough]];
            case box_alignment::neg_x_pos_z: [[fallthrough]];
            case box_alignment::neg_x_neg_y: [[fallthrough]];
            case box_alignment::neg_x_neg_z: return -box.axis_x();
 
            case box_alignment::pos_y_pos_x: [[fallthrough]];
            case box_alignment::pos_y_pos_z: [[fallthrough]];
            case box_alignment::pos_y_neg_x: [[fallthrough]];
            case box_alignment::pos_y_neg_z: return box.axis_y();
 
            case box_alignment::neg_y_pos_x: [[fallthrough]];
            case box_alignment::neg_y_pos_z: [[fallthrough]];
            case box_alignment::neg_y_neg_x: [[fallthrough]];
            case box_alignment::neg_y_neg_z: return -box.axis_y();
 
            case box_alignment::pos_z_pos_x: [[fallthrough]];
            case box_alignment::pos_z_pos_y: [[fallthrough]];
            case box_alignment::pos_z_neg_x: [[fallthrough]];
            case box_alignment::pos_z_neg_y: return box.axis_z();
 
            case box_alignment::neg_z_pos_x: [[fallthrough]];
            case box_alignment::neg_z_pos_y: [[fallthrough]];
            case box_alignment::neg_z_neg_x: [[fallthrough]];
            case box_alignment::neg_z_neg_y: return -box.axis_z();
        }
        // *INDENT-ON*
        ARMARX_TRACE;
        throw std::logic_error{"reached unreachable code"};
    }
 
    float
    box_to_grasp_candidates::box_up_len(const box_t& box, box_alignment align)
    {
        // *INDENT-OFF*
        switch (align)
        {
            case box_alignment::pos_x_pos_y: [[fallthrough]];
            case box_alignment::pos_x_pos_z: [[fallthrough]];
            case box_alignment::pos_x_neg_y: [[fallthrough]];
            case box_alignment::pos_x_neg_z: [[fallthrough]];
 
            case box_alignment::neg_x_pos_y: [[fallthrough]];
            case box_alignment::neg_x_pos_z: [[fallthrough]];
            case box_alignment::neg_x_neg_y: [[fallthrough]];
            case box_alignment::neg_x_neg_z: return box.dimension_x();
 
            case box_alignment::pos_y_pos_x: [[fallthrough]];
            case box_alignment::pos_y_pos_z: [[fallthrough]];
            case box_alignment::pos_y_neg_x: [[fallthrough]];
            case box_alignment::pos_y_neg_z: [[fallthrough]];
 
            case box_alignment::neg_y_pos_x: [[fallthrough]];
            case box_alignment::neg_y_pos_z: [[fallthrough]];
            case box_alignment::neg_y_neg_x: [[fallthrough]];
            case box_alignment::neg_y_neg_z: return box.dimension_y();
 
            case box_alignment::pos_z_pos_x: [[fallthrough]];
            case box_alignment::pos_z_pos_y: [[fallthrough]];
            case box_alignment::pos_z_neg_x: [[fallthrough]];
            case box_alignment::pos_z_neg_y: [[fallthrough]];
 
            case box_alignment::neg_z_pos_x: [[fallthrough]];
            case box_alignment::neg_z_pos_y: [[fallthrough]];
            case box_alignment::neg_z_neg_x: [[fallthrough]];
            case box_alignment::neg_z_neg_y: return box.dimension_z();
        }
        // *INDENT-ON*
        ARMARX_TRACE;
        throw std::logic_error{"reached unreachable code"};
    }
 
    float
    box_to_grasp_candidates::box_forward_len(const box_t& box, box_alignment align)
    {
        // *INDENT-OFF*
        switch (align)
        {
            case box_alignment::pos_y_pos_z: [[fallthrough]];
            case box_alignment::pos_y_neg_z: [[fallthrough]];
            case box_alignment::neg_y_pos_z: [[fallthrough]];
            case box_alignment::neg_y_neg_z: [[fallthrough]];
 
            case box_alignment::pos_z_pos_y: [[fallthrough]];
            case box_alignment::pos_z_neg_y: [[fallthrough]];
            case box_alignment::neg_z_pos_y: [[fallthrough]];
            case box_alignment::neg_z_neg_y: return box.dimension_x();
 
            case box_alignment::pos_x_pos_z: [[fallthrough]];
            case box_alignment::pos_x_neg_z: [[fallthrough]];
            case box_alignment::neg_x_pos_z: [[fallthrough]];
            case box_alignment::neg_x_neg_z: [[fallthrough]];
 
            case box_alignment::pos_z_pos_x: [[fallthrough]];
            case box_alignment::pos_z_neg_x: [[fallthrough]];
            case box_alignment::neg_z_pos_x: [[fallthrough]];
            case box_alignment::neg_z_neg_x: return box.dimension_y();
 
            case box_alignment::pos_x_pos_y: [[fallthrough]];
            case box_alignment::pos_x_neg_y: [[fallthrough]];
            case box_alignment::neg_x_pos_y: [[fallthrough]];
            case box_alignment::neg_x_neg_y: [[fallthrough]];
 
            case box_alignment::pos_y_pos_x: [[fallthrough]];
            case box_alignment::pos_y_neg_x: [[fallthrough]];
            case box_alignment::neg_y_pos_x: [[fallthrough]];
            case box_alignment::neg_y_neg_x: return box.dimension_z();
        }
        // *INDENT-ON*
        ARMARX_TRACE;
        throw std::logic_error{"reached unreachable code"};
    }
 
    bool box_to_grasp_candidates::length_limit::in_limits(const box_to_grasp_candidates::box_t& box, box_to_grasp_candidates::box_alignment align) const
    {
        const auto t_len = box_transverse_len(box, align);
        const auto u_len = box_up_len(box, align);
        const auto f_len = box_forward_len(box, align);
        return t_len < transverse_length_max &&
               t_len > transverse_length_min &&
               u_len < upward_length_max &&
               u_len > upward_length_min &&
               f_len < forward_length_max &&
               f_len > forward_length_min;
    }
 
    std::string box_to_grasp_candidates::length_limit::limit_violation_string(const box_t& box, box_alignment align) const
    {
        const auto t_len = box_transverse_len(box, align);
        const auto u_len = box_up_len(box, align);
        const auto f_len = box_forward_len(box, align);
        std::stringstream s;
        if (!(t_len < transverse_length_max && t_len > transverse_length_min))
        {
            s << "tr: " << t_len << " [" << transverse_length_min << ", " << transverse_length_max << "] ";
        }
        if (!(u_len < upward_length_max && u_len > upward_length_min))
        {
            s << "up: " << u_len << " [" << upward_length_min << ", " << upward_length_max << "] ";
        }
        if (!(f_len < forward_length_max && f_len > forward_length_min))
        {
            s << "fw: " << f_len << " [" << forward_length_min << ", " << forward_length_max << "]";
        }
        return s.str();
    }
 
    armarx::grasping::GraspCandidatePtr
    box_to_grasp_candidates::grasp::make_candidate(
        const std::string& provider_name
    ) const
    {
        ARMARX_CHECK_EXPRESSION(side && side->nh_arm_w_rob.getRobot());
        return make_candidate(provider_name, side->name,
                              side->nh_arm_w_rob.getRobot()->getGlobalPose());
    }
    armarx::grasping::GraspCandidatePtr
    box_to_grasp_candidates::grasp::make_candidate(
        const std::string& provider_name,
        const std::string& side_name,
        const VirtualRobot::RobotPtr& robot
    ) const
    {
        ARMARX_CHECK_NOT_NULL(robot);
        return make_candidate(provider_name, side_name, robot->getGlobalPose());
    }
    armarx::grasping::GraspCandidatePtr
    box_to_grasp_candidates::grasp::make_candidate(
        const std::string& provider_name,
        const std::string& side_name,
        const Eigen::Matrix4f& global_pose
    ) const
    {
        armarx::grasping::GraspCandidatePtr gc = new armarx::grasping::GraspCandidate;
        gc->robotPose = new armarx::Pose(global_pose);
        gc->graspPose = new armarx::Pose(pose);
        gc->providerName = provider_name;
        gc->side = side_name;
        gc->approachVector = new armarx::Vector3(approach);
        gc->objectType = armarx::objpose::UnknownObject;
        gc->sourceFrame = "root";
        gc->targetFrame = "root";
        return gc;
    }
 
}
 
namespace armarx
{
    box_to_grasp_candidates::box_to_grasp_candidates(armarx::RobotNameHelperPtr rnh, const VirtualRobot::RobotPtr& robot)
        : _rnh{std::move(rnh)}, _robot{robot}
    {
        ARMARX_CHECK_NOT_NULL(_robot);
    }
 
    const box_to_grasp_candidates::side_data& box_to_grasp_candidates::side(const std::string& side) const
    {
        return _sides.at(side);
    }
 
    box_to_grasp_candidates::side_data& box_to_grasp_candidates::side(const std::string& side)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(_robot);
        if (_sides.count(side))
        {
            return  _sides.at(side);
        }
        auto& data = _sides[side];
        data.name         = side;
        data.nh_arm       = _rnh->getArm(side);
        data.nh_arm_w_rob = data.nh_arm.addRobot(_robot);
        return data;
    }
 
    const std::map<std::string, box_to_grasp_candidates::side_data>& box_to_grasp_candidates::sides() const
    {
        return _sides;
    }
 
    box_to_grasp_candidates::grasp box_to_grasp_candidates::center_grasp(
        const simox::OrientedBox<float>& box,
        const side_data& side,
        box_alignment align)
    {
        ARMARX_TRACE;
        const auto g = center_grasp(
                           box.center(),
 
                           box_up_axis(box, align),
                           box_up_len(box, align),
                           side.hand_up,
 
                           box_transverse_axis(box, align),
                           side.hand_transverse,
 
                           side.tcp_shift);
        return {g.pose, g.approach, &side};
    }
}