BoxToGraspCandidatesWidgetController.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * \package    ARCHES::gui-plugins::BoxToGraspCandidatesWidgetController
 * \author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * \date       2020
 * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "BoxToGraspCandidatesWidgetController.h"
 
#include <string>
 
#include <SimoxUtility/math/convert/pos_mat3f_to_mat4f.h>
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
#include <SimoxUtility/math/distance/angle_between.h>
#include <SimoxUtility/math/project_to_plane.h>
#include <SimoxUtility/shapes/OrientedBox.h>
 
#include <RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h>
#include <RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.h>
#include <RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.h>
 
// configure
namespace armarx
{
    void
    BoxToGraspCandidatesWidgetController::loadSettings(QSettings* settings)
    {
        getRobotStateComponentPlugin().setRobotStateComponentName(
            settings->value("rsc", "Armar6StateComponent").toString().toStdString());
        _gc_topic_name = settings->value("rsc", "GraspCandidatesTopic").toString().toStdString();
    }
 
    void
    BoxToGraspCandidatesWidgetController::saveSettings(QSettings* settings)
    {
        settings->setValue(
            "rsc",
            QString::fromStdString(getRobotStateComponentPlugin().getRobotStateComponentName()));
        settings->setValue("gct", QString::fromStdString(_gc_topic_name));
    }
 
    QPointer<QDialog>
    BoxToGraspCandidatesWidgetController::getConfigDialog(QWidget* parent)
    {
        if (!_dialog)
        {
            _dialog = new SimpleConfigDialog(parent);
            _dialog->addProxyFinder<RobotStateComponentInterfacePrx>(
                "rsc", "Robot State Component", "*Component");
            _dialog->addLineEdit("gct", "Grasp Candidates Topic", "GraspCandidatesTopic");
        }
        return qobject_cast<SimpleConfigDialog*>(_dialog);
    }
 
    void
    BoxToGraspCandidatesWidgetController::configured()
    {
        getRobotStateComponentPlugin().setRobotStateComponentName(_dialog->getProxyName("rsc"));
        _gc_topic_name = _dialog->getLineEditText("gct");
    }
} // namespace armarx
 
