box_to_grasp_candidates.h

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Dense>
 
#include <SimoxUtility/shapes/OrientedBox.h>
#include <SimoxUtility/meta/eigen/enable_if_compile_time_size.h>
 
#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
 
#include "GraspCandidateHelper.h"
 
namespace armarx
{
    struct box_to_grasp_candidates
    {
        using box_t = simox::OrientedBox<float>;
        enum class box_alignment : std::uint_fast8_t
        {
            pos_x_pos_y, pos_x_pos_z,
            pos_x_neg_y, pos_x_neg_z,
 
            neg_x_pos_y, neg_x_pos_z,
            neg_x_neg_y, neg_x_neg_z,
 
            pos_y_pos_x, pos_y_pos_z,
            pos_y_neg_x, pos_y_neg_z,
 
            neg_y_pos_x, neg_y_pos_z,
            neg_y_neg_x, neg_y_neg_z,
 
            pos_z_pos_x, pos_z_pos_y,
            pos_z_neg_x, pos_z_neg_y,
 
            neg_z_pos_x, neg_z_pos_y,
            neg_z_neg_x, neg_z_neg_y
        };
        struct side_enabled_map
        {
            bool& operator[](box_alignment al)
            {
                return enabled.at(static_cast<std::uint_fast8_t>(al));
            }
            bool& operator[](std::uint_fast8_t al)
            {
                return enabled.at(al);
            }
            bool operator[](box_alignment al) const
            {
                return enabled.at(static_cast<std::uint_fast8_t>(al));
            }
            bool operator[](std::uint_fast8_t al) const
            {
                return enabled.at(al);
            }
            std::array<bool, 24> enabled
            {
                0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0
            };
        };
 
        struct length_limit
        {
            float transverse_length_min =   50;
            float transverse_length_max = 1500;
            float upward_length_min     =   25;
            float upward_length_max     =  500;
            float forward_length_min    =   50;
            float forward_length_max    =  150;
 
            bool in_limits(const box_t& box, box_alignment align) const;
            std::string limit_violation_string(const box_t& box, box_alignment align) const;
        };
        struct side_data
        {
            std::string                       name;
            armarx::RobotNameHelper::Arm      nh_arm;
            armarx::RobotNameHelper::RobotArm nh_arm_w_rob;
            Eigen::Vector3f                   tcp_shift       = Eigen::Vector3f::Constant(std::nanf(""));
            Eigen::Vector3f                   hand_up         = Eigen::Vector3f::Constant(std::nanf(""));
            Eigen::Vector3f                   hand_transverse = Eigen::Vector3f::Constant(std::nanf(""));
        };
        struct grasp
        {
            Eigen::Matrix4f pose     = Eigen::Matrix4f::Constant(std::nanf(""));
            Eigen::Vector3f approach = Eigen::Vector3f::Constant(std::nanf(""));
            const side_data* side    = nullptr;
 
            armarx::grasping::GraspCandidatePtr make_candidate(
                const std::string& provider_name
            ) const;
            armarx::grasping::GraspCandidatePtr make_candidate(
                const std::string& provider_name,
                const std::string& side_name,
                const VirtualRobot::RobotPtr& robot = nullptr
            ) const;
            armarx::grasping::GraspCandidatePtr make_candidate(
                const std::string& provider_name,
                const std::string& side_name,
                const Eigen::Matrix4f& global_pose
            ) const;
        };
    public:
        box_to_grasp_candidates(armarx::RobotNameHelperPtr rnh,
                                const VirtualRobot::RobotPtr& robot);
 
        void add_armar6_defaults();
        //extract
    public:
        static Eigen::Vector3f box_transverse_axis(const box_t& box, box_alignment align);
        static float box_transverse_len(const box_t& box, box_alignment align);
 
        static Eigen::Vector3f box_up_axis(const box_t& box, box_alignment align);
        static float box_up_len(const box_t& box, box_alignment align);
 
        static float box_forward_len(const box_t& box, box_alignment align);
        //side
    public:
        const side_data& side(const std::string& side) const;
        side_data& side(const std::string& side);
        const std::map<std::string, side_data>& sides() const;
 
        //center_grasp
    public:
        void center_grasps(const box_t& box,
                           const side_data& side,
                           const length_limit& limit,
                           auto consume_grasp);
        void center_grasps(const box_t& box,
                           const std::string& side,
                           const length_limit& limit,
                           auto consume_grasp);
        void center_grasps(const box_t& box,
                           const side_data& side,
                           const length_limit& limit,
                           auto consume_grasp,
                           const side_enabled_map& enabled);
        void center_grasps(const box_t& box,
                           const std::string& side,
                           const length_limit& limit,
                           auto consume_grasp,
                           const side_enabled_map& enabled);
 
        grasp center_grasp(const box_t& box,
                           const side_data& side,
                           box_alignment align);
 
        template<class D1, class D2, class D3>
        static
        std::enable_if_t <
        simox::meta::is_vec3_v<D1>&&
        simox::meta::is_vec3_v<D2>&&
        simox::meta::is_vec3_v<D3>, grasp >
        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);
 
    private:
        armarx::RobotNameHelperPtr       _rnh;
        VirtualRobot::RobotPtr           _robot;
        std::map<std::string, side_data> _sides;
    };
}
 
#include "box_to_grasp_candidates.ipp"