box_to_grasp_candidates.ipp

Go to the documentation of this file.

#pragma once
 
#include <SimoxUtility/math/distance/angle_between.h>
#include <SimoxUtility/math/project_to_plane.h>
#include <SimoxUtility/math/convert/pos_mat3f_to_mat4f.h>
#include <SimoxUtility/meta/enum/adapt_enum.h>
 
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <ArmarXCore/core/logging/Logging.h>
 
 
namespace simox::meta
{
    template<>
    const simox::meta::EnumNames<armarx::box_to_grasp_candidates::box_alignment>
    enum_names<armarx::box_to_grasp_candidates::box_alignment>
    {
        {armarx::box_to_grasp_candidates::box_alignment::pos_y_pos_x, "pos_y_pos_x"},
        {armarx::box_to_grasp_candidates::box_alignment::neg_y_pos_x, "neg_y_pos_x"},
        {armarx::box_to_grasp_candidates::box_alignment::pos_z_pos_x, "pos_z_pos_x"},
        {armarx::box_to_grasp_candidates::box_alignment::neg_z_pos_x, "neg_z_pos_x"},
 
        {armarx::box_to_grasp_candidates::box_alignment::pos_y_neg_x, "pos_y_neg_x"},
        {armarx::box_to_grasp_candidates::box_alignment::neg_y_neg_x, "neg_y_neg_x"},
        {armarx::box_to_grasp_candidates::box_alignment::pos_z_neg_x, "pos_z_neg_x"},
        {armarx::box_to_grasp_candidates::box_alignment::neg_z_neg_x, "neg_z_neg_x"},
 
        {armarx::box_to_grasp_candidates::box_alignment::pos_x_pos_y, "pos_x_pos_y"},
        {armarx::box_to_grasp_candidates::box_alignment::neg_x_pos_y, "neg_x_pos_y"},
        {armarx::box_to_grasp_candidates::box_alignment::pos_z_pos_y, "pos_z_pos_y"},
        {armarx::box_to_grasp_candidates::box_alignment::neg_z_pos_y, "neg_z_pos_y"},
 
        {armarx::box_to_grasp_candidates::box_alignment::pos_x_neg_y, "pos_x_neg_y"},
        {armarx::box_to_grasp_candidates::box_alignment::neg_x_neg_y, "neg_x_neg_y"},
        {armarx::box_to_grasp_candidates::box_alignment::pos_z_neg_y, "pos_z_neg_y"},
        {armarx::box_to_grasp_candidates::box_alignment::neg_z_neg_y, "neg_z_neg_y"},
 
        {armarx::box_to_grasp_candidates::box_alignment::pos_x_pos_z, "pos_x_pos_z"},
        {armarx::box_to_grasp_candidates::box_alignment::neg_x_pos_z, "neg_x_pos_z"},
        {armarx::box_to_grasp_candidates::box_alignment::pos_y_pos_z, "pos_y_pos_z"},
        {armarx::box_to_grasp_candidates::box_alignment::neg_y_pos_z, "neg_y_pos_z"},
 
        {armarx::box_to_grasp_candidates::box_alignment::pos_x_neg_z, "pos_x_neg_z"},
        {armarx::box_to_grasp_candidates::box_alignment::neg_x_neg_z, "neg_x_neg_z"},
        {armarx::box_to_grasp_candidates::box_alignment::pos_y_neg_z, "pos_y_neg_z"},
        {armarx::box_to_grasp_candidates::box_alignment::neg_y_neg_z, "neg_y_neg_z"}
    };
}
 
namespace armarx
{
    template<class D1, class D2, class D3>
    inline
    std::enable_if_t <
    simox::meta::is_vec3_v<D1>&&
    simox::meta::is_vec3_v<D2>&&
    simox::meta::is_vec3_v<D3>, box_to_grasp_candidates::grasp >
    box_to_grasp_candidates::center_grasp(
        const Eigen::MatrixBase<D1>& center_pos,
        const Eigen::MatrixBase<D2>& up, float up_len, const Eigen::Vector3f& hand_up,
        const Eigen::MatrixBase<D3>& transverse,       const Eigen::Vector3f& hand_transv,
        const Eigen::Vector3f& tcp_shift)
    {
        const Eigen::Vector3f up_normalized = up.normalized();
        ARMARX_TRACE;
        const Eigen::Vector3f ax = hand_up.cross(up).normalized();
        const float ang_u = simox::math::angle_between_vec3f_vec3f(hand_up, up_normalized, ax);
        const Eigen::Matrix3f upward_ori = Eigen::AngleAxisf(ang_u, ax).toRotationMatrix();
 
 
        ARMARX_TRACE;
        const Eigen::Vector3f hand_transv_glob = upward_ori * hand_transv;
        const Eigen::Vector3f hand_transv_glob_flat = simox::math::project_to_plane(hand_transv_glob, up_normalized);
        const Eigen::Vector3f transverse_flat  = simox::math::project_to_plane(transverse, up_normalized);
        const float ang = simox::math::angle_between_vec3f_vec3f(
                              hand_transv_glob_flat,
                              transverse_flat,
                              up_normalized
                          );
        const Eigen::Matrix3f transv_align = Eigen::AngleAxisf{ang, up_normalized.normalized()}.toRotationMatrix();
 
        const Eigen::Matrix3f gc_ori = transv_align * upward_ori;
        const Eigen::Vector3f gc_pos = center_pos + up_normalized * up_len / 2;
        return
        {
            simox::math::pos_mat3f_to_mat4f(
                gc_pos + gc_ori * tcp_shift,
                gc_ori
            ),
            -up_normalized,
            nullptr
        };
    }
 
    inline
    void box_to_grasp_candidates::center_grasps(
        const box_t& box,
        const side_data& side,
        const length_limit& limit,
        auto consume_grasp)
    {
        for (std::uint_fast8_t i = 0; i < 24; ++i)
        {
            const auto align = static_cast<box_alignment>(i);
            if (limit.in_limits(box, align))
            {
                ARMARX_VERBOSE << "align ok            " << std::to_string(align);
                consume_grasp(center_grasp(box, side, align));
            }
            else
            {
                ARMARX_VERBOSE << "align out of bounds " << std::to_string(align)
                               << limit.limit_violation_string(box, align);
            }
        }
    }
    inline
    void box_to_grasp_candidates::center_grasps(
        const box_t& box,
        const side_data& side,
        const length_limit& limit,
        auto consume_grasp,
        const side_enabled_map& enabled)
    {
        for (std::uint_fast8_t i = 0; i < 24; ++i)
        {
            const auto align = static_cast<box_alignment>(i);
            if (limit.in_limits(box, align) && enabled[align])
            {
                ARMARX_VERBOSE << "align ok            " << std::to_string(align);
                consume_grasp(center_grasp(box, side, align));
            }
            else if (!enabled[align])
            {
                ARMARX_VERBOSE << "align disabled      " << std::to_string(align);
            }
            else
            {
                ARMARX_VERBOSE << "align out of bounds " << std::to_string(align)
                               << limit.limit_violation_string(box, align);
            }
        }
    }
    inline
    void box_to_grasp_candidates::center_grasps(
        const box_t& box,
        const std::string& s,
        const length_limit& limit,
        auto consume_grasp)
    {
        center_grasps(box, side(s), limit, std::move(consume_grasp));
    }
    inline
    void box_to_grasp_candidates::center_grasps(
        const box_t& box,
        const std::string& s,
        const length_limit& limit,
        auto consume_grasp,
        const side_enabled_map& enabled)
    {
        center_grasps(box, side(s), limit, std::move(consume_grasp), enabled);
    }
}