namespace armarx
{
    BoxToGraspCandidatesWidgetController::BoxToGraspCandidatesWidgetController()
    {
        _ui.setupUi(getWidget());
 
        _box_pose.setXYZWidgets(
            _ui.doubleSpinBoxBoxTX, _ui.doubleSpinBoxBoxTY, _ui.doubleSpinBoxBoxTZ);
 
        _box_pose.setRPYWidgets(
            _ui.doubleSpinBoxBoxRX, _ui.doubleSpinBoxBoxRY, _ui.doubleSpinBoxBoxRZ);
 
        _box_extend.addWidget(_ui.doubleSpinBoxBoxSizeX);
        _box_extend.addWidget(_ui.doubleSpinBoxBoxSizeY);
        _box_extend.addWidget(_ui.doubleSpinBoxBoxSizeZ);
 
        _transversal_l.addWidget(_ui.doubleSpinBoxHandLTransvX);
        _transversal_l.addWidget(_ui.doubleSpinBoxHandLTransvY);
        _transversal_l.addWidget(_ui.doubleSpinBoxHandLTransvZ);
 
        _transversal_r.addWidget(_ui.doubleSpinBoxHandRTransvX);
        _transversal_r.addWidget(_ui.doubleSpinBoxHandRTransvY);
        _transversal_r.addWidget(_ui.doubleSpinBoxHandRTransvZ);
 
        _up_l.addWidget(_ui.doubleSpinBoxHandLUpX);
        _up_l.addWidget(_ui.doubleSpinBoxHandLUpY);
        _up_l.addWidget(_ui.doubleSpinBoxHandLUpZ);
 
        _up_r.addWidget(_ui.doubleSpinBoxHandRUpX);
        _up_r.addWidget(_ui.doubleSpinBoxHandRUpY);
        _up_r.addWidget(_ui.doubleSpinBoxHandRUpZ);
 
        _tcp_shift_l.addWidget(_ui.doubleSpinBoxTCPShiftLX);
        _tcp_shift_l.addWidget(_ui.doubleSpinBoxTCPShiftLY);
        _tcp_shift_l.addWidget(_ui.doubleSpinBoxTCPShiftLZ);
 
        _tcp_shift_r.addWidget(_ui.doubleSpinBoxTCPShiftRX);
        _tcp_shift_r.addWidget(_ui.doubleSpinBoxTCPShiftRY);
        _tcp_shift_r.addWidget(_ui.doubleSpinBoxTCPShiftRZ);
 
        const std::vector spinboxes{
            _ui.doubleSpinBoxBoundTransvMax, _ui.doubleSpinBoxBoundTransvMin,
            _ui.doubleSpinBoxBoundWidthMin,  _ui.doubleSpinBoxBoundWidthMax,
            _ui.doubleSpinBoxBoundDepthMin,  _ui.doubleSpinBoxBoundDepthMax,
 
            _ui.doubleSpinBoxBoxTX,          _ui.doubleSpinBoxBoxTY,
            _ui.doubleSpinBoxBoxTZ,          _ui.doubleSpinBoxBoxRX,
            _ui.doubleSpinBoxBoxRY,          _ui.doubleSpinBoxBoxRZ,
            _ui.doubleSpinBoxBoxSizeX,       _ui.doubleSpinBoxBoxSizeY,
            _ui.doubleSpinBoxBoxSizeZ,
 
            _ui.doubleSpinBoxHandLTransvX,   _ui.doubleSpinBoxHandLTransvY,
            _ui.doubleSpinBoxHandLTransvZ,   _ui.doubleSpinBoxHandRTransvX,
            _ui.doubleSpinBoxHandRTransvY,   _ui.doubleSpinBoxHandRTransvZ,
 
            _ui.doubleSpinBoxHandLUpX,       _ui.doubleSpinBoxHandLUpY,
            _ui.doubleSpinBoxHandLUpZ,       _ui.doubleSpinBoxHandRUpX,
            _ui.doubleSpinBoxHandRUpY,       _ui.doubleSpinBoxHandRUpZ,
 
            _ui.doubleSpinBoxTCPShiftLX,     _ui.doubleSpinBoxTCPShiftLY,
            _ui.doubleSpinBoxTCPShiftLZ,     _ui.doubleSpinBoxTCPShiftRX,
            _ui.doubleSpinBoxTCPShiftRY,     _ui.doubleSpinBoxTCPShiftRZ};
        for (QDoubleSpinBox* ptr : spinboxes)
        {
            connect(ptr,
                    qOverload<double>(&QDoubleSpinBox::valueChanged),
                    this,
                    &BoxToGraspCandidatesWidgetController::update);
        }
 
        const std::vector checkboxes{//Left
                                     _ui.checkBoxGraspLPosXPosY,
                                     _ui.checkBoxGraspLPosXPosZ,
                                     _ui.checkBoxGraspLPosXNegY,
                                     _ui.checkBoxGraspLPosXNegZ,
 
                                     _ui.checkBoxGraspLNegXPosY,
                                     _ui.checkBoxGraspLNegXPosZ,
                                     _ui.checkBoxGraspLNegXNegY,
                                     _ui.checkBoxGraspLNegXNegZ,
 
                                     _ui.checkBoxGraspLPosYPosX,
                                     _ui.checkBoxGraspLPosYPosZ,
                                     _ui.checkBoxGraspLPosYNegX,
                                     _ui.checkBoxGraspLPosYNegZ,
 
                                     _ui.checkBoxGraspLNegYPosX,
                                     _ui.checkBoxGraspLNegYPosZ,
                                     _ui.checkBoxGraspLNegYNegX,
                                     _ui.checkBoxGraspLNegYNegZ,
 
                                     _ui.checkBoxGraspLPosZPosX,
                                     _ui.checkBoxGraspLPosZPosY,
                                     _ui.checkBoxGraspLPosZNegX,
                                     _ui.checkBoxGraspLPosZNegY,
 
                                     _ui.checkBoxGraspLNegZPosX,
                                     _ui.checkBoxGraspLNegZPosY,
                                     _ui.checkBoxGraspLNegZNegX,
                                     _ui.checkBoxGraspLNegZNegY,
 
                                     //Right
                                     _ui.checkBoxGraspRPosXPosY,
                                     _ui.checkBoxGraspRPosXPosZ,
                                     _ui.checkBoxGraspRPosXNegY,
                                     _ui.checkBoxGraspRPosXNegZ,
 
                                     _ui.checkBoxGraspRNegXPosY,
                                     _ui.checkBoxGraspRNegXPosZ,
                                     _ui.checkBoxGraspRNegXNegY,
                                     _ui.checkBoxGraspRNegXNegZ,
 
                                     _ui.checkBoxGraspRPosYPosX,
                                     _ui.checkBoxGraspRPosYPosZ,
                                     _ui.checkBoxGraspRPosYNegX,
                                     _ui.checkBoxGraspRPosYNegZ,
 
                                     _ui.checkBoxGraspRNegYPosX,
                                     _ui.checkBoxGraspRNegYPosZ,
                                     _ui.checkBoxGraspRNegYNegX,
                                     _ui.checkBoxGraspRNegYNegZ,
 
                                     _ui.checkBoxGraspRPosZPosX,
                                     _ui.checkBoxGraspRPosZPosY,
                                     _ui.checkBoxGraspRPosZNegX,
                                     _ui.checkBoxGraspRPosZNegY,
 
                                     _ui.checkBoxGraspRNegZPosX,
                                     _ui.checkBoxGraspRNegZPosY,
                                     _ui.checkBoxGraspRNegZNegX,
                                     _ui.checkBoxGraspRNegZNegY};
        for (QCheckBox* ptr : checkboxes)
        {
            ptr->setChecked(true);
            connect(ptr, &QCheckBox::clicked, this, &BoxToGraspCandidatesWidgetController::update);
        }
    }
 
