box_to_grasp_candidates.h

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Dense>
 
#include <SimoxUtility/meta/eigen/enable_if_compile_time_size.h>
#include <SimoxUtility/shapes/OrientedBox.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;
    };
} // namespace armarx
 
#include "box_to_grasp_candidates.ipp"