    void
    BoxToGraspCandidatesWidgetController::onConnectComponent()
    {
        {
            std::lock_guard guard{_mutex};
            _robot = addRobot("state robot", VirtualRobot::RobotIO::eStructure);
            getTopic(_gc_topic, _gc_topic_name);
        }
        QMetaObject::invokeMethod(this, "update", Qt::QueuedConnection);
    }
 
    void
    BoxToGraspCandidatesWidgetController::update()
    {
        ARMARX_TRACE;
        std::lock_guard guard{_mutex};
        if (!_robot)
        {
            return;
        }
        synchronizeLocalClone(_robot);
        const simox::OrientedBox<float> box{_box_pose.getMat4(),
                                            _box_extend.get<Eigen::Vector3f>()};
 
        grasp_candidate_drawer gc_draw{getRobotNameHelper(), _robot};
        box_to_grasp_candidates b2gc{getRobotNameHelper(), _robot};
        //sides
        {
            auto& l = b2gc.side("Left");
            auto& r = b2gc.side("Right");
 
            l.hand_transverse = _transversal_l.get<Eigen::Vector3f>().normalized();
            r.hand_transverse = _transversal_r.get<Eigen::Vector3f>().normalized();
            l.hand_up = _up_l.get<Eigen::Vector3f>().normalized();
            r.hand_up = _up_r.get<Eigen::Vector3f>().normalized();
            l.tcp_shift = _tcp_shift_l.get<Eigen::Vector3f>().normalized();
            r.tcp_shift = _tcp_shift_r.get<Eigen::Vector3f>().normalized();
        }
        box_to_grasp_candidates::length_limit lim;
        {
            lim.transverse_length_min = _ui.doubleSpinBoxBoundTransvMin->value();
            lim.transverse_length_max = _ui.doubleSpinBoxBoundTransvMax->value();
            lim.upward_length_min = _ui.doubleSpinBoxBoundDepthMin->value();
            lim.upward_length_max = _ui.doubleSpinBoxBoundDepthMax->value();
            lim.forward_length_min = _ui.doubleSpinBoxBoundWidthMin->value();
            lim.forward_length_max = _ui.doubleSpinBoxBoundWidthMax->value();
        }
        box_to_grasp_candidates::side_enabled_map side_enabled_l;
        {
            using ba = box_to_grasp_candidates::box_alignment;
            side_enabled_l[ba::pos_x_pos_y] = _ui.checkBoxGraspLPosXPosY->isChecked();
            side_enabled_l[ba::pos_x_pos_z] = _ui.checkBoxGraspLPosXPosZ->isChecked();
            side_enabled_l[ba::pos_x_neg_y] = _ui.checkBoxGraspLPosXNegY->isChecked();
            side_enabled_l[ba::pos_x_neg_z] = _ui.checkBoxGraspLPosXNegZ->isChecked();
 
            side_enabled_l[ba::neg_x_pos_y] = _ui.checkBoxGraspLNegXPosY->isChecked();
            side_enabled_l[ba::neg_x_pos_z] = _ui.checkBoxGraspLNegXPosZ->isChecked();
            side_enabled_l[ba::neg_x_neg_y] = _ui.checkBoxGraspLNegXNegY->isChecked();
            side_enabled_l[ba::neg_x_neg_z] = _ui.checkBoxGraspLNegXNegZ->isChecked();
 
            side_enabled_l[ba::pos_y_pos_x] = _ui.checkBoxGraspLPosYPosX->isChecked();
            side_enabled_l[ba::pos_y_pos_z] = _ui.checkBoxGraspLPosYPosZ->isChecked();
            side_enabled_l[ba::pos_y_neg_x] = _ui.checkBoxGraspLPosYNegX->isChecked();
            side_enabled_l[ba::pos_y_neg_z] = _ui.checkBoxGraspLPosYNegZ->isChecked();
 
            side_enabled_l[ba::neg_y_pos_x] = _ui.checkBoxGraspLNegYPosX->isChecked();
            side_enabled_l[ba::neg_y_pos_z] = _ui.checkBoxGraspLNegYPosZ->isChecked();
            side_enabled_l[ba::neg_y_neg_x] = _ui.checkBoxGraspLNegYNegX->isChecked();
            side_enabled_l[ba::neg_y_neg_z] = _ui.checkBoxGraspLNegYNegZ->isChecked();
 
            side_enabled_l[ba::pos_z_pos_x] = _ui.checkBoxGraspLPosZPosX->isChecked();
            side_enabled_l[ba::pos_z_pos_y] = _ui.checkBoxGraspLPosZPosY->isChecked();
            side_enabled_l[ba::pos_z_neg_x] = _ui.checkBoxGraspLPosZNegX->isChecked();
            side_enabled_l[ba::pos_z_neg_y] = _ui.checkBoxGraspLPosZNegY->isChecked();
 
            side_enabled_l[ba::neg_z_pos_x] = _ui.checkBoxGraspLNegZPosX->isChecked();
            side_enabled_l[ba::neg_z_pos_y] = _ui.checkBoxGraspLNegZPosY->isChecked();
            side_enabled_l[ba::neg_z_neg_x] = _ui.checkBoxGraspLNegZNegX->isChecked();
            side_enabled_l[ba::neg_z_neg_y] = _ui.checkBoxGraspLNegZNegY->isChecked();
        }
        box_to_grasp_candidates::side_enabled_map side_enabled_r;
        {
            using ba = box_to_grasp_candidates::box_alignment;
            side_enabled_r[ba::pos_x_pos_y] = _ui.checkBoxGraspRPosXPosY->isChecked();
            side_enabled_r[ba::pos_x_pos_z] = _ui.checkBoxGraspRPosXPosZ->isChecked();
            side_enabled_r[ba::pos_x_neg_y] = _ui.checkBoxGraspRPosXNegY->isChecked();
            side_enabled_r[ba::pos_x_neg_z] = _ui.checkBoxGraspRPosXNegZ->isChecked();
 
            side_enabled_r[ba::neg_x_pos_y] = _ui.checkBoxGraspRNegXPosY->isChecked();
            side_enabled_r[ba::neg_x_pos_z] = _ui.checkBoxGraspRNegXPosZ->isChecked();
            side_enabled_r[ba::neg_x_neg_y] = _ui.checkBoxGraspRNegXNegY->isChecked();
            side_enabled_r[ba::neg_x_neg_z] = _ui.checkBoxGraspRNegXNegZ->isChecked();
 
            side_enabled_r[ba::pos_y_pos_x] = _ui.checkBoxGraspRPosYPosX->isChecked();
            side_enabled_r[ba::pos_y_pos_z] = _ui.checkBoxGraspRPosYPosZ->isChecked();
            side_enabled_r[ba::pos_y_neg_x] = _ui.checkBoxGraspRPosYNegX->isChecked();
            side_enabled_r[ba::pos_y_neg_z] = _ui.checkBoxGraspRPosYNegZ->isChecked();
 
            side_enabled_r[ba::neg_y_pos_x] = _ui.checkBoxGraspRNegYPosX->isChecked();
            side_enabled_r[ba::neg_y_pos_z] = _ui.checkBoxGraspRNegYPosZ->isChecked();
            side_enabled_r[ba::neg_y_neg_x] = _ui.checkBoxGraspRNegYNegX->isChecked();
            side_enabled_r[ba::neg_y_neg_z] = _ui.checkBoxGraspRNegYNegZ->isChecked();
 
            side_enabled_r[ba::pos_z_pos_x] = _ui.checkBoxGraspRPosZPosX->isChecked();
            side_enabled_r[ba::pos_z_pos_y] = _ui.checkBoxGraspRPosZPosY->isChecked();
            side_enabled_r[ba::pos_z_neg_x] = _ui.checkBoxGraspRPosZNegX->isChecked();
            side_enabled_r[ba::pos_z_neg_y] = _ui.checkBoxGraspRPosZNegY->isChecked();
 
            side_enabled_r[ba::neg_z_pos_x] = _ui.checkBoxGraspRNegZPosX->isChecked();
            side_enabled_r[ba::neg_z_pos_y] = _ui.checkBoxGraspRNegZPosY->isChecked();
            side_enabled_r[ba::neg_z_neg_x] = _ui.checkBoxGraspRNegZNegX->isChecked();
            side_enabled_r[ba::neg_z_neg_y] = _ui.checkBoxGraspRNegZNegY->isChecked();
        }
 
        viz::Layer layer_l = arviz.layer("grasps_l");
        viz::Layer layer_r = arviz.layer("grasps_r");
        viz::Layer layer_hand_vec = arviz.layer("hand_vec");
        viz::Layer layer_box = arviz.layer("box");
        layer_box.add(viz::Box{"box"}.set(box).transformPose(_robot->getGlobalPose()));
 
        gc_draw.draw(b2gc.side("Left"), layer_hand_vec);
        gc_draw.draw(b2gc.side("Right"), layer_hand_vec);
 
        grasping::GraspCandidateSeq graps;
        const auto consume_l = [&](const auto& g)
        {
            graps.emplace_back(g.make_candidate(getName()));
            gc_draw.draw(graps.back(), layer_l);
        };
        const auto consume_r = [&](const auto& g)
        {
            graps.emplace_back(g.make_candidate(getName()));
            gc_draw.draw(graps.back(), layer_r);
        };
        b2gc.center_grasps(box, "Left", lim, consume_l, side_enabled_l);
        b2gc.center_grasps(box, "Right", lim, consume_r, side_enabled_r);
 
        arviz.commit({layer_l, layer_r, layer_hand_vec, layer_box});
 
        _gc_topic->reportGraspCandidates(getName(), graps);
    }
} // namespace armarx